Diff: StateMachine.cpp

Revision:

4:4428ede9beee

Parent:

3:9433326c2248

Child:

5:59079b76ac7f

--- a/StateMachine.cpp	Fri Mar 16 19:44:20 2018 +0000
+++ b/StateMachine.cpp	Thu Apr 12 12:19:19 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.8f;    // translational velocity in [m/s]
-const float StateMachine::ROTATIONAL_VELOCITY = 2.0f;       // rotational velocity in [rad/s]
+const float StateMachine::TRANSLATIONAL_VELOCITY = 0.3f;    // translational velocity in [m/s]
+const float StateMachine::ROTATIONAL_VELOCITY = 1.0f;       // rotational velocity in [rad/s]
 
 /**
  * Creates and initializes a state machine object.
@@ -83,57 +83,87 @@
             
         case MOVE_FORWARD:
             
+            buttonNow = button;
             
-            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)/// touch fwd sensor first recude velocity! TODO arber and sommer 
-                {
-                    
-                    controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY/4);
-                }
-                */
+            if (buttonNow && !buttonBefore) {   // detect button rising edge
+                
+                controller.setTranslationalVelocity(0.0f);
+                controller.setRotationalVelocity(0.0f);
+                
+                state = SLOWING_DOWN;
+                
+            } else if ((irSensor0 < DISTANCE_THRESHOLD) || (irSensor1 < DISTANCE_THRESHOLD)) {
+                
+                controller.setTranslationalVelocity(0.0f);
+                controller.setRotationalVelocity(ROTATIONAL_VELOCITY);
+                
+                state = TURN_LEFT;
+                
+            } else if (irSensor5 < DISTANCE_THRESHOLD) {
+                
+                controller.setTranslationalVelocity(0.0f);
+                controller.setRotationalVelocity(-ROTATIONAL_VELOCITY);
+                
+                state = TURN_RIGHT;
+            }
+            
+            buttonBefore = buttonNow;
             
             break;
             
         case TURN_LEFT:
             
-            if(!led5)
-            {   
-                controller.setRotationalVelocity(0);
+            buttonNow = button;
+            
+            if (buttonNow && !buttonBefore) {   // detect button rising edge
+                
+                controller.setRotationalVelocity(0.0f);
+                
+                state = SLOWING_DOWN;
+                
+            } else if ((irSensor0 > DISTANCE_THRESHOLD) && (irSensor1 > DISTANCE_THRESHOLD) && (irSensor5 > DISTANCE_THRESHOLD)) {
+                
                 controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
+                controller.setRotationalVelocity(0.0f);
+                
                 state = MOVE_FORWARD;
             }
             
+            buttonBefore = buttonNow;
+            
             break;
             
         case TURN_RIGHT:
             
-            if(!led1)
-            {   
-                controller.setRotationalVelocity(0);
+            buttonNow = button;
+            
+            if (buttonNow && !buttonBefore) {   // detect button rising edge
+                
+                controller.setRotationalVelocity(0.0f);
+                
+                state = SLOWING_DOWN;
+                
+            } else if ((irSensor0 > DISTANCE_THRESHOLD) && (irSensor1 > DISTANCE_THRESHOLD) && (irSensor5 > DISTANCE_THRESHOLD)) {
+                
                 controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
+                controller.setRotationalVelocity(0.0f);
+                
                 state = MOVE_FORWARD;
             }
             
+            buttonBefore = buttonNow;
+            
             break;
             
         case SLOWING_DOWN:
             
-            // bitte implementieren
+            if ((fabs(controller.getActualTranslationalVelocity()) < 0.01f) && (fabs(controller.getActualRotationalVelocity()) < 0.01f)) {
+                
+                enableMotorDriver = 0;
+                
+                state = ROBOT_OFF;
+            }
             
-            controller.setTranslationalVelocity(0);
             break;
             
         default: