Marco Oehler / Mbed OS Lab3

ROME2 Lab3

StateMachine.cpp@2:fc9e2aebf9d5, 2020-03-25 (annotated)

Committer:
oehlemar
Date:
Wed Mar 25 14:15:52 2020 +0000
Revision:
2:fc9e2aebf9d5
Parent:
1:ff05970bb9b0 
final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oehlemar 0:6a4d3264c067 1 /*
oehlemar 0:6a4d3264c067 2 * StateMachine.cpp
oehlemar 0:6a4d3264c067 3 * Copyright (c) 2020, ZHAW
oehlemar 0:6a4d3264c067 4 * All rights reserved.
oehlemar 0:6a4d3264c067 5 */
oehlemar 0:6a4d3264c067 6
oehlemar 0:6a4d3264c067 7 #include <cmath>
oehlemar 0:6a4d3264c067 8 #include "StateMachine.h"
oehlemar 1:ff05970bb9b0 9 #include <deque>
oehlemar 1:ff05970bb9b0 10 #include "Task.h"
oehlemar 1:ff05970bb9b0 11 #include "TaskWait.h"
oehlemar 1:ff05970bb9b0 12 #include "TaskMoveTo.h"
oehlemar 0:6a4d3264c067 13
oehlemar 0:6a4d3264c067 14 using namespace std;
oehlemar 0:6a4d3264c067 15
oehlemar 0:6a4d3264c067 16 const float StateMachine::PERIOD = 0.01f; // period of task in [s]
oehlemar 0:6a4d3264c067 17 const float StateMachine::DISTANCE_THRESHOLD = 0.2f; // minimum allowed distance to obstacle in [m]
oehlemar 0:6a4d3264c067 18 const float StateMachine::TRANSLATIONAL_VELOCITY = 0.5f; // translational velocity in [m/s]
oehlemar 0:6a4d3264c067 19 const float StateMachine::ROTATIONAL_VELOCITY = 2.0f; // rotational velocity in [rad/s]
oehlemar 0:6a4d3264c067 20
oehlemar 0:6a4d3264c067 21 /**
oehlemar 0:6a4d3264c067 22 * Creates and initializes a state machine object.
oehlemar 0:6a4d3264c067 23 */
oehlemar 0:6a4d3264c067 24 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) {
oehlemar 0:6a4d3264c067 25
oehlemar 0:6a4d3264c067 26 enableMotorDriver = 0;
oehlemar 0:6a4d3264c067 27 state = ROBOT_OFF;
oehlemar 0:6a4d3264c067 28 buttonNow = button;
oehlemar 0:6a4d3264c067 29 buttonBefore = buttonNow;
oehlemar 0:6a4d3264c067 30
oehlemar 0:6a4d3264c067 31 ticker.attach(callback(this, &StateMachine::run), PERIOD);
oehlemar 0:6a4d3264c067 32 }
oehlemar 0:6a4d3264c067 33
oehlemar 0:6a4d3264c067 34 /**
oehlemar 0:6a4d3264c067 35 * Deletes the state machine object and releases all allocated resources.
oehlemar 0:6a4d3264c067 36 */
oehlemar 0:6a4d3264c067 37 StateMachine::~StateMachine() {
oehlemar 0:6a4d3264c067 38
oehlemar 0:6a4d3264c067 39 ticker.detach();
oehlemar 0:6a4d3264c067 40 }
oehlemar 0:6a4d3264c067 41
oehlemar 0:6a4d3264c067 42 /**
oehlemar 0:6a4d3264c067 43 * Gets the actual state of this state machine.
oehlemar 0:6a4d3264c067 44 * @return the actual state as an int constant.
oehlemar 0:6a4d3264c067 45 */
oehlemar 0:6a4d3264c067 46 int StateMachine::getState() {
oehlemar 0:6a4d3264c067 47
oehlemar 0:6a4d3264c067 48 return state;
oehlemar 0:6a4d3264c067 49 }
oehlemar 0:6a4d3264c067 50
oehlemar 0:6a4d3264c067 51 /**
oehlemar 0:6a4d3264c067 52 * This method is called periodically by the ticker object and implements the
oehlemar 0:6a4d3264c067 53 * logic of the state machine.
oehlemar 0:6a4d3264c067 54 */
oehlemar 0:6a4d3264c067 55 void StateMachine::run() {
oehlemar 0:6a4d3264c067 56
oehlemar 0:6a4d3264c067 57 // set the leds based on distance measurements
oehlemar 0:6a4d3264c067 58
oehlemar 0:6a4d3264c067 59 led0 = irSensor0 < DISTANCE_THRESHOLD;
oehlemar 0:6a4d3264c067 60 led1 = irSensor1 < DISTANCE_THRESHOLD;
oehlemar 0:6a4d3264c067 61 led2 = irSensor2 < DISTANCE_THRESHOLD;
oehlemar 0:6a4d3264c067 62 led3 = irSensor3 < DISTANCE_THRESHOLD;
oehlemar 0:6a4d3264c067 63 led4 = irSensor4 < DISTANCE_THRESHOLD;
oehlemar 0:6a4d3264c067 64 led5 = irSensor5 < DISTANCE_THRESHOLD;
oehlemar 0:6a4d3264c067 65
oehlemar 0:6a4d3264c067 66 // implementation of the state machine
oehlemar 0:6a4d3264c067 67
oehlemar 0:6a4d3264c067 68 switch (state) {
oehlemar 0:6a4d3264c067 69
oehlemar 0:6a4d3264c067 70 case ROBOT_OFF:
oehlemar 0:6a4d3264c067 71
oehlemar 0:6a4d3264c067 72 buttonNow = button;
oehlemar 0:6a4d3264c067 73
oehlemar 0:6a4d3264c067 74 if (buttonNow && !buttonBefore) { // detect button rising edge
oehlemar 0:6a4d3264c067 75
oehlemar 0:6a4d3264c067 76 enableMotorDriver = 1;
oehlemar 0:6a4d3264c067 77
oehlemar 1:ff05970bb9b0 78 taskList.push_back(new TaskMoveTo(controller, 0.0f, 1.0f, 0.0f));
oehlemar 1:ff05970bb9b0 79 taskList.push_back(new TaskWait(controller, 2.0f));
oehlemar 1:ff05970bb9b0 80 taskList.push_back(new TaskMoveTo(controller, 1.0f, 1.0f, -1.0f));
oehlemar 1:ff05970bb9b0 81 taskList.push_back(new TaskWait(controller, 2.0f));
oehlemar 1:ff05970bb9b0 82 taskList.push_back(new TaskMoveTo(controller, 1.0f, 0.0f, 0.0f));
oehlemar 1:ff05970bb9b0 83 taskList.push_back(new TaskWait(controller, 2.0f));
oehlemar 1:ff05970bb9b0 84 taskList.push_back(new TaskMoveTo(controller, 0.0f, 0.0f, 0.0f));
oehlemar 1:ff05970bb9b0 85 taskList.push_back(new TaskWait(controller, 2.0f));
oehlemar 0:6a4d3264c067 86
oehlemar 0:6a4d3264c067 87 state = MOVE_FORWARD;
oehlemar 0:6a4d3264c067 88 }
oehlemar 0:6a4d3264c067 89
oehlemar 0:6a4d3264c067 90 buttonBefore = buttonNow;
oehlemar 0:6a4d3264c067 91
oehlemar 0:6a4d3264c067 92 break;
oehlemar 0:6a4d3264c067 93
oehlemar 0:6a4d3264c067 94 case MOVE_FORWARD:
oehlemar 0:6a4d3264c067 95
oehlemar 0:6a4d3264c067 96 buttonNow = button;
oehlemar 0:6a4d3264c067 97
oehlemar 2:fc9e2aebf9d5 98
oehlemar 1:ff05970bb9b0 99
oehlemar 0:6a4d3264c067 100 if (buttonNow && !buttonBefore) { // detect button rising edge
oehlemar 0:6a4d3264c067 101
oehlemar 0:6a4d3264c067 102 controller.setTranslationalVelocity(0.0f);
oehlemar 0:6a4d3264c067 103 controller.setRotationalVelocity(0.0f);
oehlemar 0:6a4d3264c067 104
oehlemar 0:6a4d3264c067 105 state = SLOWING_DOWN;
oehlemar 0:6a4d3264c067 106
oehlemar 0:6a4d3264c067 107 } else if ((irSensor3 < DISTANCE_THRESHOLD) || (irSensor4 < DISTANCE_THRESHOLD)) {
oehlemar 0:6a4d3264c067 108
oehlemar 0:6a4d3264c067 109 controller.setTranslationalVelocity(0.0f);
oehlemar 0:6a4d3264c067 110 controller.setRotationalVelocity(ROTATIONAL_VELOCITY);
oehlemar 0:6a4d3264c067 111
oehlemar 0:6a4d3264c067 112 state = TURN_LEFT;
oehlemar 0:6a4d3264c067 113
oehlemar 0:6a4d3264c067 114 } else if (irSensor2 < DISTANCE_THRESHOLD) {
oehlemar 0:6a4d3264c067 115
oehlemar 0:6a4d3264c067 116 controller.setTranslationalVelocity(0.0f);
oehlemar 0:6a4d3264c067 117 controller.setRotationalVelocity(-ROTATIONAL_VELOCITY);
oehlemar 0:6a4d3264c067 118
oehlemar 0:6a4d3264c067 119 state = TURN_RIGHT;
oehlemar 0:6a4d3264c067 120
oehlemar 2:fc9e2aebf9d5 121 } else if (taskList.size() > 0) { // if statement hier unten, da kollision erkennung wichtiger
oehlemar 2:fc9e2aebf9d5 122
oehlemar 2:fc9e2aebf9d5 123 Task* task = taskList.front();
oehlemar 2:fc9e2aebf9d5 124 int result = task->run(PERIOD);
oehlemar 2:fc9e2aebf9d5 125
oehlemar 2:fc9e2aebf9d5 126 if (result == Task::DONE) {
oehlemar 2:fc9e2aebf9d5 127
oehlemar 2:fc9e2aebf9d5 128 taskList.pop_front();
oehlemar 2:fc9e2aebf9d5 129 delete task;
oehlemar 2:fc9e2aebf9d5 130
oehlemar 2:fc9e2aebf9d5 131 } else {
oehlemar 2:fc9e2aebf9d5 132
oehlemar 2:fc9e2aebf9d5 133 controller.setTranslationalVelocity(0.0f); //Sicher gehen, dass die letzen Geschw. auch wirklich 0 sind
oehlemar 2:fc9e2aebf9d5 134 controller.setRotationalVelocity(0.0f);
oehlemar 2:fc9e2aebf9d5 135 state = SLOWING_DOWN;
oehlemar 2:fc9e2aebf9d5 136
oehlemar 2:fc9e2aebf9d5 137 }
oehlemar 2:fc9e2aebf9d5 138 } else {
oehlemar 0:6a4d3264c067 139
oehlemar 0:6a4d3264c067 140 }
oehlemar 0:6a4d3264c067 141
oehlemar 0:6a4d3264c067 142 buttonBefore = buttonNow;
oehlemar 0:6a4d3264c067 143
oehlemar 0:6a4d3264c067 144 break;
oehlemar 0:6a4d3264c067 145
oehlemar 0:6a4d3264c067 146 case TURN_LEFT:
oehlemar 0:6a4d3264c067 147
oehlemar 0:6a4d3264c067 148 buttonNow = button;
oehlemar 0:6a4d3264c067 149
oehlemar 0:6a4d3264c067 150 if (buttonNow && !buttonBefore) { // detect button rising edge
oehlemar 0:6a4d3264c067 151
oehlemar 0:6a4d3264c067 152 controller.setRotationalVelocity(0.0f);
oehlemar 0:6a4d3264c067 153
oehlemar 0:6a4d3264c067 154 state = SLOWING_DOWN;
oehlemar 0:6a4d3264c067 155
oehlemar 0:6a4d3264c067 156 } else if ((irSensor2 > DISTANCE_THRESHOLD) && (irSensor3 > DISTANCE_THRESHOLD) && (irSensor4 > DISTANCE_THRESHOLD)) {
oehlemar 0:6a4d3264c067 157
oehlemar 0:6a4d3264c067 158 controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
oehlemar 0:6a4d3264c067 159 controller.setRotationalVelocity(0.0f);
oehlemar 0:6a4d3264c067 160
oehlemar 0:6a4d3264c067 161 state = MOVE_FORWARD;
oehlemar 0:6a4d3264c067 162 }
oehlemar 0:6a4d3264c067 163
oehlemar 0:6a4d3264c067 164 buttonBefore = buttonNow;
oehlemar 0:6a4d3264c067 165
oehlemar 0:6a4d3264c067 166 break;
oehlemar 0:6a4d3264c067 167
oehlemar 0:6a4d3264c067 168 case TURN_RIGHT:
oehlemar 0:6a4d3264c067 169
oehlemar 0:6a4d3264c067 170 buttonNow = button;
oehlemar 0:6a4d3264c067 171
oehlemar 0:6a4d3264c067 172 if (buttonNow && !buttonBefore) { // detect button rising edge
oehlemar 0:6a4d3264c067 173
oehlemar 0:6a4d3264c067 174 controller.setRotationalVelocity(0.0f);
oehlemar 0:6a4d3264c067 175
oehlemar 0:6a4d3264c067 176 state = SLOWING_DOWN;
oehlemar 0:6a4d3264c067 177
oehlemar 0:6a4d3264c067 178 } else if ((irSensor2 > DISTANCE_THRESHOLD) && (irSensor3 > DISTANCE_THRESHOLD) && (irSensor4 > DISTANCE_THRESHOLD)) {
oehlemar 0:6a4d3264c067 179
oehlemar 0:6a4d3264c067 180 controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
oehlemar 0:6a4d3264c067 181 controller.setRotationalVelocity(0.0f);
oehlemar 0:6a4d3264c067 182
oehlemar 0:6a4d3264c067 183 state = MOVE_FORWARD;
oehlemar 0:6a4d3264c067 184 }
oehlemar 0:6a4d3264c067 185
oehlemar 0:6a4d3264c067 186 buttonBefore = buttonNow;
oehlemar 0:6a4d3264c067 187
oehlemar 0:6a4d3264c067 188 break;
oehlemar 0:6a4d3264c067 189
oehlemar 0:6a4d3264c067 190 case SLOWING_DOWN:
oehlemar 0:6a4d3264c067 191
oehlemar 0:6a4d3264c067 192 if ((fabs(controller.getActualTranslationalVelocity()) < 0.01f) && (fabs(controller.getActualRotationalVelocity()) < 0.01f)) {
oehlemar 0:6a4d3264c067 193
oehlemar 1:ff05970bb9b0 194 while (taskList.size() > 0) {
oehlemar 1:ff05970bb9b0 195 delete taskList.front();
oehlemar 1:ff05970bb9b0 196 taskList.pop_front();
oehlemar 1:ff05970bb9b0 197 }
oehlemar 1:ff05970bb9b0 198
oehlemar 0:6a4d3264c067 199 enableMotorDriver = 0;
oehlemar 0:6a4d3264c067 200
oehlemar 0:6a4d3264c067 201 state = ROBOT_OFF;
oehlemar 0:6a4d3264c067 202 }
oehlemar 0:6a4d3264c067 203
oehlemar 0:6a4d3264c067 204 break;
oehlemar 0:6a4d3264c067 205
oehlemar 0:6a4d3264c067 206 default:
oehlemar 0:6a4d3264c067 207
oehlemar 0:6a4d3264c067 208 state = ROBOT_OFF;
oehlemar 0:6a4d3264c067 209 }
oehlemar 0:6a4d3264c067 210 }
oehlemar 0:6a4d3264c067 211