Sensoren auslesen/umwandeln/codieren und State Maschine
Dependencies: mbed
Fork of StateMachine_1 by
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
- Child:
- 4:91a9737e4821
neu
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 | 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; |