ROME_Praktikum / Mbed 2 deprecated Rome_P_3

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:   mbed

StateMachine.cpp@0:20ec9d702676, 2020-03-31 (annotated)

Committer:
Jacqueline
Date:
Tue Mar 31 11:58:30 2020 +0000
Revision:
0:20ec9d702676
Praktikum_3

Who changed what in which revision?

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