Revision 2:fc9e2aebf9d5, committed 2020-03-25

Comitter:

oehlemar

Date:

Wed Mar 25 14:15:52 2020 +0000

Parent:

1:ff05970bb9b0

Commit message:

final

Changed in this revision

--- a/StateMachine.cpp	Tue Mar 24 09:53:23 2020 +0000
+++ b/StateMachine.cpp	Wed Mar 25 14:15:52 2020 +0000
@@ -95,22 +95,7 @@
             
             buttonNow = button;
             
-            if (taskList.size() > 0) {
-            
-                Task* task = taskList.front();
-                int result = task->run(PERIOD);
-                
-                if (result == Task::DONE) {
-                    
-                    taskList.pop_front();
-                    delete task;
-                    
-                    }
-                    
-            else {
-                state = SLOWING_DOWN;
-                }
-            }
+
             
             if (buttonNow && !buttonBefore) {   // detect button rising edge
                 
@@ -133,7 +118,24 @@
                 
                 state = TURN_RIGHT;
                 
-            } else {
+            } else if (taskList.size() > 0) {           // if statement hier unten, da kollision erkennung wichtiger
+            
+                Task* task = taskList.front();
+                int result = task->run(PERIOD);
+                
+                        if (result == Task::DONE) {
+                            
+                            taskList.pop_front();
+                            delete task;
+                             
+                        } else {
+                            
+                        controller.setTranslationalVelocity(0.0f);  //Sicher gehen, dass die letzen Geschw. auch wirklich 0 sind
+                        controller.setRotationalVelocity(0.0f);
+                        state = SLOWING_DOWN;
+                        
+                        }
+            }  else {
                 
             }
             

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