Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ROME2_P2 mbed

Fork of ROME2_P3 by Zürcher Eliteeinheit

Diff: StateMachine.cpp

Revision:
2:13da48f33ab7
Parent:
0:e360940c4b88
Child:
3:9433326c2248
--- a/StateMachine.cpp	Fri Mar 16 14:40:52 2018 +0000
+++ b/StateMachine.cpp	Fri Mar 16 15:17:08 2018 +0000
@@ -11,8 +11,8 @@
 
 const float StateMachine::PERIOD = 0.01f;                   // period of task in [s]
 const float StateMachine::DISTANCE_THRESHOLD = 0.2f;        // minimum allowed distance to obstacle in [m]
-const float StateMachine::TRANSLATIONAL_VELOCITY = 0.3f;    // translational velocity in [m/s]
-const float StateMachine::ROTATIONAL_VELOCITY = 1.0f;       // rotational velocity in [rad/s]
+const float StateMachine::TRANSLATIONAL_VELOCITY = 0.8f;    // translational velocity in [m/s]
+const float StateMachine::ROTATIONAL_VELOCITY = 2.0f;       // rotational velocity in [rad/s]
 
 /**
  * Creates and initializes a state machine object.
@@ -83,26 +83,59 @@
             
         case MOVE_FORWARD:
             
-            // bitte implementieren
+            
+            if(led5)
+                {
+                    
+                    state = TURN_LEFT;
+                    controller.setTranslationalVelocity(0);
+                    controller.setRotationalVelocity(ROTATIONAL_VELOCITY);
+                }
+            else if(led1)
+                {
+                    
+                    state = TURN_RIGHT;
+                    controller.setTranslationalVelocity(0);
+                    controller.setRotationalVelocity(ROTATIONAL_VELOCITY);
+                }
+          /*  else if(led)
+                {
+                    
+                    state = TURN_RIGHT;
+                    controller.setTranslationalVelocity(0);
+                    controller.setRotationalVelocity(-0.5f);
+                }
+                */
             
             break;
             
         case TURN_LEFT:
-            
-            // bitte implementieren
+            
+            if(!led5)
+            {   
+                controller.setRotationalVelocity(0);
+                controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
+                state = MOVE_FORWARD;
+            }
             
             break;
             
         case TURN_RIGHT:
-            
-            // bitte implementieren
+            
+            if(!led1)
+            {   
+                controller.setRotationalVelocity(0);
+                controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
+                state = MOVE_FORWARD;
+            }
             
             break;
             
         case SLOWING_DOWN:
-            
-            // bitte implementieren
             
+            // bitte implementieren
+            
+            controller.setTranslationalVelocity(0);
             break;
             
         default: