Michael Ernst Peter / Mbed OS PM2_Example_ROME2_Robots

Example project

Dependencies:   PM2_Libary Eigen

StateMachine.cpp@38:8aae5cbcf25f, 2022-05-10 (annotated)

Committer:
pmic
Date:
Tue May 10 11:18:33 2022 +0200
Revision:
38:8aae5cbcf25f
Parent:
37:698d6b73b50c 
Adjusted ticker.attach

Who changed what in which revision?

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