Marco Oehler / Mbed OS Lab3

ROME2 Lab3

StateMachine.cpp@0:6a4d3264c067, 2020-03-24 (annotated)

Committer:
oehlemar
Date:
Tue Mar 24 08:39:54 2020 +0000
Revision:
0:6a4d3264c067
Child:
1:ff05970bb9b0
Lab3

Who changed what in which revision?

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