ES202_Final
Initial attempt at the final chalenge
Dependencies: ContinuousServo TCS3472_I2C Tach mbed
Revision 5:c47b952fbd58, committed 2018-05-01
- Comitter:
- m202346
- Date:
- Tue May 01 01:22:24 2018 +0000
- Parent:
- 4:b23a2400c78f
- Commit message:
- Final Run
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
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