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@18:a158713a0049, 2017-05-19 (annotated)

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