Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ROME2_P2 mbed

Fork of ROME2_P3 by Zürcher Eliteeinheit

StateMachine.cpp@5:59079b76ac7f, 2018-04-12 (annotated)

Committer:
matajarb
Date:
Thu Apr 12 14:56:27 2018 +0000
Revision:
5:59079b76ac7f
Parent:
4:4428ede9beee
Child:
6:67263dc2c2cf
lutsched eieer

Who changed what in which revision?

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