Diff: StateMachine.cpp

Revision:

0:e611a0b9f02a

Child:

1:619ca6edc4a9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachine.cpp	Mon Apr 16 13:29:27 2018 +0000
@@ -0,0 +1,78 @@
+#include "mbed.h"
+ 
+case FORWAERTSFAHREN:
+
+    buttonNow = button->read();
+    if (buttonNow && !buttonBefore){
+        controller-> setTranslationVelocity(0.0f);
+        state = LANGSAM_FAHREN;
+        }
+        else if(((IRSensorF.read() < DISTANCE_THRESHOLD)
+        && (IRSensorR.read() < DISTANCE_THRESHOLD)) {
+            controller.setTranslationVelocity(0.0f);
+            controller.setRotationVelocity(ROTATION_VELOCITY);
+            state = 270_GRAD_RECHTS;
+            }
+        else if(IRSensorR.read() > DISTANCE_THRESHOLD){
+        controller.setTranslationVelocity(0.0f);
+         controller.setRotationVelocity(ROTATION_VELOCITY);
+            state = 90_GRAD_RECHTS;
+            }
+        else if((IRSensorF.read() < DISTANCE_THRESHOLD)
+        && (IRSensorR.read() < DISTANCE_THRESHOLD) (SensorL.read() < DISTANCE_THRESHOLD)) {
+            controller.setTranslationVelocity(0.0f);
+            controller.setRotationVelocity(ROTATION_VELOCITY);
+            state = 180_GRAD_RECHTS;
+            }
+            buttonBefore = buttonNow;
+            break;
+            
+ 
+case 90_GRAD_RECHTS
+ 
+    buttonNOW = button.read();
+    if (buttonNow && !buttonBefore {  // deect button rising edge
+    controller.setRotationalVelocity(0.0f);
+    state = LANGSAM_FAHREN;
+        } else if ((irSensorR < DISTANCE_THRESHOLD) {
+        controller.setTranslationalvelocity(TRANSLATIONAL_VELOCITY);
+        state = VORWAERTSFAHREN;
+    }
+    
+    
+ 
+case 270_GRAD_RECHTS
+ 
+    buttonNow = button.read();
+    if (buttonNow && !buttonBefore {  //detect button rising edge
+        controller.set RotationalVelociy(0.0f);
+        state = LANGSAM_FAHREN;
+        } else if ((irSensorF.read() > DISTANCE_THRESHOLD)
+                && (irSensorR.read() > DISTANCE_THRESHOLD)
+                && (irSensorL.read() < DISTANCE_THRESHOLD) {
+    controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
+    state= VORWAERTSFAHREN;
+}
+
+
+case 180_GRAD_RECHTS
+ 
+    buttonNow && !button.read();
+    if (buttonNow && !buttonBefore {  // detect button rising edge
+        state = LANGSAM_FAHREN;
+        } else if (( irSensorR.read() > DISTANCE_THRESHOLD)
+            && (irSensorF.read() > DISTANCE_THRESHOLD)
+            && (irSensorL.read() > DISTANCE_THRESHOLD)) {
+    controller.setTranslationalVelocity(TRANCLATIONAL_VELOCITY) {
+    state = VORWAERTSFAHREN;
+    
+    
+    
+case LANGSAM_FAHREN;
+ 
+    if ((fabs(controller.getActualTranslationalVelocity()) < 0.01f )
+        && (fabs(controller.getActualRotationalVelocity()) < 0.1f ) {
+        enableMotorDriver = 0;
+        state = ROBOT_OFF;
+        }
+        break;
\ No newline at end of file