Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P4

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:   ROME2_P2 mbed

Fork of ROME2_P3 by Zürcher Eliteeinheit

StateMachine.cpp@2:13da48f33ab7, 2018-03-16 (annotated)

Committer:
Appalco
Date:
Fri Mar 16 15:17:08 2018 +0000
Revision:
2:13da48f33ab7
Parent:
0:e360940c4b88
Child:
3:9433326c2248
working version middle sensor will be ignored

Who changed what in which revision?

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