vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Diff: Sources/Robot.cpp
- Revision:
- 16:f157e5ccd7d3
- Parent:
- 15:915f8839fe48
diff -r 915f8839fe48 -r f157e5ccd7d3 Sources/Robot.cpp --- a/Sources/Robot.cpp Thu May 11 18:57:45 2017 +0000 +++ b/Sources/Robot.cpp Mon May 15 17:35:24 2017 +0000 @@ -97,6 +97,28 @@ *right= 0.5f; } +void Robot::driveSlowly(){ + static int i = 0; + i++; + if( i % 2 ){ + this->drive(); + } + else{ + this->stop(); + } +} + +void Robot::driveBackSlowly(){ + static int i = 0; + i++; + if( i % 2 ){ + this->driveB(); + } + else{ + this->stop(); + } +} + //Functions that use the drive functions void Robot::counterMax(int* counter, int* timer, int* lastAct, int* rando){ @@ -234,6 +256,10 @@ //static int lastAct = 0; + this->sensors[FWD] < NEAR ? this->leds[4] = 1 : this->leds[4] = 0; + this->sensors[RIGHT] < NEAR ? this->leds[RIGHT_L] = 1 : this->leds[RIGHT_L] = 0; + this->sensors[LEFT] < NEAR ? this->leds[LEFT_L] = 1 : this->leds[LEFT_L] = 0; + switch( state ){ case neutral: if( counter > MAX ){ @@ -248,13 +274,13 @@ else if( this->sensors[RIGHT] < NEAR ){ state = wallR; } - else if( this->sensors[FWD_L] < NEAR ){ + else if( this->sensors[FWD_L] < NEAR_LEGO+0.03f ){ state = legoF; } - else if( this->sensors[LEFT_L] < NEAR ){ + else if( this->sensors[LEFT_L] < NEAR_LEGO ){ state = legoL; } - else if( this->sensors[RIGHT_L] < NEAR ){ + else if( this->sensors[RIGHT_L] < NEAR_LEGO ){ state = legoR; } else{ @@ -311,8 +337,11 @@ case legoF: //counter++; - if( this->sensors[FWD_L] > 0.17f ){ - this->drive(); + if( this->sensors[FWD] < NEAR ){ + state = wallF; + } + else if( this->sensors[FWD_L] > 0.17f ){ + this->driveSlowly(); } else{ state = neutral;