Roboshark / Mbed 2 deprecated StateMachine_1

h

Dependencies:   mbed

Fork of main by Roboshark

StateMachine.cpp@3:6e28589a732f, 2018-04-19 (annotated)

Committer:
Jacqueline
Date:
Thu Apr 19 12:04:49 2018 +0000
Revision:
3:6e28589a732f
Parent:
1:619ca6edc4a9 
neu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jacqueline 0:e611a0b9f02a 1 #include "mbed.h"
Jacqueline 0:e611a0b9f02a 2
Jacqueline 0:e611a0b9f02a 3 case FORWAERTSFAHREN:
Jacqueline 0:e611a0b9f02a 4
Jacqueline 0:e611a0b9f02a 5 buttonNow = button->read();
Jacqueline 0:e611a0b9f02a 6 if (buttonNow && !buttonBefore){
Jacqueline 0:e611a0b9f02a 7 controller-> setTranslationVelocity(0.0f);
Jacqueline 0:e611a0b9f02a 8 state = LANGSAM_FAHREN;
Jacqueline 0:e611a0b9f02a 9 }
Jacqueline 3:6e28589a732f 10 else if(((irSensorF.read() < DISTANCE_F)
Jacqueline 3:6e28589a732f 11 && (irSensorR.read() < DISTANCE_R)) {
Jacqueline 0:e611a0b9f02a 12 controller.setTranslationVelocity(0.0f);
Jacqueline 0:e611a0b9f02a 13 controller.setRotationVelocity(ROTATION_VELOCITY);
Jacqueline 0:e611a0b9f02a 14 state = 270_GRAD_RECHTS;
Jacqueline 0:e611a0b9f02a 15 }
Jacqueline 3:6e28589a732f 16 else if(irSensorR.read() > DISTANCE_R){
Jacqueline 0:e611a0b9f02a 17 controller.setTranslationVelocity(0.0f);
Jacqueline 0:e611a0b9f02a 18 controller.setRotationVelocity(ROTATION_VELOCITY);
Jacqueline 0:e611a0b9f02a 19 state = 90_GRAD_RECHTS;
Jacqueline 0:e611a0b9f02a 20 }
Jacqueline 3:6e28589a732f 21 else if((irSensorF.read() < DISTANCE_F)
Jacqueline 3:6e28589a732f 22 && (irSensorR.read() < DISTANCE_R) (SensorL.read() < DISTANCE_THRESHOLD)) {
Jacqueline 0:e611a0b9f02a 23 controller.setTranslationVelocity(0.0f);
Jacqueline 0:e611a0b9f02a 24 controller.setRotationVelocity(ROTATION_VELOCITY);
Jacqueline 0:e611a0b9f02a 25 state = 180_GRAD_RECHTS;
Jacqueline 0:e611a0b9f02a 26 }
Jacqueline 0:e611a0b9f02a 27 buttonBefore = buttonNow;
Jacqueline 0:e611a0b9f02a 28 break;
Jacqueline 0:e611a0b9f02a 29
Jacqueline 0:e611a0b9f02a 30
Jacqueline 0:e611a0b9f02a 31 case 90_GRAD_RECHTS
Jacqueline 0:e611a0b9f02a 32
Jacqueline 0:e611a0b9f02a 33 buttonNOW = button.read();
Jacqueline 0:e611a0b9f02a 34 if (buttonNow && !buttonBefore { // deect button rising edge
Jacqueline 0:e611a0b9f02a 35 controller.setRotationalVelocity(0.0f);
Jacqueline 0:e611a0b9f02a 36 state = LANGSAM_FAHREN;
Jacqueline 3:6e28589a732f 37 } else if ((irSensorR < DISTANCE_R) {
Jacqueline 0:e611a0b9f02a 38 controller.setTranslationalvelocity(TRANSLATIONAL_VELOCITY);
Jacqueline 0:e611a0b9f02a 39 state = VORWAERTSFAHREN;
Jacqueline 0:e611a0b9f02a 40 }
Jacqueline 0:e611a0b9f02a 41
Jacqueline 0:e611a0b9f02a 42
Jacqueline 0:e611a0b9f02a 43
Jacqueline 0:e611a0b9f02a 44 case 270_GRAD_RECHTS
Jacqueline 0:e611a0b9f02a 45
Jacqueline 0:e611a0b9f02a 46 buttonNow = button.read();
Jacqueline 0:e611a0b9f02a 47 if (buttonNow && !buttonBefore { //detect button rising edge
Jacqueline 0:e611a0b9f02a 48 controller.set RotationalVelociy(0.0f);
Jacqueline 0:e611a0b9f02a 49 state = LANGSAM_FAHREN;
Jacqueline 3:6e28589a732f 50 } else if ((irSensorF.read() > DISTANCE_F)
Jacqueline 3:6e28589a732f 51 && (irSensorR.read() > DISTANCE_R)
Jacqueline 3:6e28589a732f 52 && (irSensorL.read() < DISTANCE_L) {
Jacqueline 0:e611a0b9f02a 53 controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
Jacqueline 0:e611a0b9f02a 54 state= VORWAERTSFAHREN;
Jacqueline 0:e611a0b9f02a 55 }
Jacqueline 0:e611a0b9f02a 56
Jacqueline 0:e611a0b9f02a 57
Jacqueline 0:e611a0b9f02a 58 case 180_GRAD_RECHTS
Jacqueline 0:e611a0b9f02a 59
Jacqueline 0:e611a0b9f02a 60 buttonNow && !button.read();
Jacqueline 0:e611a0b9f02a 61 if (buttonNow && !buttonBefore { // detect button rising edge
Jacqueline 0:e611a0b9f02a 62 state = LANGSAM_FAHREN;
Jacqueline 3:6e28589a732f 63 } else if (( irSensorR.read() > DISTANCE_R)
Jacqueline 3:6e28589a732f 64 && (irSensorF.read() > DISTANCE_F)
Jacqueline 3:6e28589a732f 65 && (irSensorL.read() > DISTANCE_L)) {
Jacqueline 0:e611a0b9f02a 66 controller.setTranslationalVelocity(TRANCLATIONAL_VELOCITY) {
Jacqueline 0:e611a0b9f02a 67 state = VORWAERTSFAHREN;
Jacqueline 0:e611a0b9f02a 68
Jacqueline 0:e611a0b9f02a 69
Jacqueline 0:e611a0b9f02a 70
Jacqueline 0:e611a0b9f02a 71 case LANGSAM_FAHREN;
Jacqueline 0:e611a0b9f02a 72
Jacqueline 0:e611a0b9f02a 73 if ((fabs(controller.getActualTranslationalVelocity()) < 0.01f )
Jacqueline 0:e611a0b9f02a 74 && (fabs(controller.getActualRotationalVelocity()) < 0.1f ) {
Jacqueline 0:e611a0b9f02a 75 enableMotorDriver = 0;
Jacqueline 0:e611a0b9f02a 76 state = ROBOT_OFF;
Jacqueline 0:e611a0b9f02a 77 }
Jacqueline 0:e611a0b9f02a 78 break;