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