TeamSurface / Mbed 2 deprecated ROME_P3

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

Library/StateMachine.cpp@1:7bf9b6c007a1, 2018-03-23 (annotated)

Committer:
kueenste
Date:
Fri Mar 23 15:40:09 2018 +0000
Revision:
1:7bf9b6c007a1
Parent:
0:7cf5bf7e9486 
schissdreck funktioniert so halbe

Who changed what in which revision?

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