Diff: Sources/Robot.cpp

Revision:

8:593f82e66bdf

Parent:

6:ba26dd3251b3

Child:

9:ac362674c480

diff -r ba26dd3251b3 -r 593f82e66bdf Sources/Robot.cpp
--- a/Sources/Robot.cpp	Wed Apr 26 14:09:08 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,348 +0,0 @@
-#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