TeamSurface / Libs

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?

UserRevisionLine numberNew 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 }