Diff: Sources/Robot.cpp

Revision:

0:15a8480061e8

Child:

2:c8c965d48f8d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Robot.cpp	Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,368 @@
+#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)
+{
+    this->left =        left;
+    this->right =       right;
+    this->powerSignal = powerSignal;
+    //this->errorMotor =  errorMotor;
+
+    this->left->period(0.00005f);
+    this->right->period(0.00005f);
+    
+    this->leds =        leds;
+    
+    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()
+{
+    //pwm determine what direction it goes.
+    *powerSignal = 1;
+    *left=  0.6f;
+    *right= 0.4f;
+}
+
+void Robot::driveB()
+{
+    //pwm determine what direction it goes.
+    *powerSignal = 1;
+    *left=  0.4f;
+    *right= 0.6f;
+}
+
+void Robot::setLeft(float pwm){
+    *powerSignal = 1;
+    *left = pwm;
+}
+
+void Robot::setRight(float pwm){
+    *powerSignal = 1;
+    *right = pwm;
+}
+
+void Robot::turnLeft()
+{
+    
+    *powerSignal = 1;
+    *left=  0.4f;
+    *right= 0.4f;
+
+}
+
+void Robot::turnLeftS()
+{
+    
+    *powerSignal = 1;
+    *left=  0.45f;
+    *right= 0.45f;
+
+}
+
+void Robot::turnRight()
+{
+    *powerSignal = 1;
+    *left=  0.6f;
+    *right= 0.6f;
+}
+
+void Robot::turnRightS()
+{
+    
+    *powerSignal = 1;
+    *left=  0.55f;
+    *right= 0.55f;
+
+}
+
+void Robot::turnAround(int left)
+{
+    *powerSignal = 1;
+
+    if (left) {
+        turnLeft();
+    }
+
+    else {
+        turnRight();
+    }
+}
+
+void Robot::stop()
+{
+    *powerSignal = 1;
+    *left=  0.5f;
+    *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.
+        *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 };
+    
+    static int state = neutral;
+    
+    static int rando =      -1;
+    
+    static int lastAct =    0;
+    
+    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:
+            if( counter > MAX ){
+                state = counterMax;
+            }
+            else if( this->sensors[FWD].read() < NEAR ){
+                state = wallF;
+                counter = 0;
+            }
+            else if( this->sensors[LEFT].read() < NEAR ){
+                state = wallL;
+                counter = 0;
+            }
+            else if( this->sensors[RIGHT].read() < NEAR ){
+                state = wallR;
+                counter = 0;
+            }
+            else{
+                this->drive();
+                counter = 0;
+            }
+            break;
+        
+        case counterMax:{
+            int i = 0;
+            if( i < 1000 ){
+                rando == -1 ? rando = rand() % 2 : rando = rando;
+                this->turnAround(rando);
+                i++;
+            }
+            else{
+                state = neutral;
+                rando = -1;
+                counter = 0;
+                i = 0;
+            }
+            break;
+        }
+        
+        case wallF:
+            if( this->sensors[FWD].read() < NEAR ){
+                rando == -1 ? rando = rand() % 2 : rando = rando;
+                this->turnAround(rando);
+                counter++;
+            }
+            else{
+                state = neutral;
+                rando = -1;
+            }
+            break;
+        
+        case wallL:
+            if( this->sensors[LEFT].read() < NEAR ){
+                this->turnRight();
+                counter++;
+            }
+            else{
+                state = neutral;
+            }
+            break;
+            
+        case wallR:
+            if( this->sensors[RIGHT].read() < NEAR ){
+                this->turnLeft();
+                counter++;
+            }
+            else{
+                state = neutral;
+            }
+            break;
+        
+    }
+    return 0;
+}
+
+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;
+}
\ No newline at end of file