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