![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Initial attempt at the final chalenge
Dependencies: ContinuousServo TCS3472_I2C Tach mbed
main.cpp
- Committer:
- m202346
- Date:
- 2018-05-01
- Revision:
- 5:c47b952fbd58
- Parent:
- 4:b23a2400c78f
File content as of revision 5:c47b952fbd58:
#include "mbed.h" #include "ContinuousServo.h" #include "Tach.h" #include "TCS3472_I2C.h" //driving Tach tLeft(p17,64); Tach tRight(p13,64); ContinuousServo left(p23); ContinuousServo right(p26); AnalogIn sonar(p19); //color sensor PwmOut LB(p25); // PWM out signal to power LED on the sensor TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object Serial pc(USBTX,USBRX); // Establish serial connection with computer //Hall effect InterruptIn hall(p21); DigitalOut led4(LED4); DigitalOut toggle(p22); int counter = 0; //need distance to stop float distance; float l; float r; float speedL; float speedR; float errorL; float errorR; float sampling_per; 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); int main() { hall.mode(PullUp); toggle = 1; led4 = 0; rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor 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) { 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 wait(0.1); speedL = tLeft.getSpeed(); speedR = tRight.getSpeed(); l = PIpwmL(0.3, speedL); r = PIpwmR(0.3, speedR); l = l - 0.41; r = r - 0.15; if(hall == 1){ led4 = 1; wait(0.4); toggle = 0; wait(0.1); toggle = 1; led4 = 0; counter = counter +1; printf("Counter=%d\n",counter); } if(son[2] > 0.026){ left.speed(l); right.speed(-r); 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] > 2900){//add value for too light float ln = PropL(rgb_data[0],2100); left.speed(ln); } else if (rgb_data[0] < 2900 && rgb_data[0] > 1900){ right.speed(-r); left.speed(l); } } else { left.stop(); right.stop(); break; } } wait(0.05); } float PIpwmL(float desired_speed,float speed){ float integral_errorL = 0.0; float sampling_per = 0.05; float errorL = desired_speed - speed; integral_errorL += (errorL*sampling_per); float left = (0.07*integral_errorL) + (0.08*errorL) + 0.48; return left; } float PIpwmR(float desired_speed,float speed){ float integral_errorR = 0.0; float sampling_per = 0.05; float errorR = desired_speed - speed; integral_errorR += (errorR*sampling_per); float right = (0.07*integral_errorR) + (0.08*errorR) + 0.25; return right; } 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.00006*integral_errorr) + (0.00009*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.00000015*integral_error) + (0.00000017*error)- 0.18; return -change; }