P-Controller Concept
This commit is contained in:
parent
2af8bfa536
commit
0f5c2e3f42
@ -49,7 +49,19 @@ void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
|
||||
}
|
||||
|
||||
void ControlModule::rotate(double angle){
|
||||
double speed = rotateSpeed * (angle + 90.0)/90.0;
|
||||
double minAngularSpeed = -1.0;
|
||||
double maxAngularSpeed = 1.0;
|
||||
double speedRange = maxAngularSpeed - minAngularSpeed;
|
||||
|
||||
double minAngle = -90.0;
|
||||
double maxAngle = 90.0;
|
||||
double angularRange = maxAngle - minAngle;
|
||||
|
||||
double progress = angle - minAngle;
|
||||
double progressPercent = progress/angularRange;
|
||||
|
||||
double speed = minAngularSpeed + progressPercent*speedRange;
|
||||
|
||||
motors[0] += speed;
|
||||
motors[1] -= speed;
|
||||
motors[2] += speed;
|
||||
|
Loading…
x
Reference in New Issue
Block a user