Sensoren auslesen/umwandeln/codieren und State Maschine
Dependencies: mbed
Fork of StateMachine_1 by
StateMachine.cpp@0:e611a0b9f02a, 2018-04-16 (annotated)
- Committer:
- Jacqueline
- Date:
- Mon Apr 16 13:29:27 2018 +0000
- Revision:
- 0:e611a0b9f02a
- Child:
- 1:619ca6edc4a9
StateMachine: Cases eingef?gt
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:e611a0b9f02a | 10 | else if(((IRSensorF.read() < DISTANCE_THRESHOLD) |
Jacqueline | 0:e611a0b9f02a | 11 | && (IRSensorR.read() < DISTANCE_THRESHOLD)) { |
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 | 0:e611a0b9f02a | 16 | else if(IRSensorR.read() > DISTANCE_THRESHOLD){ |
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 | 0:e611a0b9f02a | 21 | else if((IRSensorF.read() < DISTANCE_THRESHOLD) |
Jacqueline | 0:e611a0b9f02a | 22 | && (IRSensorR.read() < DISTANCE_THRESHOLD) (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 | 0:e611a0b9f02a | 37 | } else if ((irSensorR < DISTANCE_THRESHOLD) { |
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 | 0:e611a0b9f02a | 50 | } else if ((irSensorF.read() > DISTANCE_THRESHOLD) |
Jacqueline | 0:e611a0b9f02a | 51 | && (irSensorR.read() > DISTANCE_THRESHOLD) |
Jacqueline | 0:e611a0b9f02a | 52 | && (irSensorL.read() < DISTANCE_THRESHOLD) { |
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 | 0:e611a0b9f02a | 63 | } else if (( irSensorR.read() > DISTANCE_THRESHOLD) |
Jacqueline | 0:e611a0b9f02a | 64 | && (irSensorF.read() > DISTANCE_THRESHOLD) |
Jacqueline | 0:e611a0b9f02a | 65 | && (irSensorL.read() > DISTANCE_THRESHOLD)) { |
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; |