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@0:7cf5bf7e9486, 2018-03-23 (annotated)

Committer:
kueenste
Date:
Fri Mar 23 13:07:58 2018 +0000
Revision:
0:7cf5bf7e9486
Child:
1:7bf9b6c007a1
P3 Anfang;

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 0:7cf5bf7e9486 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) {
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 0:7cf5bf7e9486 74 controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
kueenste 0:7cf5bf7e9486 75 controller.setRotationalVelocity(0.0f);
kueenste 0:7cf5bf7e9486 76
kueenste 0:7cf5bf7e9486 77 state = MOVE_FORWARD;
kueenste 0:7cf5bf7e9486 78 }
kueenste 0:7cf5bf7e9486 79
kueenste 0:7cf5bf7e9486 80 buttonBefore = buttonNow;
kueenste 0:7cf5bf7e9486 81
kueenste 0:7cf5bf7e9486 82 break;
kueenste 0:7cf5bf7e9486 83
kueenste 0:7cf5bf7e9486 84 case MOVE_FORWARD:
kueenste 0:7cf5bf7e9486 85
kueenste 0:7cf5bf7e9486 86 buttonNow = button;
kueenste 0:7cf5bf7e9486 87
kueenste 0:7cf5bf7e9486 88 if (buttonNow && !buttonBefore) { // detect button rising edge
kueenste 0:7cf5bf7e9486 89
kueenste 0:7cf5bf7e9486 90 controller.setTranslationalVelocity(0.0f);
kueenste 0:7cf5bf7e9486 91 controller.setRotationalVelocity(0.0f);
kueenste 0:7cf5bf7e9486 92
kueenste 0:7cf5bf7e9486 93 state = SLOWING_DOWN;
kueenste 0:7cf5bf7e9486 94
kueenste 0:7cf5bf7e9486 95 } else if ((irSensor0 < DISTANCE_THRESHOLD) || (irSensor1 < DISTANCE_THRESHOLD)) {
kueenste 0:7cf5bf7e9486 96
kueenste 0:7cf5bf7e9486 97 controller.setTranslationalVelocity(0.0f);
kueenste 0:7cf5bf7e9486 98 controller.setRotationalVelocity(ROTATIONAL_VELOCITY);
kueenste 0:7cf5bf7e9486 99
kueenste 0:7cf5bf7e9486 100 state = TURN_LEFT;
kueenste 0:7cf5bf7e9486 101
kueenste 0:7cf5bf7e9486 102 } else if (irSensor5 < DISTANCE_THRESHOLD) {
kueenste 0:7cf5bf7e9486 103
kueenste 0:7cf5bf7e9486 104 controller.setTranslationalVelocity(0.0f);
kueenste 0:7cf5bf7e9486 105 controller.setRotationalVelocity(-ROTATIONAL_VELOCITY);
kueenste 0:7cf5bf7e9486 106
kueenste 0:7cf5bf7e9486 107 state = TURN_RIGHT;
kueenste 0:7cf5bf7e9486 108 }
kueenste 0:7cf5bf7e9486 109
kueenste 0:7cf5bf7e9486 110 buttonBefore = buttonNow;
kueenste 0:7cf5bf7e9486 111
kueenste 0:7cf5bf7e9486 112 break;
kueenste 0:7cf5bf7e9486 113
kueenste 0:7cf5bf7e9486 114 case TURN_LEFT:
kueenste 0:7cf5bf7e9486 115
kueenste 0:7cf5bf7e9486 116 buttonNow = button;
kueenste 0:7cf5bf7e9486 117
kueenste 0:7cf5bf7e9486 118 if (buttonNow && !buttonBefore) { // detect button rising edge
kueenste 0:7cf5bf7e9486 119
kueenste 0:7cf5bf7e9486 120 controller.setRotationalVelocity(0.0f);
kueenste 0:7cf5bf7e9486 121
kueenste 0:7cf5bf7e9486 122 state = SLOWING_DOWN;
kueenste 0:7cf5bf7e9486 123
kueenste 0:7cf5bf7e9486 124 } else if ((irSensor0 > DISTANCE_THRESHOLD) && (irSensor1 > DISTANCE_THRESHOLD) && (irSensor5 > DISTANCE_THRESHOLD)) {
kueenste 0:7cf5bf7e9486 125
kueenste 0:7cf5bf7e9486 126 controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
kueenste 0:7cf5bf7e9486 127 controller.setRotationalVelocity(0.0f);
kueenste 0:7cf5bf7e9486 128
kueenste 0:7cf5bf7e9486 129 state = MOVE_FORWARD;
kueenste 0:7cf5bf7e9486 130 }
kueenste 0:7cf5bf7e9486 131
kueenste 0:7cf5bf7e9486 132 buttonBefore = buttonNow;
kueenste 0:7cf5bf7e9486 133
kueenste 0:7cf5bf7e9486 134 break;
kueenste 0:7cf5bf7e9486 135
kueenste 0:7cf5bf7e9486 136 case TURN_RIGHT:
kueenste 0:7cf5bf7e9486 137
kueenste 0:7cf5bf7e9486 138 buttonNow = button;
kueenste 0:7cf5bf7e9486 139
kueenste 0:7cf5bf7e9486 140 if (buttonNow && !buttonBefore) { // detect button rising edge
kueenste 0:7cf5bf7e9486 141
kueenste 0:7cf5bf7e9486 142 controller.setRotationalVelocity(0.0f);
kueenste 0:7cf5bf7e9486 143
kueenste 0:7cf5bf7e9486 144 state = SLOWING_DOWN;
kueenste 0:7cf5bf7e9486 145
kueenste 0:7cf5bf7e9486 146 } else if ((irSensor0 > DISTANCE_THRESHOLD) && (irSensor1 > DISTANCE_THRESHOLD) && (irSensor5 > DISTANCE_THRESHOLD)) {
kueenste 0:7cf5bf7e9486 147
kueenste 0:7cf5bf7e9486 148 controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
kueenste 0:7cf5bf7e9486 149 controller.setRotationalVelocity(0.0f);
kueenste 0:7cf5bf7e9486 150
kueenste 0:7cf5bf7e9486 151 state = MOVE_FORWARD;
kueenste 0:7cf5bf7e9486 152 }
kueenste 0:7cf5bf7e9486 153
kueenste 0:7cf5bf7e9486 154 buttonBefore = buttonNow;
kueenste 0:7cf5bf7e9486 155
kueenste 0:7cf5bf7e9486 156 break;
kueenste 0:7cf5bf7e9486 157
kueenste 0:7cf5bf7e9486 158 case SLOWING_DOWN:
kueenste 0:7cf5bf7e9486 159
kueenste 0:7cf5bf7e9486 160 if ((fabs(controller.getActualTranslationalVelocity()) < 0.01f) && (fabs(controller.getActualRotationalVelocity()) < 0.01f)) {
kueenste 0:7cf5bf7e9486 161
kueenste 0:7cf5bf7e9486 162 enableMotorDriver = 0;
kueenste 0:7cf5bf7e9486 163
kueenste 0:7cf5bf7e9486 164 state = ROBOT_OFF;
kueenste 0:7cf5bf7e9486 165 }
kueenste 0:7cf5bf7e9486 166
kueenste 0:7cf5bf7e9486 167 break;
kueenste 0:7cf5bf7e9486 168
kueenste 0:7cf5bf7e9486 169 default:
kueenste 0:7cf5bf7e9486 170
kueenste 0:7cf5bf7e9486 171 state = ROBOT_OFF;
kueenste 0:7cf5bf7e9486 172 }
kueenste 0:7cf5bf7e9486 173 }
kueenste 0:7cf5bf7e9486 174