Initial attempt at the final chalenge
Dependencies: ContinuousServo TCS3472_I2C Tach mbed
Diff: main.cpp
- Revision:
- 2:141024530b2f
- Parent:
- 0:10faa982ae23
- Child:
- 3:2cdd1e2df380
diff -r 10faa982ae23 -r 141024530b2f main.cpp --- 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