Allan Brignoli / Mbed 2 deprecated Rome2_P6

gugus

Dependencies:   mbed

StateMachine.cpp@0:1a0321f1ffbc, 2018-05-18 (annotated)

Committer:
Brignall
Date:
Fri May 18 12:18:21 2018 +0000
Revision:
0:1a0321f1ffbc
lala;

Who changed what in which revision?

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