Revision 3:2cdd1e2df380, committed 2018-04-26

Comitter:

nbchaskin

Date:

Thu Apr 26 15:42:36 2018 +0000

Parent:

2:141024530b2f

Child:

4:b23a2400c78f

Commit message:

Need to make go straight better and turn appropriately

Changed in this revision

--- a/main.cpp	Thu Apr 26 15:15:07 2018 +0000
+++ b/main.cpp	Thu Apr 26 15:42:36 2018 +0000
@@ -21,9 +21,8 @@
 int counter = 0;
 
 //need distance to stop
-float distance;
-distance = //inches;
-distance = //conversion to analog value;
+float distance = 0.0;
+//distance = 0.0;//inches;
 
 float l;
 float r;
@@ -33,6 +32,10 @@
 float errorR;
 float sampling_per;
 
+float PIpwmL(float desired_speed,float speed); 
+float PIpwmR(float desired_speed,float speed);
+
+float sonar = 1.0;
 int main() {
     hall.mode(PullUp);
     toggle = 1;
@@ -51,7 +54,8 @@
         speedR = tRight.getSpeed();
         l = PIpwmL(0.3, speedL);
         r = PIpwmR(0.3, speedR); 
-
+        l = l - 0.2;
+        r = r - 0.2;
         left.speed(l);
         right.speed(-r);
         
@@ -68,27 +72,39 @@
         if (sonar > distance){
             left.speed(l);
             right.speed(-r);
-            if (rgb_data[] < 1100){//add value for too dark
-                r = r + 0.05;
-                l = l - 0.05;
+            if (rgb_data[0] < 900){//add value for too dark
+                while (rgb_data[0] < 900){
+                    r = r + 0.01;
+                    right.speed(-r);
+                    left.speed(l);
+                    rgb_sensor.getAllColors( rgb_data );
+                    wait(0.1);
+                }
+                }
+            else if (rgb_data[0] > 3500){//add value for too light
+                while (rgb_data[0] > 3500){
+                    l = l + 0.01;
+                    right.speed(-r);
+                    left.speed(l);
+                    rgb_sensor.getAllColors( rgb_data );
+                    wait(0.1);
+                }
+                }
+            else if (rgb_data[0] < 3500 && rgb_data[0] > 900){
                 right.speed(-r);
                 left.speed(l);
-                }
-            else if (rgb_data[] > 4500){//add value for too light
-                r = r - 0.05;
-                l = l + 0.05;
-                right.speed(-r);
-                left.speed(l);
-                }
             }
         else {
+            left.stop();
+            right.stop();
             break;
             }
+            }
+    wait(0.05);
     }
 }
 
-float PIpwmL(float desired_speed,float speed)
-{
+float PIpwmL(float desired_speed,float speed){
     float integral_errorL = 0.0;  
     float sampling_per = 0.05;    
     float errorL = desired_speed - speed;
@@ -96,8 +112,7 @@
     float left = (0.07*integral_errorL) + (0.08*errorL) + 0.48;   
     return left;
     }
-float PIpwmR(float desired_speed,float speed)
-{
+float PIpwmR(float desired_speed,float speed){
         float integral_errorR = 0.0;
         float sampling_per = 0.05;
         float errorR = desired_speed - speed;