Diff: TaskMoveTo.cpp

Revision:

1:ff05970bb9b0

Parent:

0:6a4d3264c067

Child:

2:fc9e2aebf9d5

--- a/TaskMoveTo.cpp	Tue Mar 24 08:39:54 2020 +0000
+++ b/TaskMoveTo.cpp	Tue Mar 24 09:53:23 2020 +0000
@@ -85,12 +85,28 @@
     // bitte implementieren!
     
     double rho = sqrt(pow(x-controller.getX(),2)+pow(y-controller.getY(),2));
+    
+    if (rho = 0) return DONE;
+    
     double gamma = atan2(y-controller.getY(),x-controller.getX())- controller.getAlpha();
     double delta = gamma + controller.getAlpha()-alpha;
+    double beta;
     
+    if (gamma < -PI ){
+        gamma += 2*PI;}
+    else if(gamma > PI){
+        gamma -= 2*PI;} 
+    else if(delta < -PI ){
+        delta += 2*PI;}
+    else if(delta > PI){
+        delta -= 2*PI;}  
+    else if (gamma = 0){
+             beta = 0;}
+    else {}
+        
+    beta = K2*gamma+K3*delta;
     double x_trans = K1*rho;
-    double beta = K2*gamma+K3*delta;
-    
+     
     controller.setX(x_trans);
     controller.setAlpha(beta);