ROME2 / Mbed 2 deprecated ROME2_P5

ROME_P5

Dependencies:   mbed

StateMachine.cpp@0:29be10cb0afc, 2018-04-27 (annotated)

Committer:
Inaueadr
Date:
Fri Apr 27 08:47:34 2018 +0000
Revision:
0:29be10cb0afc
Hallo

Who changed what in which revision?

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