Erster Entwurft für Controlling

AutonomousMode/ControlModule/control_module.cpp

@@ -15,15 +15,41 @@ ControlModule::~ControlModule()
 }
 
 void ControlModule::moveForward(){
-    for(int i = 0; motors; i++){
-
+    for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
+        motors[i] += forwardSpeed;
     }
 };
 
-void ControlModule::moveSide(){
-
+void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
+    float speed = moveSideSpeed * (contourColumsMiddle - imageColumsMiddle)/imageColumsMiddle;
+    motors[0] += speed;
+    motors[1] -= speed;
+    motors[2] -= speed;
+    motors[3] += speed;
 }
 
-void ControlModule::rotate(){
+void ControlModule::rotate(int angle){
+    float speed = rotateSpeed * (angle + 90)/90;
+    motors[0] += speed;
+    motors[1] -= speed;
+    motors[2] += speed;
+    motors[3] -= speed;
+}
 
+void ControlModule::unit(){
+    float max = 10E-12;
+    for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
+        if(motors[i] > max)
+        max = motors[i];
+    }
+    for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
+        motors[i] /= max;
+    }
+}
+
+void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle){
+    moveForward();
+    moveSide(imageColumsMiddle, contourColumsMiddle);
+    rotate(angle);
+    unit();
 }

AutonomousMode/ControlModule/control_module.h

@@ -12,6 +12,9 @@ public:
     ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed);
     ~ControlModule();
     void moveForward();
-    void rotate();
-    void moveSide();
+    void moveSide(int imageColumsMiddle, int contourColumsMiddle);
+    void rotate(int angle);
+    void unit();            //Brings the max Value to 1.0
+
+    void calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle); //Funktion to be called
 };