vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Sources/Robot.cpp@16:f157e5ccd7d3, 2017-05-15 (annotated)
- Committer:
- beacon
- Date:
- Mon May 15 17:35:24 2017 +0000
- Revision:
- 16:f157e5ccd7d3
- Parent:
- 15:915f8839fe48
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
beacon | 0:d267b248eff4 | 1 | #include "Robot.h" |
beacon | 4:67d7177c213f | 2 | #include "Declarations.h" |
beacon | 0:d267b248eff4 | 3 | |
beacon | 0:d267b248eff4 | 4 | /* Work in progress -------------------------------------------- */ |
beacon | 0:d267b248eff4 | 5 | |
EpicG10 | 15:915f8839fe48 | 6 | Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste ) |
beacon | 0:d267b248eff4 | 7 | { |
beacon | 1:388c915756f5 | 8 | this->left = left; |
beacon | 1:388c915756f5 | 9 | this->right = right; |
beacon | 10:f76476943a6c | 10 | this->powerSignal = powerSignal; |
beacon | 10:f76476943a6c | 11 | //this->errorMotor = errorMotor; |
beacon | 0:d267b248eff4 | 12 | |
beacon | 0:d267b248eff4 | 13 | this->left->period(0.00005f); |
beacon | 0:d267b248eff4 | 14 | this->right->period(0.00005f); |
beacon | 1:388c915756f5 | 15 | |
beacon | 1:388c915756f5 | 16 | this->leds = leds; |
beacon | 1:388c915756f5 | 17 | |
beacon | 1:388c915756f5 | 18 | this->FarbVoltage = FarbVoltage; |
beacon | 4:67d7177c213f | 19 | this->frontS = frontS; |
beacon | 4:67d7177c213f | 20 | this->leftS = leftS; |
beacon | 4:67d7177c213f | 21 | this->rightS = rightS; |
beacon | 4:67d7177c213f | 22 | |
beacon | 6:ba26dd3251b3 | 23 | this->Arm = Arm; |
beacon | 10:f76476943a6c | 24 | this->Greifer = Greifer; |
EpicG10 | 15:915f8839fe48 | 25 | this->Leiste = Leiste; |
beacon | 1:388c915756f5 | 26 | |
beacon | 0:d267b248eff4 | 27 | } |
beacon | 0:d267b248eff4 | 28 | |
beacon | 4:67d7177c213f | 29 | //Drive functions |
beacon | 0:d267b248eff4 | 30 | void Robot::drive() |
beacon | 0:d267b248eff4 | 31 | { |
beacon | 0:d267b248eff4 | 32 | //pwm determine what direction it goes. |
beacon | 0:d267b248eff4 | 33 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 34 | *left= 0.6f; |
beacon | 1:388c915756f5 | 35 | *right= 0.4f; |
beacon | 1:388c915756f5 | 36 | } |
beacon | 1:388c915756f5 | 37 | |
beacon | 4:67d7177c213f | 38 | void Robot::driveB() |
beacon | 4:67d7177c213f | 39 | { |
beacon | 4:67d7177c213f | 40 | //pwm determine what direction it goes. |
beacon | 4:67d7177c213f | 41 | *powerSignal = 1; |
beacon | 4:67d7177c213f | 42 | *left= 0.4f; |
beacon | 4:67d7177c213f | 43 | *right= 0.6f; |
beacon | 4:67d7177c213f | 44 | } |
beacon | 4:67d7177c213f | 45 | |
beacon | 1:388c915756f5 | 46 | void Robot::turnLeft() |
beacon | 1:388c915756f5 | 47 | { |
beacon | 1:388c915756f5 | 48 | |
beacon | 1:388c915756f5 | 49 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 50 | *left= 0.4f; |
beacon | 1:388c915756f5 | 51 | *right= 0.4f; |
beacon | 1:388c915756f5 | 52 | |
beacon | 0:d267b248eff4 | 53 | } |
beacon | 0:d267b248eff4 | 54 | |
beacon | 4:67d7177c213f | 55 | void Robot::turnLeftS() |
beacon | 4:67d7177c213f | 56 | { |
beacon | 4:67d7177c213f | 57 | |
beacon | 4:67d7177c213f | 58 | *powerSignal = 1; |
beacon | 4:67d7177c213f | 59 | *left= 0.45f; |
beacon | 4:67d7177c213f | 60 | *right= 0.45f; |
beacon | 4:67d7177c213f | 61 | |
beacon | 4:67d7177c213f | 62 | } |
beacon | 4:67d7177c213f | 63 | |
beacon | 1:388c915756f5 | 64 | void Robot::turnRight() |
beacon | 1:388c915756f5 | 65 | { |
beacon | 1:388c915756f5 | 66 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 67 | *left= 0.6f; |
beacon | 1:388c915756f5 | 68 | *right= 0.6f; |
beacon | 1:388c915756f5 | 69 | } |
beacon | 1:388c915756f5 | 70 | |
beacon | 4:67d7177c213f | 71 | void Robot::turnRightS() |
beacon | 4:67d7177c213f | 72 | { |
beacon | 4:67d7177c213f | 73 | |
beacon | 4:67d7177c213f | 74 | *powerSignal = 1; |
beacon | 4:67d7177c213f | 75 | *left= 0.55f; |
beacon | 4:67d7177c213f | 76 | *right= 0.55f; |
beacon | 4:67d7177c213f | 77 | |
beacon | 4:67d7177c213f | 78 | } |
beacon | 4:67d7177c213f | 79 | |
beacon | 1:388c915756f5 | 80 | void Robot::turnAround(int left) |
beacon | 1:388c915756f5 | 81 | { |
beacon | 0:d267b248eff4 | 82 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 83 | |
beacon | 1:388c915756f5 | 84 | if (left) { |
beacon | 1:388c915756f5 | 85 | turnLeft(); |
beacon | 1:388c915756f5 | 86 | } |
beacon | 1:388c915756f5 | 87 | |
beacon | 1:388c915756f5 | 88 | else { |
beacon | 1:388c915756f5 | 89 | turnRight(); |
beacon | 1:388c915756f5 | 90 | } |
beacon | 1:388c915756f5 | 91 | } |
beacon | 1:388c915756f5 | 92 | |
beacon | 1:388c915756f5 | 93 | void Robot::stop() |
beacon | 1:388c915756f5 | 94 | { |
beacon | 1:388c915756f5 | 95 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 96 | *left= 0.5f; |
beacon | 1:388c915756f5 | 97 | *right= 0.5f; |
beacon | 0:d267b248eff4 | 98 | } |
beacon | 0:d267b248eff4 | 99 | |
beacon | 16:f157e5ccd7d3 | 100 | void Robot::driveSlowly(){ |
beacon | 16:f157e5ccd7d3 | 101 | static int i = 0; |
beacon | 16:f157e5ccd7d3 | 102 | i++; |
beacon | 16:f157e5ccd7d3 | 103 | if( i % 2 ){ |
beacon | 16:f157e5ccd7d3 | 104 | this->drive(); |
beacon | 16:f157e5ccd7d3 | 105 | } |
beacon | 16:f157e5ccd7d3 | 106 | else{ |
beacon | 16:f157e5ccd7d3 | 107 | this->stop(); |
beacon | 16:f157e5ccd7d3 | 108 | } |
beacon | 16:f157e5ccd7d3 | 109 | } |
beacon | 16:f157e5ccd7d3 | 110 | |
beacon | 16:f157e5ccd7d3 | 111 | void Robot::driveBackSlowly(){ |
beacon | 16:f157e5ccd7d3 | 112 | static int i = 0; |
beacon | 16:f157e5ccd7d3 | 113 | i++; |
beacon | 16:f157e5ccd7d3 | 114 | if( i % 2 ){ |
beacon | 16:f157e5ccd7d3 | 115 | this->driveB(); |
beacon | 16:f157e5ccd7d3 | 116 | } |
beacon | 16:f157e5ccd7d3 | 117 | else{ |
beacon | 16:f157e5ccd7d3 | 118 | this->stop(); |
beacon | 16:f157e5ccd7d3 | 119 | } |
beacon | 16:f157e5ccd7d3 | 120 | } |
beacon | 16:f157e5ccd7d3 | 121 | |
beacon | 4:67d7177c213f | 122 | |
beacon | 4:67d7177c213f | 123 | //Functions that use the drive functions |
beacon | 4:67d7177c213f | 124 | void Robot::counterMax(int* counter, int* timer, int* lastAct, int* rando){ |
beacon | 4:67d7177c213f | 125 | if (*lastAct != 0){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 126 | *timer = 0; |
beacon | 4:67d7177c213f | 127 | *lastAct = 0; |
beacon | 4:67d7177c213f | 128 | } |
beacon | 4:67d7177c213f | 129 | |
beacon | 4:67d7177c213f | 130 | if (*rando == -1){ //If rando was unused, set a new number. |
beacon | 4:67d7177c213f | 131 | *rando = rand() % 2; |
beacon | 4:67d7177c213f | 132 | } |
beacon | 4:67d7177c213f | 133 | |
beacon | 4:67d7177c213f | 134 | if (this->sensors[FWD] < NEAR){ //While something is seen turn around. |
beacon | 4:67d7177c213f | 135 | this->turnAround(*rando); |
beacon | 4:67d7177c213f | 136 | } |
beacon | 4:67d7177c213f | 137 | |
beacon | 4:67d7177c213f | 138 | else{ |
beacon | 4:67d7177c213f | 139 | *rando = -1; |
beacon | 4:67d7177c213f | 140 | *counter = 0; |
beacon | 4:67d7177c213f | 141 | } |
beacon | 4:67d7177c213f | 142 | } |
beacon | 4:67d7177c213f | 143 | |
beacon | 10:f76476943a6c | 144 | void Robot::wallRight(int* counter, int* timer, int* lastAct){ |
beacon | 10:f76476943a6c | 145 | *counter += 1; |
beacon | 10:f76476943a6c | 146 | |
beacon | 10:f76476943a6c | 147 | if (*lastAct != 1){ //If this wasn't the last called action, reset the timer. |
beacon | 10:f76476943a6c | 148 | *timer = 0; |
beacon | 10:f76476943a6c | 149 | *lastAct = 1; |
beacon | 10:f76476943a6c | 150 | } |
beacon | 10:f76476943a6c | 151 | |
beacon | 10:f76476943a6c | 152 | this->turnLeft(); |
beacon | 10:f76476943a6c | 153 | } |
beacon | 10:f76476943a6c | 154 | |
beacon | 4:67d7177c213f | 155 | void Robot::wallLeft(int* counter, int* timer, int* lastAct){ |
beacon | 4:67d7177c213f | 156 | *counter += 1; |
beacon | 4:67d7177c213f | 157 | |
beacon | 4:67d7177c213f | 158 | if (*lastAct != 2){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 159 | *timer = 0; |
beacon | 4:67d7177c213f | 160 | *lastAct = 2; |
beacon | 4:67d7177c213f | 161 | } |
beacon | 4:67d7177c213f | 162 | |
beacon | 4:67d7177c213f | 163 | this->turnRight(); |
beacon | 4:67d7177c213f | 164 | } |
beacon | 4:67d7177c213f | 165 | |
beacon | 4:67d7177c213f | 166 | void Robot::wallFront(int* counter, int* timer, int* lastAct){ |
beacon | 4:67d7177c213f | 167 | if (*lastAct != 3){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 168 | *timer = 0; |
beacon | 4:67d7177c213f | 169 | *lastAct = 3; |
beacon | 4:67d7177c213f | 170 | } |
beacon | 4:67d7177c213f | 171 | |
beacon | 4:67d7177c213f | 172 | *counter = MAX; //By setting the counter to MAX, next time it will go into the first if-statement (action 0). |
beacon | 4:67d7177c213f | 173 | } |
beacon | 4:67d7177c213f | 174 | |
beacon | 4:67d7177c213f | 175 | |
beacon | 5:1aaf5de776ff | 176 | void Robot::legoFront(int* counter, int* timer, int* lastAct, int* legoFound, int* found){ |
beacon | 4:67d7177c213f | 177 | //*counter += 1; |
beacon | 4:67d7177c213f | 178 | *legoFound = 0; |
beacon | 4:67d7177c213f | 179 | |
beacon | 5:1aaf5de776ff | 180 | if (*lastAct != 4){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 181 | *timer = 0; |
beacon | 4:67d7177c213f | 182 | *lastAct = 4; |
beacon | 4:67d7177c213f | 183 | } |
beacon | 4:67d7177c213f | 184 | |
beacon | 5:1aaf5de776ff | 185 | if (this->sensors[FWD] < NEAR){ //If Sam sees a wall turn around |
beacon | 4:67d7177c213f | 186 | *legoFound = -1; |
beacon | 5:1aaf5de776ff | 187 | *counter = MAX; //setting counter to MAX will couse sam to turnAround |
beacon | 4:67d7177c213f | 188 | } |
beacon | 4:67d7177c213f | 189 | |
beacon | 5:1aaf5de776ff | 190 | if (this->sensors[FWD_L] > 0.16f){ |
beacon | 4:67d7177c213f | 191 | this->drive(); |
beacon | 4:67d7177c213f | 192 | } |
beacon | 4:67d7177c213f | 193 | else{ |
beacon | 11:292bdbd85a9c | 194 | this->stop(); |
beacon | 5:1aaf5de776ff | 195 | *found = 1; |
beacon | 4:67d7177c213f | 196 | } |
beacon | 4:67d7177c213f | 197 | } |
beacon | 4:67d7177c213f | 198 | |
beacon | 4:67d7177c213f | 199 | void Robot::legoRight(int* counter, int* timer, int* lastAct, int* legoFound){ |
beacon | 4:67d7177c213f | 200 | //*counter += 1; |
beacon | 4:67d7177c213f | 201 | *legoFound = 1; |
beacon | 4:67d7177c213f | 202 | |
beacon | 4:67d7177c213f | 203 | if (*lastAct != 5){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 204 | *timer = 0; |
beacon | 4:67d7177c213f | 205 | *lastAct = 5; |
beacon | 4:67d7177c213f | 206 | } |
beacon | 4:67d7177c213f | 207 | |
beacon | 5:1aaf5de776ff | 208 | if (this->sensors[FWD_L] > 0.22f){ |
beacon | 4:67d7177c213f | 209 | this->turnRight(); |
beacon | 4:67d7177c213f | 210 | } |
beacon | 4:67d7177c213f | 211 | else{ |
beacon | 4:67d7177c213f | 212 | this->stop(); |
beacon | 5:1aaf5de776ff | 213 | *legoFound = -1; |
beacon | 4:67d7177c213f | 214 | } |
beacon | 4:67d7177c213f | 215 | } |
beacon | 4:67d7177c213f | 216 | |
beacon | 4:67d7177c213f | 217 | void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){ |
beacon | 4:67d7177c213f | 218 | //*counter += 1; |
beacon | 4:67d7177c213f | 219 | *legoFound = 2; |
beacon | 0:d267b248eff4 | 220 | |
beacon | 4:67d7177c213f | 221 | if (*lastAct != 6){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 222 | *timer = 0; |
beacon | 4:67d7177c213f | 223 | *lastAct = 6; |
beacon | 4:67d7177c213f | 224 | } |
beacon | 4:67d7177c213f | 225 | |
beacon | 5:1aaf5de776ff | 226 | if (this->sensors[FWD_L] > 0.22f){ |
beacon | 4:67d7177c213f | 227 | this->turnLeft(); |
beacon | 4:67d7177c213f | 228 | } |
beacon | 4:67d7177c213f | 229 | else{ |
beacon | 4:67d7177c213f | 230 | this->stop(); |
beacon | 5:1aaf5de776ff | 231 | *legoFound = -1; |
beacon | 4:67d7177c213f | 232 | } |
beacon | 4:67d7177c213f | 233 | } |
beacon | 4:67d7177c213f | 234 | |
beacon | 4:67d7177c213f | 235 | |
beacon | 4:67d7177c213f | 236 | void Robot::nothingFound(int* counter, int* timer, int* lastAct){ |
beacon | 4:67d7177c213f | 237 | *counter = 0; |
beacon | 4:67d7177c213f | 238 | if (*lastAct != 7){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 239 | *timer = 0; |
beacon | 4:67d7177c213f | 240 | *lastAct = 7; |
beacon | 4:67d7177c213f | 241 | } |
beacon | 4:67d7177c213f | 242 | |
beacon | 4:67d7177c213f | 243 | this->drive(); |
beacon | 4:67d7177c213f | 244 | } |
beacon | 4:67d7177c213f | 245 | |
beacon | 4:67d7177c213f | 246 | |
beacon | 11:292bdbd85a9c | 247 | int Robot::search(int* timer){ |
EpicG10 | 13:6c1f8e35b242 | 248 | //return 1; |
beacon | 11:292bdbd85a9c | 249 | enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR }; |
beacon | 4:67d7177c213f | 250 | |
beacon | 11:292bdbd85a9c | 251 | static int state = neutral; |
beacon | 4:67d7177c213f | 252 | |
beacon | 11:292bdbd85a9c | 253 | static int counter = 0; //counter is used blabla |
beacon | 4:67d7177c213f | 254 | |
beacon | 11:292bdbd85a9c | 255 | static int rando = -1; |
beacon | 10:f76476943a6c | 256 | |
beacon | 11:292bdbd85a9c | 257 | //static int lastAct = 0; |
beacon | 4:67d7177c213f | 258 | |
beacon | 16:f157e5ccd7d3 | 259 | this->sensors[FWD] < NEAR ? this->leds[4] = 1 : this->leds[4] = 0; |
beacon | 16:f157e5ccd7d3 | 260 | this->sensors[RIGHT] < NEAR ? this->leds[RIGHT_L] = 1 : this->leds[RIGHT_L] = 0; |
beacon | 16:f157e5ccd7d3 | 261 | this->sensors[LEFT] < NEAR ? this->leds[LEFT_L] = 1 : this->leds[LEFT_L] = 0; |
beacon | 16:f157e5ccd7d3 | 262 | |
beacon | 11:292bdbd85a9c | 263 | switch( state ){ |
beacon | 11:292bdbd85a9c | 264 | case neutral: |
beacon | 11:292bdbd85a9c | 265 | if( counter > MAX ){ |
beacon | 11:292bdbd85a9c | 266 | state = max; |
beacon | 11:292bdbd85a9c | 267 | } |
EpicG10 | 13:6c1f8e35b242 | 268 | else if( this->sensors[FWD] < NEAR ){ |
beacon | 11:292bdbd85a9c | 269 | state = wallF; |
EpicG10 | 13:6c1f8e35b242 | 270 | } |
beacon | 11:292bdbd85a9c | 271 | else if( this->sensors[LEFT] < NEAR ){ |
beacon | 11:292bdbd85a9c | 272 | state = wallL; |
beacon | 11:292bdbd85a9c | 273 | } |
beacon | 11:292bdbd85a9c | 274 | else if( this->sensors[RIGHT] < NEAR ){ |
beacon | 11:292bdbd85a9c | 275 | state = wallR; |
beacon | 11:292bdbd85a9c | 276 | } |
beacon | 16:f157e5ccd7d3 | 277 | else if( this->sensors[FWD_L] < NEAR_LEGO+0.03f ){ |
beacon | 11:292bdbd85a9c | 278 | state = legoF; |
beacon | 11:292bdbd85a9c | 279 | } |
beacon | 16:f157e5ccd7d3 | 280 | else if( this->sensors[LEFT_L] < NEAR_LEGO ){ |
beacon | 11:292bdbd85a9c | 281 | state = legoL; |
beacon | 11:292bdbd85a9c | 282 | } |
beacon | 16:f157e5ccd7d3 | 283 | else if( this->sensors[RIGHT_L] < NEAR_LEGO ){ |
beacon | 11:292bdbd85a9c | 284 | state = legoR; |
beacon | 11:292bdbd85a9c | 285 | } |
beacon | 11:292bdbd85a9c | 286 | else{ |
beacon | 11:292bdbd85a9c | 287 | this->drive(); |
beacon | 11:292bdbd85a9c | 288 | counter = 0; |
beacon | 11:292bdbd85a9c | 289 | } |
beacon | 11:292bdbd85a9c | 290 | break; |
beacon | 11:292bdbd85a9c | 291 | |
beacon | 11:292bdbd85a9c | 292 | case max: { |
beacon | 11:292bdbd85a9c | 293 | int time = 0; |
beacon | 11:292bdbd85a9c | 294 | if( time < 15 && this->sensors[FWD] > NEAR ){ |
beacon | 11:292bdbd85a9c | 295 | rando == -1 ? rando = rand() % 2 : rando = rando; |
beacon | 11:292bdbd85a9c | 296 | this->turnAround(rando); |
beacon | 11:292bdbd85a9c | 297 | } |
beacon | 11:292bdbd85a9c | 298 | else{ |
beacon | 11:292bdbd85a9c | 299 | state = neutral; |
beacon | 11:292bdbd85a9c | 300 | counter = 0; |
beacon | 11:292bdbd85a9c | 301 | rando = -1; |
beacon | 11:292bdbd85a9c | 302 | } |
beacon | 11:292bdbd85a9c | 303 | break; |
beacon | 11:292bdbd85a9c | 304 | } |
beacon | 11:292bdbd85a9c | 305 | |
beacon | 11:292bdbd85a9c | 306 | case wallF: |
beacon | 11:292bdbd85a9c | 307 | counter++; |
beacon | 11:292bdbd85a9c | 308 | if( this->sensors[FWD] < NEAR ){ |
beacon | 11:292bdbd85a9c | 309 | rando == -1 ? rando = rand() % 2 : rando = rando; |
beacon | 11:292bdbd85a9c | 310 | this->turnAround(rando); |
beacon | 11:292bdbd85a9c | 311 | } |
beacon | 11:292bdbd85a9c | 312 | else{ |
beacon | 11:292bdbd85a9c | 313 | state = neutral; |
beacon | 11:292bdbd85a9c | 314 | rando = -1; |
beacon | 11:292bdbd85a9c | 315 | } |
beacon | 11:292bdbd85a9c | 316 | break; |
beacon | 11:292bdbd85a9c | 317 | |
beacon | 11:292bdbd85a9c | 318 | case wallL: |
beacon | 11:292bdbd85a9c | 319 | counter++; |
beacon | 11:292bdbd85a9c | 320 | if( this->sensors[LEFT] < NEAR ){ |
beacon | 11:292bdbd85a9c | 321 | this->turnRight(); |
beacon | 11:292bdbd85a9c | 322 | } |
beacon | 11:292bdbd85a9c | 323 | else{ |
beacon | 11:292bdbd85a9c | 324 | state = neutral; |
beacon | 11:292bdbd85a9c | 325 | } |
beacon | 11:292bdbd85a9c | 326 | break; |
beacon | 11:292bdbd85a9c | 327 | |
beacon | 11:292bdbd85a9c | 328 | case wallR: |
beacon | 11:292bdbd85a9c | 329 | counter++; |
beacon | 11:292bdbd85a9c | 330 | if( this->sensors[RIGHT] < NEAR ){ |
beacon | 11:292bdbd85a9c | 331 | this->turnLeft(); |
beacon | 11:292bdbd85a9c | 332 | } |
beacon | 11:292bdbd85a9c | 333 | else{ |
beacon | 11:292bdbd85a9c | 334 | state = neutral; |
beacon | 11:292bdbd85a9c | 335 | } |
beacon | 11:292bdbd85a9c | 336 | break; |
beacon | 11:292bdbd85a9c | 337 | |
beacon | 11:292bdbd85a9c | 338 | case legoF: |
beacon | 11:292bdbd85a9c | 339 | //counter++; |
beacon | 16:f157e5ccd7d3 | 340 | if( this->sensors[FWD] < NEAR ){ |
beacon | 16:f157e5ccd7d3 | 341 | state = wallF; |
beacon | 16:f157e5ccd7d3 | 342 | } |
beacon | 16:f157e5ccd7d3 | 343 | else if( this->sensors[FWD_L] > 0.17f ){ |
beacon | 16:f157e5ccd7d3 | 344 | this->driveSlowly(); |
beacon | 11:292bdbd85a9c | 345 | } |
beacon | 11:292bdbd85a9c | 346 | else{ |
beacon | 11:292bdbd85a9c | 347 | state = neutral; |
beacon | 11:292bdbd85a9c | 348 | counter = 0; |
beacon | 11:292bdbd85a9c | 349 | this->stop(); |
beacon | 11:292bdbd85a9c | 350 | return 1; |
beacon | 11:292bdbd85a9c | 351 | } |
beacon | 11:292bdbd85a9c | 352 | break; |
beacon | 11:292bdbd85a9c | 353 | |
beacon | 11:292bdbd85a9c | 354 | case legoL: |
beacon | 11:292bdbd85a9c | 355 | counter++; |
EpicG10 | 13:6c1f8e35b242 | 356 | if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){ |
beacon | 11:292bdbd85a9c | 357 | this->turnLeft(); |
beacon | 11:292bdbd85a9c | 358 | } |
beacon | 11:292bdbd85a9c | 359 | else{ |
beacon | 11:292bdbd85a9c | 360 | state = legoF; |
beacon | 11:292bdbd85a9c | 361 | } |
beacon | 11:292bdbd85a9c | 362 | break; |
beacon | 11:292bdbd85a9c | 363 | |
beacon | 11:292bdbd85a9c | 364 | case legoR: |
beacon | 11:292bdbd85a9c | 365 | counter++; |
EpicG10 | 13:6c1f8e35b242 | 366 | if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){ |
beacon | 11:292bdbd85a9c | 367 | this->turnRight(); |
beacon | 11:292bdbd85a9c | 368 | } |
beacon | 11:292bdbd85a9c | 369 | else{ |
beacon | 11:292bdbd85a9c | 370 | state = legoF; |
beacon | 11:292bdbd85a9c | 371 | } |
beacon | 11:292bdbd85a9c | 372 | break; |
beacon | 11:292bdbd85a9c | 373 | |
beacon | 4:67d7177c213f | 374 | } |
beacon | 11:292bdbd85a9c | 375 | return 0; |
beacon | 4:67d7177c213f | 376 | } |
beacon | 4:67d7177c213f | 377 | |
beacon | 10:f76476943a6c | 378 | int Robot::getErrorMotor(){ |
beacon | 10:f76476943a6c | 379 | return 0; //errorMotor; |
beacon | 0:d267b248eff4 | 380 | } |
beacon | 0:d267b248eff4 | 381 | |
beacon | 11:292bdbd85a9c | 382 | /*int Robot::searchh(int* timer){ |
beacon | 11:292bdbd85a9c | 383 | enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR }; |
beacon | 11:292bdbd85a9c | 384 | |
beacon | 11:292bdbd85a9c | 385 | static int state = neutral; |
beacon | 11:292bdbd85a9c | 386 | |
beacon | 11:292bdbd85a9c | 387 | static int counter = 0; //counter is used blabla |
beacon | 11:292bdbd85a9c | 388 | |
beacon | 11:292bdbd85a9c | 389 | //static int lastAct = 0; |
beacon | 11:292bdbd85a9c | 390 | |
beacon | 11:292bdbd85a9c | 391 | switch( state ){ |
beacon | 11:292bdbd85a9c | 392 | case neutral: |
beacon | 11:292bdbd85a9c | 393 | if( counter > MAX ){ |
beacon | 11:292bdbd85a9c | 394 | state = max; |
beacon | 11:292bdbd85a9c | 395 | } |
beacon | 11:292bdbd85a9c | 396 | else if( this->sensora[FWD] < NEAR ){ |
beacon | 11:292bdbd85a9c | 397 | state = wallF; |
beacon | 11:292bdbd85a9c | 398 | counter = 0; |
beacon | 11:292bdbd85a9c | 399 | } |
beacon | 11:292bdbd85a9c | 400 | else if( this->sensors[LEFT] < NEAR ){ |
beacon | 11:292bdbd85a9c | 401 | state = wallL; |
beacon | 11:292bdbd85a9c | 402 | counter = 0; |
beacon | 11:292bdbd85a9c | 403 | } |
beacon | 11:292bdbd85a9c | 404 | else if( this->sensors[RIGHT] < NEAR ){ |
beacon | 11:292bdbd85a9c | 405 | state = wallR; |
beacon | 11:292bdbd85a9c | 406 | counter = 0; |
beacon | 11:292bdbd85a9c | 407 | } |
beacon | 11:292bdbd85a9c | 408 | else if( this->sensors[FWD_L] < NEAR ){ |
beacon | 11:292bdbd85a9c | 409 | state = legoF; |
beacon | 11:292bdbd85a9c | 410 | counter = 0; |
beacon | 11:292bdbd85a9c | 411 | } |
beacon | 11:292bdbd85a9c | 412 | else if( this->sensors[LEFT_L] < NEAR ){ |
beacon | 11:292bdbd85a9c | 413 | state = legoL; |
beacon | 11:292bdbd85a9c | 414 | counter = 0; |
beacon | 11:292bdbd85a9c | 415 | } |
beacon | 11:292bdbd85a9c | 416 | else if( this->sensors[RIGHT_L] < NEAR ){ |
beacon | 11:292bdbd85a9c | 417 | state = legoR; |
beacon | 11:292bdbd85a9c | 418 | counter = 0; |
beacon | 11:292bdbd85a9c | 419 | } |
beacon | 11:292bdbd85a9c | 420 | break; |
beacon | 11:292bdbd85a9c | 421 | |
beacon | 11:292bdbd85a9c | 422 | case max: |
beacon | 11:292bdbd85a9c | 423 | |
beacon | 11:292bdbd85a9c | 424 | break; |
beacon | 11:292bdbd85a9c | 425 | */ |
beacon | 11:292bdbd85a9c | 426 | |
beacon | 0:d267b248eff4 | 427 | //reset |
beacon | 0:d267b248eff4 | 428 | //} |
beacon | 0:d267b248eff4 | 429 | |
beacon | 0:d267b248eff4 | 430 | //Add management for Overpower!! Pin PB_15 |