ROME2 - TI / Mbed 2 deprecated ROME2 - Praktikum

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@1:08ca9b208045, 2017-03-31 (annotated)

Committer:
solcager
Date:
Fri Mar 31 11:00:19 2017 +0000
Revision:
1:08ca9b208045
P3

Who changed what in which revision?

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