Initial attempt at the final chalenge
Dependencies: ContinuousServo TCS3472_I2C Tach mbed
Diff: main.cpp
- Revision:
- 5:c47b952fbd58
- Parent:
- 4:b23a2400c78f
diff -r b23a2400c78f -r c47b952fbd58 main.cpp --- a/main.cpp Mon Apr 30 15:47:32 2018 +0000 +++ b/main.cpp Tue May 01 01:22:24 2018 +0000 @@ -8,6 +8,7 @@ ContinuousServo left(p23); ContinuousServo right(p26); +AnalogIn sonar(p19); //color sensor PwmOut LB(p25); // PWM out signal to power LED on the sensor @@ -21,8 +22,7 @@ int counter = 0; //need distance to stop -float distance = 0.0; -//distance = 0.0;//inches; +float distance; float l; float r; @@ -37,11 +37,10 @@ float PropL(float actual_rgb, float desired_rgb); float PropR(float actual_rgb, float desired_rgb); -float sonar = 1.0; 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 @@ -50,66 +49,71 @@ pc.baud(921600); while(1) { + float son[6]; + for (int count = 0;count<6;count++){ + son[count] = sonar; + wait(0.01); + } + + float a; + int i; + int j; + int n = 6; + for (i = 0; i < n; i++) + { + for (j = i + 1; j < n; j++) + { + if (son[i] > son[j]) + { + a = son[i]; + son[i] = son[j]; + son[j] = a; + } + } + } + pc.printf("%f\n",son[2]); + distance = 10;//inches; + distance = 0.003*distance - 0.0057; + wait(0.05); + 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.29; - r = r - 0.05; - //left.speed(l); - //right.speed(-r); + l = l - 0.41; + r = r - 0.15; 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){ + if(son[2] > 0.026){ left.speed(l); right.speed(-r); - 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); - //} + + if (rgb_data[0] < 1900){//add value for too dark + float rn = PropR(rgb_data[0],2100); + rn = rn + r; + right.speed(-rn); } - else if (rgb_data[0] > 3200){//add value for too light - float ln = PropL(rgb_data[0],2500); - //ln = ln + l; + else if (rgb_data[0] > 2900){//add value for too light + float ln = PropL(rgb_data[0],2100); 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] < 3200 && rgb_data[0] > 1800){ - right.speed(-r); - left.speed(l); + else if (rgb_data[0] < 2900 && rgb_data[0] > 1900){ + right.speed(-r); + left.speed(l); + } } else { left.stop(); @@ -119,7 +123,7 @@ } wait(0.05); } -} + float PIpwmL(float desired_speed,float speed){ float integral_errorL = 0.0; @@ -142,7 +146,7 @@ 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); + float changer = (0.00006*integral_errorr) + (0.00009*errorr); return changer; } float PropL(float actual_rgb, float desired_rgb){ @@ -150,6 +154,6 @@ 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); + float change = (0.00000015*integral_error) + (0.00000017*error)- 0.18; return -change; } \ No newline at end of file