ROME2 SM
Embed:
(wiki syntax)
Show/hide line numbers
StateMachine.cpp
00001 /* 00002 * StateMachine.cpp 00003 * Copyright (c) 2020, ZHAW 00004 * All rights reserved. 00005 */ 00006 00007 #include <cmath> 00008 #include "StateMachine.h" 00009 00010 using namespace std; 00011 00012 const float StateMachine::PERIOD = 0.01f; // period of task in [s] 00013 const float StateMachine::DISTANCE_THRESHOLD = 0.25f; // minimum allowed distance to obstacle in [m] 00014 const float StateMachine::TRANSLATIONAL_VELOCITY = 1.0f; // translational velocity in [m/s] 00015 const float StateMachine::ROTATIONAL_VELOCITY = 1.5f; // rotational velocity in [rad/s] 00016 00017 /** 00018 * Creates and initializes a state machine object. 00019 */ 00020 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) { 00021 00022 enableMotorDriver = 0; 00023 state = ROBOT_OFF; 00024 buttonNow = button; 00025 buttonBefore = buttonNow; 00026 00027 ticker.attach(callback(this, &StateMachine::run), PERIOD); 00028 } 00029 00030 /** 00031 * Deletes the state machine object and releases all allocated resources. 00032 */ 00033 StateMachine::~StateMachine() { 00034 00035 ticker.detach(); 00036 } 00037 00038 /** 00039 * Gets the actual state of this state machine. 00040 * @return the actual state as an int constant. 00041 */ 00042 int StateMachine::getState() { 00043 00044 return state; 00045 } 00046 00047 /** 00048 * This method is called periodically by the ticker object and implements the 00049 * logic of the state machine. 00050 */ 00051 void StateMachine::run() { 00052 00053 // set the leds based on distance measurements 00054 00055 led0 = irSensor0 < DISTANCE_THRESHOLD; 00056 led1 = irSensor1 < DISTANCE_THRESHOLD; 00057 led2 = irSensor2 < DISTANCE_THRESHOLD; 00058 led3 = irSensor3 < DISTANCE_THRESHOLD; 00059 led4 = irSensor4 < DISTANCE_THRESHOLD; 00060 led5 = irSensor5 < DISTANCE_THRESHOLD; 00061 00062 // implementation of the state machine 00063 00064 switch (state) { 00065 00066 case ROBOT_OFF: 00067 00068 buttonNow = button; 00069 00070 if (buttonNow && !buttonBefore) { // detect button rising edge 00071 00072 enableMotorDriver = 1; 00073 00074 controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY); 00075 controller.setRotationalVelocity(0.0f); 00076 00077 state = MOVE_FORWARD; 00078 } 00079 00080 buttonBefore = buttonNow; 00081 00082 break; 00083 00084 case MOVE_FORWARD: 00085 00086 if (!led2 && !led3 && !led4) { 00087 controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY); 00088 controller.setRotationalVelocity(0.0f); 00089 } 00090 else if(led2 || (led2&&led3)){ 00091 state = TURN_LEFT; 00092 } 00093 else if(led4 || (led3&&led4)){ 00094 state = TURN_RIGHT; 00095 } 00096 else if(led2 && led3 && led4){ 00097 state = SLOWING_DOWN; 00098 } 00099 00100 00101 // bitte implementieren! 00102 00103 break; 00104 00105 case TURN_LEFT: 00106 00107 00108 00109 if(led4 || led3 || (led4&&led3)){ 00110 controller.setTranslationalVelocity(0.0f); 00111 controller.setRotationalVelocity(-ROTATIONAL_VELOCITY); 00112 } 00113 00114 state = MOVE_FORWARD; 00115 // bitte implementieren! 00116 00117 break; 00118 00119 case TURN_RIGHT: 00120 00121 if(led3 || led2 || (led2&&led3)){ 00122 controller.setTranslationalVelocity(0.0f); 00123 controller.setRotationalVelocity(ROTATIONAL_VELOCITY); 00124 } 00125 00126 state = MOVE_FORWARD; 00127 // bitte implementieren! 00128 00129 break; 00130 00131 case SLOWING_DOWN: 00132 if (controller.getActualTranslationalVelocity()>0.0f){ 00133 controller.setTranslationalVelocity(0.0f); 00134 } else if (controller.getActualTranslationalVelocity()<= 0.0001f){ 00135 enableMotorDriver = 0; 00136 } 00137 state = ROBOT_OFF; 00138 00139 // bitte implementieren! 00140 00141 break; 00142 00143 default: 00144 00145 state = ROBOT_OFF; 00146 } 00147 } 00148
Generated on Sat Jul 23 2022 17:01:00 by
1.7.2