zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official-US

a

Dependencies:   Servo ServoArm mbed

Fork of PES_Official-TestF by zhaw_st16b_pes2_10

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?

UserRevisionLine numberNew 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