P-Controller Concept

AutonomousMode/ControlModule/control_module.cpp

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