Diff: Sources/Robot.cpp

Revision:

3:63da1d1fae15

Parent:

2:c8c965d48f8d

Child:

4:d611df1ed42b

--- a/Sources/Robot.cpp	Thu May 25 18:12:28 2017 +0000
+++ b/Sources/Robot.cpp	Fri May 26 08:01:48 2017 +0000
@@ -1,10 +1,8 @@
 #include "Robot.h"
 #include "Declarations.h"
 
-/* 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, Ultraschall* USsensor, Pixy* pixy)
-{
+Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Pixy* pixy){
     this->left =        left;
     this->right =       right;
     this->powerSignal = powerSignal;
@@ -15,31 +13,26 @@
     
     this->leds =        leds;
     
-    this->FarbVoltage = FarbVoltage;
+    //this->FarbVoltage = FarbVoltage;
     this->frontS =      frontS;
     this->leftS =       leftS;
     this->rightS =      rightS;
     
     this->Arm =         Arm;
     this->Greifer =     Greifer;
-    this->Leiste =      Leiste;
-    this->USsensor =    USsensor;
     
     this->pixy =        pixy;
-
 }
 
 //Drive functions
-void Robot::drive()
-{
+void Robot::drive(){
     //pwm determine what direction it goes.
     *powerSignal = 1;
     *left=  0.6f;
     *right= 0.4f;
 }
 
-void Robot::driveB()
-{
+void Robot::driveB(){
     //pwm determine what direction it goes.
     *powerSignal = 1;
     *left=  0.4f;
@@ -56,55 +49,43 @@
     *right = pwm;
 }
 
-void Robot::turnLeft()
-{
-    
+void Robot::turnLeft(){
     *powerSignal = 1;
     *left=  0.4f;
     *right= 0.4f;
 
 }
 
-void Robot::turnLeftS()
-{
-    
+void Robot::turnLeftS(){
     *powerSignal = 1;
     *left=  0.45f;
     *right= 0.45f;
-
 }
 
-void Robot::turnRight()
-{
+void Robot::turnRight(){
     *powerSignal = 1;
     *left=  0.6f;
     *right= 0.6f;
 }
 
-void Robot::turnRightS()
-{
-    
+void Robot::turnRightS(){
     *powerSignal = 1;
     *left=  0.55f;
     *right= 0.55f;
-
 }
 
-void Robot::turnAround(int left)
-{
+void Robot::turnAround(int left){
     *powerSignal = 1;
 
-    if (left) {
+    if (left){
         turnLeft();
     }
-
-    else {
+    else{
         turnRight();
     }
 }
 
-void Robot::stop()
-{
+void Robot::stop(){
     *powerSignal = 1;
     *left=  0.5f;
     *right= 0.5f;
@@ -132,152 +113,13 @@
     }
 }
 
-/*
-//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.
-        *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::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;
-        
-    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, int* found){
-    //*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){     //If Sam sees a wall turn around
-        *legoFound = -1;
-        *counter = MAX;                 //setting counter to MAX will couse sam to turnAround
-    }
-    
-    if (this->sensors[FWD_L] > 0.16f){
-        this->driveSlowly();
-    }
-    else{
-        this->stop();
-        *found = 1;
-    }
-}
-
-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] > 0.22f){
-        this->turnRight();
-    }
-    else{
-        this->stop();
-        *legoFound = -1;
-    }
-}
-
-void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){
-    //*counter += 1;
-    *legoFound = 2;
-    
-    if (*lastAct != 6){          //If this wasn't the last called action, reset the timer.
-        *timer = 0;
-        *lastAct = 6;
-    }
-    
-    if (this->sensors[FWD_L] > 0.22f){
-        this->turnLeft();
-    }
-    else{
-        this->stop();
-        *legoFound = -1;
-    }
-}
-
-
-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();
-}*/
-
-
 int Robot::search(int* timer){
-    enum states {neutral = 0, counterMax, wallF, wallL, wallR };
-    
+    enum states {neutral = 0, counterMax, wallF, wallL, wallR};
     static int state = neutral;
-    
-    static int rando =      -1;
-    
-    static int lastAct =    0;
-    
+    static int rando = -1;
     int oldx = this->pixy->getX();
-    
     static int counter = 0;
-    
-    /*
-    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;
-    */
-    
-    
-    //printf("\n\rcurrent robot state:  %d", state);
+  
     if( this->pixy->getDetects() ) return 1;
     switch( state ){
         case neutral:
@@ -354,14 +196,6 @@
     return 1;
 }
 
-float Robot::see(int sensor){
-    if( sensor == FWD_L ){
-        return this->USsensor.read();
-    }
-    else{
-        return this->sensors[sensor].read();
-    }
-}
 
 int Robot::getErrorMotor(){
     return 0; //errorMotor;