Diff: Sources/Robot.cpp

Revision:

16:f157e5ccd7d3

Parent:

15:915f8839fe48

--- a/Sources/Robot.cpp	Thu May 11 18:57:45 2017 +0000
+++ b/Sources/Robot.cpp	Mon May 15 17:35:24 2017 +0000
@@ -97,6 +97,28 @@
     *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){
@@ -234,6 +256,10 @@
     
     //static int lastAct = 0;
     
+    this->sensors[FWD] < NEAR ?       this->leds[4] = 1         : this->leds[4] = 0;
+    this->sensors[RIGHT] < NEAR ?     this->leds[RIGHT_L] = 1   : this->leds[RIGHT_L] = 0;
+    this->sensors[LEFT] < NEAR ?      this->leds[LEFT_L] = 1    : this->leds[LEFT_L] = 0;
+    
     switch( state ){
         case neutral:
             if( counter > MAX ){
@@ -248,13 +274,13 @@
             else if( this->sensors[RIGHT] < NEAR ){
                 state = wallR;
             }
-            else if( this->sensors[FWD_L] < NEAR ){
+            else if( this->sensors[FWD_L] < NEAR_LEGO+0.03f ){
                 state = legoF;
             }
-            else if( this->sensors[LEFT_L] < NEAR ){
+            else if( this->sensors[LEFT_L] < NEAR_LEGO ){
                 state = legoL;
             }
-            else if( this->sensors[RIGHT_L] < NEAR ){
+            else if( this->sensors[RIGHT_L] < NEAR_LEGO ){
                 state = legoR;
             }
             else{
@@ -311,8 +337,11 @@
         
         case legoF:
             //counter++;
-            if( this->sensors[FWD_L] > 0.17f ){
-                this->drive();
+            if( this->sensors[FWD] < NEAR ){
+                state = wallF;
+            }
+            else if( this->sensors[FWD_L] > 0.17f ){
+                this->driveSlowly();
             }
             else{
                 state = neutral;