Diff: Sources/Robot.cpp

Revision:

4:67d7177c213f

Parent:

3:fa61be4ecaa6

Child:

5:1aaf5de776ff

--- a/Sources/Robot.cpp	Wed Mar 22 13:33:07 2017 +0000
+++ b/Sources/Robot.cpp	Wed Apr 19 12:23:52 2017 +0000
@@ -1,8 +1,9 @@
 #include "Robot.h"
+#include "Declarations.h"
 
 /* Work in progress -------------------------------------------- */
 
-Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage )
+Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS )
 {
     this->powerSignal = enableSignal;
     this->left =        left;
@@ -14,10 +15,15 @@
     this->leds =        leds;
     
     this->FarbVoltage = FarbVoltage;
+    this->frontS =      frontS;
+    this->leftS =       leftS;
+    this->rightS =      rightS;
+    
+    this->arm =         arm;
 
 }
 
-/* work in progress ------------------------------------------- */
+//Drive functions
 void Robot::drive()
 {
     //pwm determine what direction it goes.
@@ -26,6 +32,14 @@
     *right= 0.4f;
 }
 
+void Robot::driveB()
+{
+    //pwm determine what direction it goes.
+    *powerSignal = 1;
+    *left=  0.4f;
+    *right= 0.6f;
+}
+
 void Robot::turnLeft()
 {
     
@@ -35,6 +49,15 @@
 
 }
 
+void Robot::turnLeftS()
+{
+    
+    *powerSignal = 1;
+    *left=  0.45f;
+    *right= 0.45f;
+
+}
+
 void Robot::turnRight()
 {
     *powerSignal = 1;
@@ -42,6 +65,15 @@
     *right= 0.6f;
 }
 
+void Robot::turnRightS()
+{
+    
+    *powerSignal = 1;
+    *left=  0.55f;
+    *right= 0.55f;
+
+}
+
 void Robot::turnAround(int left)
 {
     *powerSignal = 1;
@@ -62,67 +94,238 @@
     *right= 0.5f;
 }
 
