Nim leo niiiim
StateMachine.cpp@0:da791f233257, 2018-05-11 (annotated)
- Committer:
- Kiwicjam
- Date:
- Fri May 11 12:21:19 2018 +0000
- Revision:
- 0:da791f233257
start of rome2 p5;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Kiwicjam | 0:da791f233257 | 1 | /* |
Kiwicjam | 0:da791f233257 | 2 | * StateMachine.cpp |
Kiwicjam | 0:da791f233257 | 3 | * Copyright (c) 2018, ZHAW |
Kiwicjam | 0:da791f233257 | 4 | * All rights reserved. |
Kiwicjam | 0:da791f233257 | 5 | */ |
Kiwicjam | 0:da791f233257 | 6 | |
Kiwicjam | 0:da791f233257 | 7 | #include <cmath> |
Kiwicjam | 0:da791f233257 | 8 | #include "StateMachine.h" |
Kiwicjam | 0:da791f233257 | 9 | |
Kiwicjam | 0:da791f233257 | 10 | using namespace std; |
Kiwicjam | 0:da791f233257 | 11 | |
Kiwicjam | 0:da791f233257 | 12 | const float StateMachine::PERIOD = 0.01f; // period of task in [s] |
Kiwicjam | 0:da791f233257 | 13 | const float StateMachine::DISTANCE_THRESHOLD = 0.2f; // minimum allowed distance to obstacle in [m] |
Kiwicjam | 0:da791f233257 | 14 | const float StateMachine::TRANSLATIONAL_VELOCITY = 0.3f; // translational velocity in [m/s] |
Kiwicjam | 0:da791f233257 | 15 | const float StateMachine::ROTATIONAL_VELOCITY = 1.0f; // rotational velocity in [rad/s] |
Kiwicjam | 0:da791f233257 | 16 | |
Kiwicjam | 0:da791f233257 | 17 | /** |
Kiwicjam | 0:da791f233257 | 18 | * Creates and initializes a state machine object. |
Kiwicjam | 0:da791f233257 | 19 | */ |
Kiwicjam | 0:da791f233257 | 20 | StateMachine::StateMachine(Controller& controller, DigitalOut& enableMotorDriver, DigitalOut& led0, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4, DigitalOut& led5, DigitalIn& button, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, IRSensor& irSensor4, IRSensor& irSensor5) : controller(controller), enableMotorDriver(enableMotorDriver), led0(led0), led1(led1), led2(led2), led3(led3), led4(led4), led5(led5), button(button), irSensor0(irSensor0), irSensor1(irSensor1), irSensor2(irSensor2), irSensor3(irSensor3), irSensor4(irSensor4), irSensor5(irSensor5) { |
Kiwicjam | 0:da791f233257 | 21 | |
Kiwicjam | 0:da791f233257 | 22 | enableMotorDriver = 0; |
Kiwicjam | 0:da791f233257 | 23 | state = ROBOT_OFF; |
Kiwicjam | 0:da791f233257 | 24 | buttonNow = button; |
Kiwicjam | 0:da791f233257 | 25 | buttonBefore = buttonNow; |
Kiwicjam | 0:da791f233257 | 26 | taskList.clear(); |
Kiwicjam | 0:da791f233257 | 27 | |
Kiwicjam | 0:da791f233257 | 28 | ticker.attach(callback(this, &StateMachine::run), PERIOD); |
Kiwicjam | 0:da791f233257 | 29 | } |
Kiwicjam | 0:da791f233257 | 30 | |
Kiwicjam | 0:da791f233257 | 31 | /** |
Kiwicjam | 0:da791f233257 | 32 | * Deletes the state machine object and releases all allocated resources. |
Kiwicjam | 0:da791f233257 | 33 | */ |
Kiwicjam | 0:da791f233257 | 34 | StateMachine::~StateMachine() { |
Kiwicjam | 0:da791f233257 | 35 | |
Kiwicjam | 0:da791f233257 | 36 | ticker.detach(); |
Kiwicjam | 0:da791f233257 | 37 | } |
Kiwicjam | 0:da791f233257 | 38 | |
Kiwicjam | 0:da791f233257 | 39 | /** |
Kiwicjam | 0:da791f233257 | 40 | * Gets the actual state of this state machine. |
Kiwicjam | 0:da791f233257 | 41 | * @return the actual state as an int constant. |
Kiwicjam | 0:da791f233257 | 42 | */ |
Kiwicjam | 0:da791f233257 | 43 | int StateMachine::getState() { |
Kiwicjam | 0:da791f233257 | 44 | |
Kiwicjam | 0:da791f233257 | 45 | return state; |
Kiwicjam | 0:da791f233257 | 46 | } |
Kiwicjam | 0:da791f233257 | 47 | |
Kiwicjam | 0:da791f233257 | 48 | /** |
Kiwicjam | 0:da791f233257 | 49 | * This method is called periodically by the ticker object and implements the |
Kiwicjam | 0:da791f233257 | 50 | * logic of the state machine. |
Kiwicjam | 0:da791f233257 | 51 | */ |
Kiwicjam | 0:da791f233257 | 52 | void StateMachine::run() { |
Kiwicjam | 0:da791f233257 | 53 | |
Kiwicjam | 0:da791f233257 | 54 | // set the leds based on distance measurements |
Kiwicjam | 0:da791f233257 | 55 | /* |
Kiwicjam | 0:da791f233257 | 56 | led0 = irSensor0 < DISTANCE_THRESHOLD; |
Kiwicjam | 0:da791f233257 | 57 | led1 = irSensor1 < DISTANCE_THRESHOLD; |
Kiwicjam | 0:da791f233257 | 58 | led2 = irSensor2 < DISTANCE_THRESHOLD; |
Kiwicjam | 0:da791f233257 | 59 | led3 = irSensor3 < DISTANCE_THRESHOLD; |
Kiwicjam | 0:da791f233257 | 60 | led4 = irSensor4 < DISTANCE_THRESHOLD; |
Kiwicjam | 0:da791f233257 | 61 | led5 = irSensor5 < DISTANCE_THRESHOLD; |
Kiwicjam | 0:da791f233257 | 62 | */ |
Kiwicjam | 0:da791f233257 | 63 | // implementation of the state machine |
Kiwicjam | 0:da791f233257 | 64 | |
Kiwicjam | 0:da791f233257 | 65 | switch (state) { |
Kiwicjam | 0:da791f233257 | 66 | |
Kiwicjam | 0:da791f233257 | 67 | case ROBOT_OFF: |
Kiwicjam | 0:da791f233257 | 68 | |
Kiwicjam | 0:da791f233257 | 69 | buttonNow = button; |
Kiwicjam | 0:da791f233257 | 70 | |
Kiwicjam | 0:da791f233257 | 71 | if (buttonNow && !buttonBefore) { // detect button rising edge |
Kiwicjam | 0:da791f233257 | 72 | |
Kiwicjam | 0:da791f233257 | 73 | enableMotorDriver = 1; |
Kiwicjam | 0:da791f233257 | 74 | |
Kiwicjam | 0:da791f233257 | 75 | taskList.push_back(new TaskWait(controller, 0.5f)); |
Kiwicjam | 0:da791f233257 | 76 | taskList.push_back(new TaskMove(controller, 0.1f, 0.0f)); |
Kiwicjam | 0:da791f233257 | 77 | |
Kiwicjam | 0:da791f233257 | 78 | state = PROCESSING_TASKS; |
Kiwicjam | 0:da791f233257 | 79 | } |
Kiwicjam | 0:da791f233257 | 80 | |
Kiwicjam | 0:da791f233257 | 81 | buttonBefore = buttonNow; |
Kiwicjam | 0:da791f233257 | 82 | |
Kiwicjam | 0:da791f233257 | 83 | break; |
Kiwicjam | 0:da791f233257 | 84 | |
Kiwicjam | 0:da791f233257 | 85 | case PROCESSING_TASKS: |
Kiwicjam | 0:da791f233257 | 86 | |
Kiwicjam | 0:da791f233257 | 87 | buttonNow = button; |
Kiwicjam | 0:da791f233257 | 88 | |
Kiwicjam | 0:da791f233257 | 89 | if (buttonNow && !buttonBefore) { // detect button rising edge |
Kiwicjam | 0:da791f233257 | 90 | |
Kiwicjam | 0:da791f233257 | 91 | controller.setTranslationalVelocity(0.0f); |
Kiwicjam | 0:da791f233257 | 92 | controller.setRotationalVelocity(0.0f); |
Kiwicjam | 0:da791f233257 | 93 | |
Kiwicjam | 0:da791f233257 | 94 | state = SLOWING_DOWN; |
Kiwicjam | 0:da791f233257 | 95 | |
Kiwicjam | 0:da791f233257 | 96 | } else if ((irSensor0 < DISTANCE_THRESHOLD) || (irSensor1 < DISTANCE_THRESHOLD)) { |
Kiwicjam | 0:da791f233257 | 97 | |
Kiwicjam | 0:da791f233257 | 98 | controller.setTranslationalVelocity(0.0f); |
Kiwicjam | 0:da791f233257 | 99 | controller.setRotationalVelocity(ROTATIONAL_VELOCITY); |
Kiwicjam | 0:da791f233257 | 100 | |
Kiwicjam | 0:da791f233257 | 101 | state = TURN_LEFT; |
Kiwicjam | 0:da791f233257 | 102 | |
Kiwicjam | 0:da791f233257 | 103 | } else if (irSensor5 < DISTANCE_THRESHOLD) { |
Kiwicjam | 0:da791f233257 | 104 | |
Kiwicjam | 0:da791f233257 | 105 | controller.setTranslationalVelocity(0.0f); |
Kiwicjam | 0:da791f233257 | 106 | controller.setRotationalVelocity(-ROTATIONAL_VELOCITY); |
Kiwicjam | 0:da791f233257 | 107 | |
Kiwicjam | 0:da791f233257 | 108 | state = TURN_RIGHT; |
Kiwicjam | 0:da791f233257 | 109 | |
Kiwicjam | 0:da791f233257 | 110 | } else if (taskList.size() > 0) { |
Kiwicjam | 0:da791f233257 | 111 | |
Kiwicjam | 0:da791f233257 | 112 | Task* task = taskList.front(); |
Kiwicjam | 0:da791f233257 | 113 | int result = task->run(PERIOD); |
Kiwicjam | 0:da791f233257 | 114 | if (result == Task::DONE) { |
Kiwicjam | 0:da791f233257 | 115 | taskList.pop_front(); |
Kiwicjam | 0:da791f233257 | 116 | delete task; |
Kiwicjam | 0:da791f233257 | 117 | } |
Kiwicjam | 0:da791f233257 | 118 | |
Kiwicjam | 0:da791f233257 | 119 | } else { |
Kiwicjam | 0:da791f233257 | 120 | |
Kiwicjam | 0:da791f233257 | 121 | controller.setTranslationalVelocity(0.0f); |
Kiwicjam | 0:da791f233257 | 122 | controller.setRotationalVelocity(0.0f); |
Kiwicjam | 0:da791f233257 | 123 | |
Kiwicjam | 0:da791f233257 | 124 | state = SLOWING_DOWN; |
Kiwicjam | 0:da791f233257 | 125 | } |
Kiwicjam | 0:da791f233257 | 126 | |
Kiwicjam | 0:da791f233257 | 127 | buttonBefore = buttonNow; |
Kiwicjam | 0:da791f233257 | 128 | |
Kiwicjam | 0:da791f233257 | 129 | break; |
Kiwicjam | 0:da791f233257 | 130 | |
Kiwicjam | 0:da791f233257 | 131 | case TURN_LEFT: |
Kiwicjam | 0:da791f233257 | 132 | |
Kiwicjam | 0:da791f233257 | 133 | buttonNow = button; |
Kiwicjam | 0:da791f233257 | 134 | |
Kiwicjam | 0:da791f233257 | 135 | if (buttonNow && !buttonBefore) { // detect button rising edge |
Kiwicjam | 0:da791f233257 | 136 | |
Kiwicjam | 0:da791f233257 | 137 | controller.setTranslationalVelocity(0.0f); |
Kiwicjam | 0:da791f233257 | 138 | controller.setRotationalVelocity(0.0f); |
Kiwicjam | 0:da791f233257 | 139 | |
Kiwicjam | 0:da791f233257 | 140 | state = SLOWING_DOWN; |
Kiwicjam | 0:da791f233257 | 141 | |
Kiwicjam | 0:da791f233257 | 142 | } else if ((irSensor0 > DISTANCE_THRESHOLD) && (irSensor1 > DISTANCE_THRESHOLD) && (irSensor5 > DISTANCE_THRESHOLD)) { |
Kiwicjam | 0:da791f233257 | 143 | |
Kiwicjam | 0:da791f233257 | 144 | state = PROCESSING_TASKS; |
Kiwicjam | 0:da791f233257 | 145 | } |
Kiwicjam | 0:da791f233257 | 146 | |
Kiwicjam | 0:da791f233257 | 147 | buttonBefore = buttonNow; |
Kiwicjam | 0:da791f233257 | 148 | |
Kiwicjam | 0:da791f233257 | 149 | break; |
Kiwicjam | 0:da791f233257 | 150 | |
Kiwicjam | 0:da791f233257 | 151 | case TURN_RIGHT: |
Kiwicjam | 0:da791f233257 | 152 | |
Kiwicjam | 0:da791f233257 | 153 | buttonNow = button; |
Kiwicjam | 0:da791f233257 | 154 | |
Kiwicjam | 0:da791f233257 | 155 | if (buttonNow && !buttonBefore) { // detect button rising edge |
Kiwicjam | 0:da791f233257 | 156 | |
Kiwicjam | 0:da791f233257 | 157 | controller.setTranslationalVelocity(0.0f); |
Kiwicjam | 0:da791f233257 | 158 | controller.setRotationalVelocity(0.0f); |
Kiwicjam | 0:da791f233257 | 159 | |
Kiwicjam | 0:da791f233257 | 160 | state = SLOWING_DOWN; |
Kiwicjam | 0:da791f233257 | 161 | |
Kiwicjam | 0:da791f233257 | 162 | } else if ((irSensor0 > DISTANCE_THRESHOLD) && (irSensor1 > DISTANCE_THRESHOLD) && (irSensor5 > DISTANCE_THRESHOLD)) { |
Kiwicjam | 0:da791f233257 | 163 | |
Kiwicjam | 0:da791f233257 | 164 | state = PROCESSING_TASKS; |
Kiwicjam | 0:da791f233257 | 165 | } |
Kiwicjam | 0:da791f233257 | 166 | |
Kiwicjam | 0:da791f233257 | 167 | buttonBefore = buttonNow; |
Kiwicjam | 0:da791f233257 | 168 | |
Kiwicjam | 0:da791f233257 | 169 | break; |
Kiwicjam | 0:da791f233257 | 170 | |
Kiwicjam | 0:da791f233257 | 171 | case SLOWING_DOWN: |
Kiwicjam | 0:da791f233257 | 172 | |
Kiwicjam | 0:da791f233257 | 173 | if ((fabs(controller.getActualTranslationalVelocity()) < 0.01f) && (fabs(controller.getActualRotationalVelocity()) < 0.01f)) { |
Kiwicjam | 0:da791f233257 | 174 | |
Kiwicjam | 0:da791f233257 | 175 | enableMotorDriver = 0; |
Kiwicjam | 0:da791f233257 | 176 | |
Kiwicjam | 0:da791f233257 | 177 | while (taskList.size() > 0) { |
Kiwicjam | 0:da791f233257 | 178 | delete taskList.front(); |
Kiwicjam | 0:da791f233257 | 179 | taskList.pop_front(); |
Kiwicjam | 0:da791f233257 | 180 | } |
Kiwicjam | 0:da791f233257 | 181 | |
Kiwicjam | 0:da791f233257 | 182 | state = ROBOT_OFF; |
Kiwicjam | 0:da791f233257 | 183 | } |
Kiwicjam | 0:da791f233257 | 184 | |
Kiwicjam | 0:da791f233257 | 185 | break; |
Kiwicjam | 0:da791f233257 | 186 | |
Kiwicjam | 0:da791f233257 | 187 | default: |
Kiwicjam | 0:da791f233257 | 188 | |
Kiwicjam | 0:da791f233257 | 189 | state = ROBOT_OFF; |
Kiwicjam | 0:da791f233257 | 190 | } |
Kiwicjam | 0:da791f233257 | 191 | } |