![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
a
Dependencies: Servo ServoArm mbed
Fork of PES_Official-TestF by
Sources/Robot.cpp
- Committer:
- beacon
- Date:
- 2017-03-21
- Revision:
- 2:01e9de508316
- Parent:
- 1:388c915756f5
- Child:
- 3:fa61be4ecaa6
File content as of revision 2:01e9de508316:
#include "Robot.h" /* Work in progress -------------------------------------------- */ Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage ) { this->powerSignal = enableSignal; 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 ------------------------------------------- */ void Robot::drive() { //pwm determine what direction it goes. *powerSignal = 1; *left= 0.6f; *right= 0.4f; } void Robot::turnLeft() { *powerSignal = 1; *left= 0.4f; *right= 0.4f; } void Robot::turnRight() { *powerSignal = 1; *left= 0.6f; *right= 0.6f; } void Robot::turnAround(int left) { *powerSignal = 1; if (left) { turnLeft(); } else { turnRight(); } } void Robot::stop() { *powerSignal = 1; *left= 0.5f; *right= 0.5f; } void Robot::search(int* counter, Timer* t){ int rando; if (*counter >= 5) { t->reset(); rando = rand() % 2; while (this->sensors[FWD] < NEAR){ if ( t->read() > TIMEOUT ){ break; } this->leds[FWD] = 1; this->turnAround(rando); } this->leds[FWD] = 0; } else if (this->sensors[RIGHT] < NEAR){ t-> reset(); *counter += 1; while (this->sensors[RIGHT] < NEAR){ if ( t->read() > TIMEOUT ){ break; } this->turnLeft(); this->leds[RIGHT] = 1; } this->leds[RIGHT] = 0; } else if (this->sensors[LEFT] < NEAR){ t-> reset(); *counter += 1; while (this->sensors[LEFT] < NEAR){ if ( t->read() > TIMEOUT ){ break; } this->turnRight(); this->leds[LEFT] = 1; } this->leds[LEFT] = 0; } else if (this->sensors[FWD] < NEAR) { t-> reset(); rando = rand() % 2; while (this->sensors[FWD] < NEAR){ if ( t->read() > TIMEOUT ){ break; } 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); //if (errorMotor){ //reset //} //Add management for Overpower!! Pin PB_15