vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Diff: Sources/Robot.cpp
- Revision:
- 17:4e1be70bdedb
- Parent:
- 15:915f8839fe48
diff -r 915f8839fe48 -r 4e1be70bdedb Sources/Robot.cpp --- a/Sources/Robot.cpp Thu May 11 18:57:45 2017 +0000 +++ b/Sources/Robot.cpp Thu May 18 14:27:09 2017 +0000 @@ -3,7 +3,7 @@ /* Work in progress -------------------------------------------- */ -Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste ) +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 ) { this->left = left; this->right = right; @@ -23,6 +23,7 @@ this->Arm = Arm; this->Greifer = Greifer; this->Leiste = Leiste; + this->USsensor = USsensor; } @@ -97,7 +98,29 @@ *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){ if (*lastAct != 0){ //If this wasn't the last called action, reset the timer. @@ -166,7 +189,7 @@ } if (this->sensors[FWD_L] > 0.16f){ - this->drive(); + this->driveSlowly(); } else{ this->stop(); @@ -219,42 +242,50 @@ } this->drive(); -} +}*/ int Robot::search(int* timer){ - //return 1; enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR }; static int state = neutral; - static int counter = 0; //counter is used blabla + static int counter = 0; //counter is used blabla + + static int rando = -1; + + static int lastAct = 0; - static int rando = -1; + //static int stay = -1; //Stay is used to remain in a certain state + /* + this->sensors[FWD_L] < NEAR ? this->leds[4] = 1 : this->leds[4] = 0; + this->sensors[RIGHT_L] < NEAR ? this->leds[RIGHT_L] = 1 : this->leds[RIGHT_L] = 0; + this->sensors[LEFT_L] < NEAR ? this->leds[LEFT_L] = 1 : this->leds[LEFT_L] = 0; + */ - //static int lastAct = 0; + //printf("\n\rcurrent robot state: %d", state); switch( state ){ case neutral: if( counter > MAX ){ state = max; } - else if( this->sensors[FWD] < NEAR ){ + else if( this->see(FWD) < NEAR ){ state = wallF; } - else if( this->sensors[LEFT] < NEAR ){ + else if( this->see(LEFT) < NEAR ){ state = wallL; } - else if( this->sensors[RIGHT] < NEAR ){ + else if( this->see(RIGHT) < NEAR ){ state = wallR; } - else if( this->sensors[FWD_L] < NEAR ){ + else if( this->see(FWD_L) < NEAR_LEGO + 0.015f ){ state = legoF; } - else if( this->sensors[LEFT_L] < NEAR ){ + else if( this->see(LEFT_L) < NEAR_LEGO ){ state = legoL; } - else if( this->sensors[RIGHT_L] < NEAR ){ + else if( this->see(RIGHT_L) < NEAR_LEGO ){ state = legoR; } else{ @@ -265,7 +296,7 @@ case max: { int time = 0; - if( time < 15 && this->sensors[FWD] > NEAR ){ + if( time < 15 && this->see(FWD) > NEAR ){ rando == -1 ? rando = rand() % 2 : rando = rando; this->turnAround(rando); } @@ -279,7 +310,7 @@ case wallF: counter++; - if( this->sensors[FWD] < NEAR ){ + if( this->see(FWD) < NEAR ){ rando == -1 ? rando = rand() % 2 : rando = rando; this->turnAround(rando); } @@ -291,7 +322,7 @@ case wallL: counter++; - if( this->sensors[LEFT] < NEAR ){ + if( this->see(LEFT) < NEAR ){ this->turnRight(); } else{ @@ -301,7 +332,7 @@ case wallR: counter++; - if( this->sensors[RIGHT] < NEAR ){ + if( this->see(RIGHT) < NEAR ){ this->turnLeft(); } else{ @@ -311,11 +342,16 @@ case legoF: //counter++; - if( this->sensors[FWD_L] > 0.17f ){ - this->drive(); + if( this->see(FWD) < NEAR ){ + state = wallF; + //stay = -1; + } + else if( this->see(FWD_L) < 0.19f ){ + this->driveBackSlowly(); } else{ state = neutral; + //stay = -1; counter = 0; this->stop(); return 1; @@ -324,21 +360,25 @@ case legoL: counter++; - if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){ + if( this->see(FWD_L) > NEAR_LEGO + 0.05f ){ this->turnLeft(); } else{ - state = legoF; + state = neutral; + this->drive(); + //stay = 1; } break; case legoR: counter++; - if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){ + if( this->see(FWD_L) > NEAR_LEGO + 0.05f ){ this->turnRight(); } else{ - state = legoF; + this->drive(); + state = neutral; + //stay = 1; } break; @@ -346,56 +386,15 @@ return 0; } -int Robot::getErrorMotor(){ - return 0; //errorMotor; +float Robot::see(int sensor){ + if( sensor == FWD_L ){ + return this->USsensor.read(); + } + else{ + return this->sensors[sensor].read(); + } } -/*int Robot::searchh(int* timer){ - enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR }; - - static int state = neutral; - - static int counter = 0; //counter is used blabla - - //static int lastAct = 0; - - switch( state ){ - case neutral: - if( counter > MAX ){ - state = max; - } - else if( this->sensora[FWD] < NEAR ){ - state = wallF; - counter = 0; - } - else if( this->sensors[LEFT] < NEAR ){ - state = wallL; - counter = 0; - } - else if( this->sensors[RIGHT] < NEAR ){ - state = wallR; - counter = 0; - } - else if( this->sensors[FWD_L] < NEAR ){ - state = legoF; - counter = 0; - } - else if( this->sensors[LEFT_L] < NEAR ){ - state = legoL; - counter = 0; - } - else if( this->sensors[RIGHT_L] < NEAR ){ - state = legoR; - counter = 0; - } - break; - - case max: - - break; - */ - -//reset -//} - -//Add management for Overpower!! Pin PB_15 \ No newline at end of file +int Robot::getErrorMotor(){ + return 0; //errorMotor; +} \ No newline at end of file