|
|
@@ -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; |