Marco Oehler / Mbed 2 deprecated Lab2

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:1a972ed770da, 2020-03-09 (annotated)

Committer:
oehlemar
Date:
Mon Mar 09 16:23:04 2020 +0000
Revision:
0:1a972ed770da
LAB2

Who changed what in which revision?

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