Diff: main.cpp

Revision:

2:141024530b2f

Parent:

0:10faa982ae23

Child:

3:2cdd1e2df380

--- a/main.cpp	Wed Apr 25 14:19:25 2018 +0000
+++ b/main.cpp	Thu Apr 26 15:15:07 2018 +0000
@@ -25,10 +25,18 @@
 distance = //inches;
 distance = //conversion to analog value;
 
+float l;
+float r;
+float speedL;
+float speedR;
+float errorL;
+float errorR;
+float sampling_per;
+
 int main() {
     hall.mode(PullUp);
     toggle = 1;
-    led4 = 0;
+    //led4 = 0;
     
     rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
     rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
@@ -39,27 +47,38 @@
         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
         
+        speedL = tLeft.getSpeed();
+        speedR = tRight.getSpeed();
+        l = PIpwmL(0.3, speedL);
+        r = PIpwmR(0.3, speedR); 
+
+        left.speed(l);
+        right.speed(-r);
+        
        if(hall == 1){
-           led4 = 1;   
+           //led4 = 1;   
            wait(0.4);
            toggle = 0;
            wait(0.1);
            toggle = 1;
-           led4 = 0;
+           //led4 = 0;
            counter = counter +1;
            printf("Counter=%d\n",counter);
            }
         if (sonar > distance){
-            float speed = 0.5;
-            left.speed(speed);
-            right.speed(-speed);
-            if (rgb_data[] > ){//add value for too dark
-                right.speed(-speed - 0.05);
-                left.speed(speed - 0.05);
+            left.speed(l);
+            right.speed(-r);
+            if (rgb_data[] < 1100){//add value for too dark
+                r = r + 0.05;
+                l = l - 0.05;
+                right.speed(-r);
+                left.speed(l);
                 }
-            else if (rgb_data[] <){//add value for too light
-                right.speed(-speed + 0.05);
-                left.speed(speed + 0.05);
+            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 {
@@ -67,3 +86,22 @@
             }
     }
 }
+
+float PIpwmL(float desired_speed,float speed)
+{
+    float integral_errorL = 0.0;  
+    float sampling_per = 0.05;    
+    float errorL = desired_speed - speed;
+    integral_errorL += (errorL*sampling_per);   
+    float left = (0.07*integral_errorL) + (0.08*errorL) + 0.48;   
+    return left;
+    }
+float PIpwmR(float desired_speed,float speed)
+{
+        float integral_errorR = 0.0;
+        float sampling_per = 0.05;
+        float errorR = desired_speed - speed;
+        integral_errorR += (errorR*sampling_per);
+        float right = (0.07*integral_errorR) + (0.08*errorR) + 0.25;
+        return right;
+    }
\ No newline at end of file