Diff: main.cpp

Revision:

4:b23a2400c78f

Parent:

3:2cdd1e2df380

Child:

5:c47b952fbd58

--- a/main.cpp	Thu Apr 26 15:42:36 2018 +0000
+++ b/main.cpp	Mon Apr 30 15:47:32 2018 +0000
@@ -34,6 +34,8 @@
 
 float PIpwmL(float desired_speed,float speed); 
 float PIpwmR(float desired_speed,float speed);
+float PropL(float actual_rgb, float desired_rgb);
+float PropR(float actual_rgb, float desired_rgb);
 
 float sonar = 1.0;
 int main() {
@@ -45,19 +47,22 @@
     rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
     int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
     float PWMbrightness = 1.0; // float specifying brightness of LED (between 0.0 and 1.0)
+    pc.baud(921600);
  
     while(1) {
         LB = PWMbrightness; // set brightness of sensor LED
         rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness
+        pc.printf( "unfiltered: %d, red: %d, green: %d, blue: %d \n", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
+        wait(0.1);
         
         speedL = tLeft.getSpeed();
         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);
+        l = l - 0.29;
+        r = r - 0.05;
+        //left.speed(l);
+        //right.speed(-r);
         
        if(hall == 1){
            //led4 = 1;   
@@ -72,26 +77,38 @@
         if (sonar > distance){
             left.speed(l);
             right.speed(-r);
-            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);
-                }
+            if (rgb_data[0] < 1800){//add value for too dark
+                float rn = PropR(rgb_data[0],2500);
+                //rn = rn + r;
+                right.speed(-rn);                
+                //float rn = r;
+                //rn = rn + 0.005;
+                //left.speed(0.1);
+                //while ((rgb_data[0] < 1500)){
+                    //rn = rn + 0.01;
+                    //right.speed(-rn);
+                   // left.speed(l);
+                    //rgb_sensor.getAllColors( rgb_data );
+                    //pc.printf( "unfiltered: %d, red: %d, green: %d, blue: %d \n", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
+                  //  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] > 3200){//add value for too light
+                float ln = PropL(rgb_data[0],2500);
+                //ln = ln + l;
+                left.speed(ln);
+                //right.speed(-0.1);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                
+                //while ((rgb_data[0] > 3300)){
+                    //ln = ln + 0.005;
+                    //right.speed(-r);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                
+                    //left.speed(ln);
+                    //rgb_sensor.getAllColors( rgb_data );
+                    //pc.printf( "unfiltered: %d, red: %d, green: %d, blue: %d \n", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
+                   // wait(0.1);
+                //}
                 }
-                }
-            else if (rgb_data[0] < 3500 && rgb_data[0] > 900){
-                right.speed(-r);
+            else if (rgb_data[0] < 3200 && rgb_data[0] > 1800){
+              right.speed(-r);
                 left.speed(l);
             }
         else {
@@ -119,4 +136,20 @@
         integral_errorR += (errorR*sampling_per);
         float right = (0.07*integral_errorR) + (0.08*errorR) + 0.25;
         return right;
-    }
\ No newline at end of file
+    }
+float PropR(float actual_rgb, float desired_rgb){
+        float integral_errorr = 0.0;
+        float sampling_periodr = 0.05;
+        float errorr = desired_rgb - actual_rgb;
+        integral_errorr += (errorr*sampling_periodr);
+        float changer = (0.0005*integral_errorr) + (0.0008*errorr);
+        return changer;
+        }
+float PropL(float actual_rgb, float desired_rgb){
+        float integral_error = 0.0;
+        float sampling_period = 0.05;
+        float error = desired_rgb - actual_rgb;
+        integral_error += (error*sampling_period);
+        float change = (0.0005*integral_error) + (0.0008*error);
+        return -change;
+        }
\ No newline at end of file