Diff: Sources/Robot.cpp

Revision:

1:388c915756f5

Parent:

0:d267b248eff4

Child:

2:01e9de508316

--- a/Sources/Robot.cpp	Sat Mar 11 10:14:00 2017 +0000
+++ b/Sources/Robot.cpp	Sun Mar 19 12:20:26 2017 +0000
@@ -2,14 +2,19 @@
 
 /* Work in progress -------------------------------------------- */
 
-Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal)
+Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage )
 {
     this->powerSignal = enableSignal;
-    this->left = left;
-    this->right = right;
+    this->left =        left;
+    this->right =       right;
 
     this->left->period(0.00005f);
     this->right->period(0.00005f);
+    
+    this->leds =        leds;
+    
+    this->FarbVoltage = FarbVoltage;
+
 }
 
 /* work in progress ------------------------------------------- */
@@ -17,41 +22,95 @@
 {
     //pwm determine what direction it goes.
     *powerSignal = 1;
-    *left=  0.8f;
-    *right= 0.7f;
-    *left=  0.7f;
-    *right= 0.8f;
+    *left=  0.6f;
+    *right= 0.4f;
+}
+
+void Robot::turnLeft()
+{
+    
+    *powerSignal = 1;
+    *left=  0.4f;
+    *right= 0.4f;
+
 }
 
-void Robot::turnLeft(){
+void Robot::turnRight()
+{
+    *powerSignal = 1;
+    *left=  0.6f;
+    *right= 0.6f;
+}
+
+void Robot::turnAround(int left)
+{
     *powerSignal = 1;
-    *left=  0.35f;
-    *right= 0.65f;
-    
+
+    if (left) {
+        turnLeft();
+    }
+
+    else {
+        turnRight();
+    }
+}
+
+void Robot::stop()
+{
+    *powerSignal = 1;
+    *left=  0.5f;
+    *right= 0.5f;
 }
 
-void Robot::turnRight(){
-    *powerSignal = 1;
-    *left=  0.65f;
-    *right= 0.35f;
-}
-
-void Robot::turnAround(int left){
-    *powerSignal = 1;
+void Robot::search(int* counter){
+    int rando;
     
-    if (left){
-        turnLeft();
+    if (*counter >= 5) {
+        rando = rand() % 2;
+        while (this->sensors[FWD] < NEAR){
+            this->leds[FWD] = 1;
+            this->turnAround(rando);
+        }
+     this->leds[FWD] = 0;   
+    }
+    
+    else if (this->sensors[RIGHT] < NEAR){
+        *counter += 1;
+        while (this->sensors[RIGHT] < NEAR){
+            this->turnLeft();
+            this->leds[RIGHT] = 1;
+        }
+        this->leds[RIGHT] = 0;
     }
     
-    else{
-        turnRight();
+    else if (this->sensors[LEFT] < NEAR){
+        *counter += 1;
+        while (this->sensors[LEFT] < NEAR){
+            this->turnRight();
+            this->leds[LEFT] = 1;
+        }
+        this->leds[LEFT] = 0;
+    }
+    
+    else if (this->sensors[FWD] < NEAR) {
+        rando = rand() % 2;
+        while (this->sensors[FWD] < NEAR){
+            this->leds[FWD] = 1;
+            this->turnAround(rando);
+        }
+     this->leds[FWD] = 0;   
+    }
+    
+    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);