Robot.cpp

00001 #include "Robot.h"
00002 #include "Declarations.h"
00003 
00004 /* Work in progress -------------------------------------------- */
00005 
00006 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 )
00007 {
00008     this->left =        left;
00009     this->right =       right;
00010     this->powerSignal = powerSignal;
00011     //this->errorMotor =  errorMotor;
00012 
00013     this->left->period(0.00005f);
00014     this->right->period(0.00005f);
00015     
00016     this->leds =        leds;
00017     
00018     this->FarbVoltage = FarbVoltage;
00019     this->frontS =      frontS;
00020     this->leftS =       leftS;
00021     this->rightS =      rightS;
00022     
00023     this->Arm =         Arm;
00024     this->Greifer =     Greifer;
00025     this->Leiste =      Leiste;
00026     this->USsensor =    USsensor;
00027 
00028 }
00029 
00030 //Drive functions
00031 void Robot::drive()
00032 {
00033     //pwm determine what direction it goes.
00034     *powerSignal = 1;
00035     *left=  0.6f;
00036     *right= 0.4f;
00037 }
00038 
00039 void Robot::driveB()
00040 {
00041     //pwm determine what direction it goes.
00042     *powerSignal = 1;
00043     *left=  0.4f;
00044     *right= 0.6f;
00045 }
00046 
00047 void Robot::turnLeft()
00048 {
00049     
00050     *powerSignal = 1;
00051     *left=  0.4f;
00052     *right= 0.4f;
00053 
00054 }
00055 
00056 void Robot::turnLeftS()
00057 {
00058     
00059     *powerSignal = 1;
00060     *left=  0.45f;
00061     *right= 0.45f;
00062 
00063 }
00064 
00065 void Robot::turnRight()
00066 {
00067     *powerSignal = 1;
00068     *left=  0.6f;
00069     *right= 0.6f;
00070 }
00071 
00072 void Robot::turnRightS()
00073 {
00074     
00075     *powerSignal = 1;
00076     *left=  0.55f;
00077     *right= 0.55f;
00078 
00079 }
00080 
00081 void Robot::turnAround(int left)
00082 {
00083     *powerSignal = 1;
00084 
00085     if (left) {
00086         turnLeft();
00087     }
00088 
00089     else {
00090         turnRight();
00091     }
00092 }
00093 
00094 void Robot::stop()
00095 {
00096     *powerSignal = 1;
00097     *left=  0.5f;
00098     *right= 0.5f;
00099 }
00100 
00101 void Robot::driveSlowly(){
00102     static int i = 0;
00103     i++;
00104     if( i % 2 ){
00105         this->drive();
00106     }
00107     else{
00108         this->stop();
00109     }
00110 }
00111 
00112 void Robot::driveBackSlowly(){
00113     static int i = 0;
00114     i++;
00115     if( i % 2 ){
00116         this->driveB();
00117     }
00118     else{
00119         this->stop();
00120     }
00121 }
00122 
00123 /*
00124 //Functions that use the drive functions
00125 void Robot::counterMax(int* counter, int* timer, int* lastAct, int* rando){
00126     if (*lastAct != 0){                 //If this wasn't the last called action, reset the timer.
00127         *timer = 0;
00128         *lastAct = 0;
00129     }
00130     
00131     if (*rando == -1){                  //If rando was unused, set a new number.
00132         *rando = rand() % 2;
00133     }
00134     
00135     if (this->sensors[FWD] < NEAR){   //While something is seen turn around.
00136         this->turnAround(*rando);
00137     }
00138     
00139     else{
00140         *rando = -1;
00141         *counter = 0;
00142     }
00143 }
00144 
00145 void Robot::wallRight(int* counter, int* timer, int* lastAct){
00146     *counter += 1;
00147         
00148     if (*lastAct != 1){             //If this wasn't the last called action, reset the timer.
00149         *timer = 0;
00150         *lastAct = 1;
00151     }
00152         
00153     this->turnLeft();
00154 }
00155 
00156 void Robot::wallLeft(int* counter, int* timer, int* lastAct){
00157     *counter += 1;
00158         
00159     if (*lastAct != 2){             //If this wasn't the last called action, reset the timer.
00160         *timer = 0;
00161         *lastAct = 2;
00162     }
00163         
00164     this->turnRight();
00165 }
00166 
00167 void Robot::wallFront(int* counter, int* timer, int* lastAct){
00168     if (*lastAct != 3){              //If this wasn't the last called action, reset the timer.
00169         *timer = 0;
00170         *lastAct = 3;
00171     }
00172         
00173     *counter = MAX;                   //By setting the counter to MAX, next time it will go into the first if-statement (action 0).
00174 }
00175 
00176 
00177 void Robot::legoFront(int* counter, int* timer, int* lastAct, int* legoFound, int* found){
00178     //*counter += 1;
00179     *legoFound = 0;
00180     
00181     if (*lastAct != 4){                 //If this wasn't the last called action, reset the timer.
00182         *timer = 0;
00183         *lastAct = 4;
00184     }
00185     
00186     if (this->sensors[FWD] < NEAR){     //If Sam sees a wall turn around
00187         *legoFound = -1;
00188         *counter = MAX;                 //setting counter to MAX will couse sam to turnAround
00189     }
00190     
00191     if (this->sensors[FWD_L] > 0.16f){
00192         this->driveSlowly();
00193     }
00194     else{
00195         this->stop();
00196         *found = 1;
00197     }
00198 }
00199 
00200 void Robot::legoRight(int* counter, int* timer, int* lastAct, int* legoFound){
00201     //*counter += 1;
00202     *legoFound = 1;
00203     
00204     if (*lastAct != 5){          //If this wasn't the last called action, reset the timer.
00205         *timer = 0;
00206         *lastAct = 5;
00207     }
00208     
00209     if (this->sensors[FWD_L] > 0.22f){
00210         this->turnRight();
00211     }
00212     else{
00213         this->stop();
00214         *legoFound = -1;
00215     }
00216 }
00217 
00218 void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){
00219     //*counter += 1;
00220     *legoFound = 2;
00221     
00222     if (*lastAct != 6){          //If this wasn't the last called action, reset the timer.
00223         *timer = 0;
00224         *lastAct = 6;
00225     }
00226     
00227     if (this->sensors[FWD_L] > 0.22f){
00228         this->turnLeft();
00229     }
00230     else{
00231         this->stop();
00232         *legoFound = -1;
00233     }
00234 }
00235 
00236 
00237 void Robot::nothingFound(int* counter, int* timer, int* lastAct){
00238     *counter = 0;    
00239     if (*lastAct != 7){          //If this wasn't the last called action, reset the timer.
00240         *timer = 0;
00241         *lastAct = 7;
00242     }
00243     
00244     this->drive();
00245 }*/
00246 
00247 
00248 int Robot::search(int* timer){
00249     enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR };
00250     
00251     static int state = neutral;
00252     
00253     static int counter =    0;      //counter is used blabla
00254     
00255     static int rando =      -1;
00256     
00257     static int lastAct =    0;
00258     
00259     //static int stay =       -1;     //Stay is used to remain in a certain state
00260     /*
00261     this->sensors[FWD_L] < NEAR ?       this->leds[4] = 1         : this->leds[4] = 0;
00262     this->sensors[RIGHT_L] < NEAR ?     this->leds[RIGHT_L] = 1   : this->leds[RIGHT_L] = 0;
00263     this->sensors[LEFT_L] < NEAR ?      this->leds[LEFT_L] = 1    : this->leds[LEFT_L] = 0;
00264     */
00265     
00266     
00267         //printf("\n\rcurrent robot state:  %d", state);
00268     switch( state ){
00269         case neutral:
00270             if( counter > MAX ){
00271                 state = max;
00272             }
00273             else if( this->see(FWD) < NEAR ){
00274                 state = wallF;
00275             }
00276             else if( this->see(LEFT) < NEAR ){
00277                 state = wallL;
00278             }
00279             else if( this->see(RIGHT) < NEAR ){
00280                 state = wallR;
00281             }
00282             else if( this->see(FWD_L) < NEAR_LEGO + 0.015f ){
00283                 state = legoF;
00284             }
00285             else if( this->see(LEFT_L) < NEAR_LEGO ){
00286                 state = legoL;
00287             }
00288             else if( this->see(RIGHT_L) < NEAR_LEGO ){
00289                 state = legoR;
00290             }
00291             else{
00292                 this->drive();
00293                 counter = 0;
00294             }
00295             break;
00296             
00297         case max: {
00298             int time = 0;
00299             if( time < 15 && this->see(FWD) > NEAR ){
00300                 rando == -1 ? rando = rand() % 2 : rando = rando;
00301                 this->turnAround(rando);
00302             }
00303             else{
00304                 state = neutral;
00305                 counter = 0;
00306                 rando = -1;
00307             }
00308             break;
00309         }
00310         
00311         case wallF:
00312             counter++;
00313             if( this->see(FWD) < NEAR ){
00314                 rando == -1 ? rando = rand() % 2 : rando = rando;
00315                 this->turnAround(rando);
00316             }
00317             else{
00318                 state = neutral;
00319                 rando = -1;
00320             }
00321             break;
00322         
00323         case wallL:
00324             counter++;
00325             if( this->see(LEFT) < NEAR ){
00326                 this->turnRight();
00327             }
00328             else{
00329                 state = neutral;
00330             }
00331             break;
00332             
00333         case wallR:
00334             counter++;
00335             if( this->see(RIGHT) < NEAR ){
00336                 this->turnLeft();
00337             }
00338             else{
00339                 state = neutral;
00340             }
00341             break;
00342         
00343         case legoF:
00344             //counter++;
00345             if( this->see(FWD) < NEAR ){
00346                 state = wallF;
00347                 //stay = -1;
00348             }
00349             else if( this->see(FWD_L) < 0.19f ){
00350                 this->driveBackSlowly();
00351             }
00352             else{
00353                 state = neutral;
00354                 //stay = -1;
00355                 counter = 0;
00356                 this->stop();
00357                 return 1;
00358             }
00359             break;
00360         
00361         case legoL:
00362             counter++;
00363             if( this->see(FWD_L) > NEAR_LEGO + 0.05f ){
00364                 this->turnLeft();
00365             }
00366             else{
00367                 state = neutral;
00368                 this->drive();
00369                 //stay = 1;
00370             }
00371             break;
00372         
00373         case legoR:
00374             counter++;
00375             if( this->see(FWD_L) > NEAR_LEGO + 0.05f ){
00376                 this->turnRight();
00377             }
00378             else{
00379                 this->drive();
00380                 state = neutral;
00381                 //stay = 1;
00382             }
00383             break;
00384         
00385     }
00386     return 0;
00387 }
00388 
00389 float Robot::see(int sensor){
00390     if( sensor == FWD_L ){
00391         return this->USsensor.read();
00392     }
00393     else{
00394         return this->sensors[sensor].read();
00395     }
00396 }
00397 
00398 int Robot::getErrorMotor(){
00399     return 0; //errorMotor;
00400 }