ROME2 Lab3
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 #include <deque> 00010 #include "Task.h" 00011 #include "TaskWait.h" 00012 #include "TaskMoveTo.h" 00013 00014 using namespace std; 00015 00016 const float StateMachine::PERIOD = 0.01f; // period of task in [s] 00017 const float StateMachine::DISTANCE_THRESHOLD = 0.2f; // minimum allowed distance to obstacle in [m] 00018 const float StateMachine::TRANSLATIONAL_VELOCITY = 0.5f; // translational velocity in [m/s] 00019 const float StateMachine::ROTATIONAL_VELOCITY = 2.0f; // rotational velocity in [rad/s] 00020 00021 /** 00022 * Creates and initializes a state machine object. 00023 */ 00024 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) { 00025 00026 enableMotorDriver = 0; 00027 state = ROBOT_OFF; 00028 buttonNow = button; 00029 buttonBefore = buttonNow; 00030 00031 ticker.attach(callback(this, &StateMachine::run), PERIOD); 00032 } 00033 00034 /** 00035 * Deletes the state machine object and releases all allocated resources. 00036 */ 00037 StateMachine::~StateMachine() { 00038 00039 ticker.detach(); 00040 } 00041 00042 /** 00043 * Gets the actual state of this state machine. 00044 * @return the actual state as an int constant. 00045 */ 00046 int StateMachine::getState() { 00047 00048 return state; 00049 } 00050 00051 /** 00052 * This method is called periodically by the ticker object and implements the 00053 * logic of the state machine. 00054 */ 00055 void StateMachine::run() { 00056 00057 // set the leds based on distance measurements 00058 00059 led0 = irSensor0 < DISTANCE_THRESHOLD; 00060 led1 = irSensor1 < DISTANCE_THRESHOLD; 00061 led2 = irSensor2 < DISTANCE_THRESHOLD; 00062 led3 = irSensor3 < DISTANCE_THRESHOLD; 00063 led4 = irSensor4 < DISTANCE_THRESHOLD; 00064 led5 = irSensor5 < DISTANCE_THRESHOLD; 00065 00066 // implementation of the state machine 00067 00068 switch (state) { 00069 00070 case ROBOT_OFF: 00071 00072 buttonNow = button; 00073 00074 if (buttonNow && !buttonBefore) { // detect button rising edge 00075 00076 enableMotorDriver = 1; 00077 00078 taskList.push_back(new TaskMoveTo(controller, 0.0f, 1.0f, 0.0f)); 00079 taskList.push_back(new TaskWait(controller, 2.0f)); 00080 taskList.push_back(new TaskMoveTo(controller, 1.0f, 1.0f, -1.0f)); 00081 taskList.push_back(new TaskWait(controller, 2.0f)); 00082 taskList.push_back(new TaskMoveTo(controller, 1.0f, 0.0f, 0.0f)); 00083 taskList.push_back(new TaskWait(controller, 2.0f)); 00084 taskList.push_back(new TaskMoveTo(controller, 0.0f, 0.0f, 0.0f)); 00085 taskList.push_back(new TaskWait(controller, 2.0f)); 00086 00087 state = MOVE_FORWARD; 00088 } 00089 00090 buttonBefore = buttonNow; 00091 00092 break; 00093 00094 case MOVE_FORWARD: 00095 00096 buttonNow = button; 00097 00098 00099 00100 if (buttonNow && !buttonBefore) { // detect button rising edge 00101 00102 controller.setTranslationalVelocity(0.0f); 00103 controller.setRotationalVelocity(0.0f); 00104 00105 state = SLOWING_DOWN; 00106 00107 } else if ((irSensor3 < DISTANCE_THRESHOLD) || (irSensor4 < DISTANCE_THRESHOLD)) { 00108 00109 controller.setTranslationalVelocity(0.0f); 00110 controller.setRotationalVelocity(ROTATIONAL_VELOCITY); 00111 00112 state = TURN_LEFT; 00113 00114 } else if (irSensor2 < DISTANCE_THRESHOLD) { 00115 00116 controller.setTranslationalVelocity(0.0f); 00117 controller.setRotationalVelocity(-ROTATIONAL_VELOCITY); 00118 00119 state = TURN_RIGHT; 00120 00121 } else if (taskList.size() > 0) { // if statement hier unten, da kollision erkennung wichtiger 00122 00123 Task* task = taskList.front(); 00124 int result = task->run(PERIOD); 00125 00126 if (result == Task::DONE) { 00127 00128 taskList.pop_front(); 00129 delete task; 00130 00131 } else { 00132 00133 controller.setTranslationalVelocity(0.0f); //Sicher gehen, dass die letzen Geschw. auch wirklich 0 sind 00134 controller.setRotationalVelocity(0.0f); 00135 state = SLOWING_DOWN; 00136 00137 } 00138 } else { 00139 00140 } 00141 00142 buttonBefore = buttonNow; 00143 00144 break; 00145 00146 case TURN_LEFT: 00147 00148 buttonNow = button; 00149 00150 if (buttonNow && !buttonBefore) { // detect button rising edge 00151 00152 controller.setRotationalVelocity(0.0f); 00153 00154 state = SLOWING_DOWN; 00155 00156 } else if ((irSensor2 > DISTANCE_THRESHOLD) && (irSensor3 > DISTANCE_THRESHOLD) && (irSensor4 > DISTANCE_THRESHOLD)) { 00157 00158 controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY); 00159 controller.setRotationalVelocity(0.0f); 00160 00161 state = MOVE_FORWARD; 00162 } 00163 00164 buttonBefore = buttonNow; 00165 00166 break; 00167 00168 case TURN_RIGHT: 00169 00170 buttonNow = button; 00171 00172 if (buttonNow && !buttonBefore) { // detect button rising edge 00173 00174 controller.setRotationalVelocity(0.0f); 00175 00176 state = SLOWING_DOWN; 00177 00178 } else if ((irSensor2 > DISTANCE_THRESHOLD) && (irSensor3 > DISTANCE_THRESHOLD) && (irSensor4 > DISTANCE_THRESHOLD)) { 00179 00180 controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY); 00181 controller.setRotationalVelocity(0.0f); 00182 00183 state = MOVE_FORWARD; 00184 } 00185 00186 buttonBefore = buttonNow; 00187 00188 break; 00189 00190 case SLOWING_DOWN: 00191 00192 if ((fabs(controller.getActualTranslationalVelocity()) < 0.01f) && (fabs(controller.getActualRotationalVelocity()) < 0.01f)) { 00193 00194 while (taskList.size() > 0) { 00195 delete taskList.front(); 00196 taskList.pop_front(); 00197 } 00198 00199 enableMotorDriver = 0; 00200 00201 state = ROBOT_OFF; 00202 } 00203 00204 break; 00205 00206 default: 00207 00208 state = ROBOT_OFF; 00209 } 00210 } 00211
Generated on Wed Jul 13 2022 16:04:31 by
1.7.2