ES202_Final
/
ES202_challenge
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: ContinuousServo TCS3472_I2C Tach mbed
Revision 4:b23a2400c78f, committed 2018-04-30
- Comitter:
- m202346
- Date:
- Mon Apr 30 15:47:32 2018 +0000
- Parent:
- 3:2cdd1e2df380
- Child:
- 5:c47b952fbd58
- Commit message:
- after monday class
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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