-void Robot::search(int* counter, Timer* t){
-    int rando;
+
+//Functions that use the drive functions
+void Robot::wallRight(int* counter, int* timer, int* lastAct){
+    *counter += 1;
+        
+    if (*lastAct != 1){             //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 1;
+    }
+        
+    this->turnLeft();
+}
+
+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.
+        *timer = 0;
+        *lastAct = 0;
+    }
+    
+    if (*rando == -1){                  //If rando was unused, set a new number.
+        *rando = rand() % 2;
+    }
+    
+    if (this->sensors[FWD] < NEAR){   //While something is seen turn around.
+        this->turnAround(*rando);
+    }
+    
+    else{
+        *rando = -1;
+        *counter = 0;
+    }
+}
+
+void Robot::wallLeft(int* counter, int* timer, int* lastAct){
+    *counter += 1;
+        
+    if (*lastAct != 2){             //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 2;
+    }
+        
+    this->turnRight();
+}
+
+void Robot::wallFront(int* counter, int* timer, int* lastAct){
+    if (*lastAct != 3){              //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 3;
+    }
+        
+    *counter = MAX;                   //By setting the counter to MAX, next time it will go into the first if-statement (action 0).
+}
+
+
+void Robot::legoFront(int* counter, int* timer, int* lastAct, int* legoFound){
+    //*counter += 1;
+    *legoFound = 0;
+    
+    if (*lastAct != 4){               //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 4;
+    }
+    
+    if (this->sensors[FWD] < NEAR){
+        *legoFound = -1;
+    }
+    
+    else if (this->sensors[FWD_L] > 0.15f){
+        this->drive();
+    }
+    else{
+        this->stop();
+        *legoFound = 3;
+    }
+}
+
+void Robot::legoRight(int* counter, int* timer, int* lastAct, int* legoFound){
+    //*counter += 1;
+    *legoFound = 1;
+    
+    if (*lastAct != 5){          //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 5;
+    }
+    
+    if (this->sensors[FWD_L] > NEAR){
+        this->turnRight();
+    }
+    else{
+        this->stop();
+        *legoFound = 3;
+    }
+}
+
+void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){
+    //*counter += 1;
+    *legoFound = 2;
     
-    if (*counter >= 5) {
-        //t->reset();
+    if (*lastAct != 6){          //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 6;
+    }
+    
+    if (this->sensors[FWD_L] > NEAR){
+        this->turnLeft();
+    }
+    else{
+        this->stop();
+        *legoFound = 3;
+    }
+}
+
+
+void Robot::nothingFound(int* counter, int* timer, int* lastAct){
+    *counter = 0;    
+    if (*lastAct != 7){          //If this wasn't the last called action, reset the timer.
+        *timer = 0;
+        *lastAct = 7;
+    }
+    
+    this->drive();
+}
+
+
+void Robot::search(int* counter, int* timer, int* found){
+    
+    static int rando = -1;          //Rando will be used, to randomize turnAround()
+                                    //-1 := unused => set; 0 := turnRight(); 1 := turnLeft();
+    
+    static int lastAct = -1;        //Is used, to check if the same action in Robot::search() was made multiple times.
+                                    //-1 := unused; x != -1 := action x was called last.
+                                    
+    static int legoFound = -1;      //Is used to determine, on what side the lego was found.
+                                    //-1 := unused; 0:= front; 1:= right, 2:= left.
+                                    
+    *timer += 1;                    //timer holds the time in 0.1s
+    
+    
+    if (*counter >= MAX/* && legoFound == -1*/) {                   //Robot is stuck turning left & right
+        counterMax(counter, timer, &lastAct, &rando);
+    }
+    
+    //Wall actions:
+    else if (this->sensors[RIGHT] < NEAR && legoFound == -1){   //Robot has spotted an obstacle on the right.
+        wallRight(counter, timer, &lastAct);
+    }
+    
+    else if (this->sensors[LEFT] < NEAR && legoFound == -1) {   //Robot has spotted an obstacle on the left.
+        wallLeft(counter, timer, &lastAct);
+    }
+    
+    else if (this->sensors[FWD] < NEAR && legoFound == -1)  {   //Robot has spotted an obstacle infront
+        wallFront(counter, timer, &lastAct);
+    }
+    
+    //Lego actions:
+    else if (this->sensors[FWD_L] < NEAR_LEGO && legoFound == -1 || legoFound == 0){    //There's a Lego in front.
+        legoFront(counter, timer, &lastAct, &legoFound);
+    }
+
+    else if (this->sensors[RIGHT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 1){  //There's a Lego on the right.
+        legoRight(counter, timer, &lastAct, &legoFound);
+    }
+    
+    else if (this->sensors[LEFT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 2){   //There's a Lego on the left.
+        legoLeft(counter, timer, &lastAct, &legoFound);
+    }
+    
+    //Lego is ready to be grabbed.
+    else if(legoFound == 3){
+        *found = 1;             //When found is set to 1, main will call arms functions
+        *counter = 0;
+        *timer = 0;
+        legoFound = -1;
         
-        rando = rand() % 2;
-        while (this->sensors[FWD] < NEAR){
+        //Old! Check if still useful!{
+            //check if this was the last action
+                //reset timer
+    
+            //dont add up counter
+            //use arm
+            //set legoFound to -1
+        //}
+    }
+    
+    
+    //Nothing found
+    else {
+        nothingFound(counter, timer, &lastAct);
+    }
+}
+
+void Robot::lego(int* counter, Timer* t){
+    
+    if (this->sensors[RIGHT_L] < NEAR_LEGO){
+        t->reset();
+        
+        *counter += 1;
+        while (this->sensors[FWD] > NEAR_LEGO){
             if ( t->read() > TIMEOUT ){
                 break;
             }
             
-            this->leds[FWD] = 1;
-            this->turnAround(rando);
+            this->turnRight();
         }
-        this->leds[FWD] = 0;   
+        this->stop();
     }
     
-    else if (this->sensors[RIGHT] < NEAR){
+    else if (this->sensors[LEFT] < NEAR_LEGO){
         t-> reset();
         
         *counter += 1;
-        while (this->sensors[RIGHT] < NEAR){
+        while (this->sensors[FWD] > NEAR_LEGO){
             if ( t->read() > TIMEOUT ){
                 break;
             }
             
             this->turnLeft();
-            this->leds[RIGHT] = 1;
         }
-        this->leds[RIGHT] = 0;
+        this->stop();
     }
     
-    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;
+    else if (this->sensors[FWD] < 0.12f){
+        while (this->sensors[FWD] < 0.12f){
+            this->driveB();
         }
-        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);
+        while (this->sensors[FWD] < NEAR_LEGO){
+            this->turnRight();
         }
-     this->leds[FWD] = 0;   
+        while(1){
+            this->stop();
+        }
     }
     
     else {