Diff: Sources/Robot.cpp

Revision:

9:ac362674c480

diff -r 593f82e66bdf -r ac362674c480 Sources/Robot.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Robot.cpp	Tue May 02 08:39:35 2017 +0000
@@ -0,0 +1,348 @@
+#include "Robot.h"
+#include "Declarations.h"
+
+/* Work in progress -------------------------------------------- */
+
+Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, Servo* Arm )
+{
+    this->powerSignal = enableSignal;
+    this->left =        left;
+    this->right =       right;
+
+    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;
+
+}
+
+//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::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;
+}
+
+
+//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, 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->drive();
+    }
+    else{
+        *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();
+}
+
+
+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) {          //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, found);
+    }
+
+    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;
+        
+        //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->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();
+    }
+}
+
+//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
+//}
+
+//Add management for Overpower!! Pin PB_15
\ No newline at end of file