Diff: StateMachine.cpp

Revision:

4:91a9737e4821

Parent:

3:6e28589a732f

--- a/StateMachine.cpp	Thu Apr 19 12:04:49 2018 +0000
+++ b/StateMachine.cpp	Fri Apr 20 17:49:36 2018 +0000
@@ -1,78 +1,35 @@
-#include "mbed.h"
- 
-case FORWAERTSFAHREN:
+
+// Statemachine
+//V04.18
+// V. Ahlers
+
+#include <mbed.h>
+#include "StateMachine.h"
+
+using namespace std;
+
+StateMachine::StateMachine(int IrR, int IrL, int IrF) : IrR(IrR), IrL(IrL), IrF(IrF) {}
+
+StateMachine::~StateMachine (){}
 
-    buttonNow = button->read();
-    if (buttonNow && !buttonBefore){
-        controller-> setTranslationVelocity(0.0f);
-        state = LANGSAM_FAHREN;
-        }
-        else if(((irSensorF.read() < DISTANCE_F)
-        && (irSensorR.read() < DISTANCE_R)) {
-            controller.setTranslationVelocity(0.0f);
-            controller.setRotationVelocity(ROTATION_VELOCITY);
-            state = 270_GRAD_RECHTS;
-            }
-        else if(irSensorR.read() > DISTANCE_R){
-        controller.setTranslationVelocity(0.0f);
-         controller.setRotationVelocity(ROTATION_VELOCITY);
-            state = 90_GRAD_RECHTS;
-            }
-        else if((irSensorF.read() < DISTANCE_F)
-        && (irSensorR.read() < DISTANCE_R) (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_R) {
-        controller.setTranslationalvelocity(TRANSLATIONAL_VELOCITY);
-        state = VORWAERTSFAHREN;
-    }
+int StateMachine :: drive() {
     
-    
- 
-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_F)
-                && (irSensorR.read() > DISTANCE_R)
-                && (irSensorL.read() < DISTANCE_L) {
-    controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
-    state= VORWAERTSFAHREN;
-}
-
+    if((IrR==0) && (IrL==0) && (IrF==1)){
+        caseDrive = 2;
+    }else { if ((IrR==0) && (IrL==1) && (IrF==0)){
+        caseDrive = 2;
+    }else { if ((IrR==0) && (IrL==1) && (IrF==1)){
+        caseDrive = 2;
+    }else { if ((IrR==1) && (IrL==0) && (IrF==0)){
+        caseDrive = 1;
+    }else { if ((IrR==1) && (IrL==1) && (IrF==1)){
+        caseDrive = 3;
+    }else { if ((IrR==1) && (IrL==1) && (IrF==0)){
+        caseDrive = 1;
+    }else { if ((IrR==1) && (IrL==1) && (IrF==1)){
+        caseDrive = 4;
+    }else{ caseDrive=0;
+    }}}}}}}
 
-case 180_GRAD_RECHTS
- 
-    buttonNow && !button.read();
-    if (buttonNow && !buttonBefore {  // detect button rising edge
-        state = LANGSAM_FAHREN;
-        } else if (( irSensorR.read() > DISTANCE_R)
-            && (irSensorF.read() > DISTANCE_F)
-            && (irSensorL.read() > DISTANCE_L)) {
-    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
+    return caseDrive;
+}
\ No newline at end of file