![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Diff: Sources/Robot.cpp
- Revision:
- 1:388c915756f5
- Parent:
- 0:d267b248eff4
- Child:
- 2:01e9de508316
--- a/Sources/Robot.cpp Sat Mar 11 10:14:00 2017 +0000 +++ b/Sources/Robot.cpp Sun Mar 19 12:20:26 2017 +0000 @@ -2,14 +2,19 @@ /* Work in progress -------------------------------------------- */ -Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal) +Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage ) { this->powerSignal = enableSignal; - this->left = left; - this->right = right; + this->left = left; + this->right = right; this->left->period(0.00005f); this->right->period(0.00005f); + + this->leds = leds; + + this->FarbVoltage = FarbVoltage; + } /* work in progress ------------------------------------------- */ @@ -17,41 +22,95 @@ { //pwm determine what direction it goes. *powerSignal = 1; - *left= 0.8f; - *right= 0.7f; - *left= 0.7f; - *right= 0.8f; + *left= 0.6f; + *right= 0.4f; +} + +void Robot::turnLeft() +{ + + *powerSignal = 1; + *left= 0.4f; + *right= 0.4f; + } -void Robot::turnLeft(){ +void Robot::turnRight() +{ + *powerSignal = 1; + *left= 0.6f; + *right= 0.6f; +} + +void Robot::turnAround(int left) +{ *powerSignal = 1; - *left= 0.35f; - *right= 0.65f; - + + if (left) { + turnLeft(); + } + + else { + turnRight(); + } +} + +void Robot::stop() +{ + *powerSignal = 1; + *left= 0.5f; + *right= 0.5f; } -void Robot::turnRight(){ - *powerSignal = 1; - *left= 0.65f; - *right= 0.35f; -} - -void Robot::turnAround(int left){ - *powerSignal = 1; +void Robot::search(int* counter){ + int rando; - if (left){ - turnLeft(); + if (*counter >= 5) { + rando = rand() % 2; + while (this->sensors[FWD] < NEAR){ + this->leds[FWD] = 1; + this->turnAround(rando); + } + this->leds[FWD] = 0; + } + + else if (this->sensors[RIGHT] < NEAR){ + *counter += 1; + while (this->sensors[RIGHT] < NEAR){ + this->turnLeft(); + this->leds[RIGHT] = 1; + } + this->leds[RIGHT] = 0; } - else{ - turnRight(); + else if (this->sensors[LEFT] < NEAR){ + *counter += 1; + while (this->sensors[LEFT] < NEAR){ + this->turnRight(); + this->leds[LEFT] = 1; + } + this->leds[LEFT] = 0; + } + + else if (this->sensors[FWD] < NEAR) { + rando = rand() % 2; + while (this->sensors[FWD] < NEAR){ + this->leds[FWD] = 1; + this->turnAround(rando); + } + this->leds[FWD] = 0; + } + + else { + *counter = 0; + this->drive(); } } //void Robot::init(){ // Robot.DistanceSensors.init(&sensorVoltage, &bit0, &bit1, &bit2, ii); //} - + //Remember to set //DigitalOut powerMotor(PB_2) = 1; //DigitalIn errorMotor(PB_14);