Diff: TaskMoveTo.cpp

Revision:

2:fc9e2aebf9d5

Parent:

1:ff05970bb9b0

--- a/TaskMoveTo.cpp	Tue Mar 24 09:53:23 2020 +0000
+++ b/TaskMoveTo.cpp	Wed Mar 25 14:15:52 2020 +0000
@@ -80,13 +80,18 @@
  */
 int TaskMoveTo::run(float period) {
    
-    return RUNNING;
+    
     
     // bitte implementieren!
     
-    double rho = sqrt(pow(x-controller.getX(),2)+pow(y-controller.getY(),2));
+    double rho = sqrt(pow(x-controller.getX(),2)+pow(y-controller.getY(),2)); // if rho < 0.001 return done else return running
+    
     
-    if (rho = 0) return DONE;
+    if (rho < 0.00001) {  //double nicht mit 0 verlgeichen, if statement für ob man schon an gewünschter koordinate ist oder nicht
+        return DONE;
+    } else { 
+    return RUNNING;
+    }
     
     double gamma = atan2(y-controller.getY(),x-controller.getX())- controller.getAlpha();
     double delta = gamma + controller.getAlpha()-alpha;
@@ -100,15 +105,15 @@
         delta += 2*PI;}
     else if(delta > PI){
         delta -= 2*PI;}  
-    else if (gamma = 0){
+    else if (gamma <= 0.000001){
              beta = 0;}
     else {}
         
-    beta = K2*gamma+K3*delta;
-    double x_trans = K1*rho;
+    beta = K2*gamma+K1*sin(gamma)*cos(gamma)*((gamma+K3*delta)/gamma); //falsche formel
+    double x_trans = K1*rho*cos(gamma);
      
-    controller.setX(x_trans);
-    controller.setAlpha(beta);
+    controller.setTranslationalVelocity(x_trans); //GESCHIWNDIGKEITEN NICHT POSITION
+    controller.setRotationalVelocity(beta);
     
     return DONE;
 }