Diff: Sources/Robot.cpp

Revision:

10:f76476943a6c

Parent:

6:ba26dd3251b3

Child:

11:292bdbd85a9c

--- a/Sources/Robot.cpp	Tue May 02 08:00:19 2017 +0000
+++ b/Sources/Robot.cpp	Wed May 03 13:54:51 2017 +0000
@@ -3,11 +3,12 @@
 
 /* Work in progress -------------------------------------------- */
 
-Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, Servo* Arm )
+Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer )
 {
-    this->powerSignal = enableSignal;
     this->left =        left;
     this->right =       right;
+    this->powerSignal = powerSignal;
+    //this->errorMotor =  errorMotor;
 
     this->left->period(0.00005f);
     this->right->period(0.00005f);
@@ -20,6 +21,7 @@
     this->rightS =      rightS;
     
     this->Arm =         Arm;
+    this->Greifer =     Greifer;
 
 }
 
@@ -96,17 +98,6 @@
 
 
 //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;
@@ -127,6 +118,17 @@
     }
 }
 
+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::wallLeft(int* counter, int* timer, int* lastAct){
     *counter += 1;
         
@@ -236,11 +238,11 @@
         counterMax(counter, timer, &lastAct, &rando);
     }
     
-    /*//Wall actions:
+    //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);
     }
@@ -250,15 +252,15 @@
     }
     
     //Lego actions:
-    else if (this->sensors[FWD_L] < NEAR_LEGO && legoFound == -1 || legoFound == 0){    //There's a Lego in front.
+    else if (this->sensors[FWD_L] < NEAR_LEGO + 0.03f && legoFound == -1 || legoFound == 0){ //There's a Lego in front.
         legoFront(counter, timer, &lastAct, &legoFound, found);
     }
 
-    else if (this->sensors[RIGHT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 1){  //There's a Lego on the right.
+    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.
+    else if (this->sensors[LEFT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 2){       //There's a Lego on the left.
         legoLeft(counter, timer, &lastAct, &legoFound);
     }
     
@@ -268,15 +270,6 @@
         *counter = 0;
         *timer = 0;
         legoFound = -1;
-        
-        //Old! Check if still useful!{
-            //check if this was the last action
-                //reset timer
-    
-            //dont add up counter
-            //use arm
-            //set legoFound to -1
-        //}
     }
     
     
@@ -286,62 +279,10 @@
     }
 }
 
-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->turnRight();
-        }
-        this->stop();
-    }
-    
-    else if (this->sensors[LEFT] < NEAR_LEGO){
-        t-> reset();
-        
-        *counter += 1;
-        while (this->sensors[FWD] > NEAR_LEGO){
-            if ( t->read() > TIMEOUT ){
-                break;
-            }
-            
-            this->turnLeft();
-        }
-        this->stop();
-    }
-    
-    else if (this->sensors[FWD] < 0.12f){
-        while (this->sensors[FWD] < 0.12f){
-            this->driveB();
-        }
-        while (this->sensors[FWD] < NEAR_LEGO){
-            this->turnRight();
-        }
-        while(1){
-            this->stop();
-        }
-    }
-    
-    else {
-        *counter = 0;
-        this->drive();
-    }
+int Robot::getErrorMotor(){
+    return 0; //errorMotor;
 }
 
-//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
 //}