Diff: Sources/Robot.cpp

Revision:

17:4e1be70bdedb

Parent:

15:915f8839fe48

Child:

18:a158713a0049

--- 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