ES202_Final / Mbed 2 deprecated ES202_challenge

Initial attempt at the final chalenge

Dependencies:   ContinuousServo TCS3472_I2C Tach mbed

main.cpp@5:c47b952fbd58, 2018-05-01 (annotated)

Committer:
m202346
Date:
Tue May 01 01:22:24 2018 +0000
Revision:
5:c47b952fbd58
Parent:
4:b23a2400c78f 
Final Run

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nbchaskin 0:10faa982ae23 1 #include "mbed.h"
nbchaskin 0:10faa982ae23 2 #include "ContinuousServo.h"
nbchaskin 0:10faa982ae23 3 #include "Tach.h"
nbchaskin 0:10faa982ae23 4 #include "TCS3472_I2C.h"
nbchaskin 0:10faa982ae23 5 //driving
nbchaskin 0:10faa982ae23 6 Tach tLeft(p17,64);
nbchaskin 0:10faa982ae23 7 Tach tRight(p13,64);
nbchaskin 0:10faa982ae23 8
nbchaskin 0:10faa982ae23 9 ContinuousServo left(p23);
nbchaskin 0:10faa982ae23 10 ContinuousServo right(p26);
m202346 5:c47b952fbd58 11 AnalogIn sonar(p19);
nbchaskin 0:10faa982ae23 12
nbchaskin 0:10faa982ae23 13 //color sensor
nbchaskin 0:10faa982ae23 14 PwmOut LB(p25); // PWM out signal to power LED on the sensor
nbchaskin 0:10faa982ae23 15 TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
nbchaskin 0:10faa982ae23 16 Serial pc(USBTX,USBRX); // Establish serial connection with computer
nbchaskin 0:10faa982ae23 17
nbchaskin 0:10faa982ae23 18 //Hall effect
nbchaskin 0:10faa982ae23 19 InterruptIn hall(p21);
nbchaskin 0:10faa982ae23 20 DigitalOut led4(LED4);
nbchaskin 0:10faa982ae23 21 DigitalOut toggle(p22);
nbchaskin 0:10faa982ae23 22 int counter = 0;
nbchaskin 0:10faa982ae23 23
nbchaskin 0:10faa982ae23 24 //need distance to stop
m202346 5:c47b952fbd58 25 float distance;
nbchaskin 0:10faa982ae23 26
nbchaskin 2:141024530b2f 27 float l;
nbchaskin 2:141024530b2f 28 float r;
nbchaskin 2:141024530b2f 29 float speedL;
nbchaskin 2:141024530b2f 30 float speedR;
nbchaskin 2:141024530b2f 31 float errorL;
nbchaskin 2:141024530b2f 32 float errorR;
nbchaskin 2:141024530b2f 33 float sampling_per;
nbchaskin 2:141024530b2f 34
nbchaskin 3:2cdd1e2df380 35 float PIpwmL(float desired_speed,float speed);
nbchaskin 3:2cdd1e2df380 36 float PIpwmR(float desired_speed,float speed);
m202346 4:b23a2400c78f 37 float PropL(float actual_rgb, float desired_rgb);
m202346 4:b23a2400c78f 38 float PropR(float actual_rgb, float desired_rgb);
nbchaskin 3:2cdd1e2df380 39
nbchaskin 0:10faa982ae23 40 int main() {
nbchaskin 0:10faa982ae23 41 hall.mode(PullUp);
nbchaskin 0:10faa982ae23 42 toggle = 1;
m202346 5:c47b952fbd58 43 led4 = 0;
nbchaskin 0:10faa982ae23 44
nbchaskin 0:10faa982ae23 45 rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
nbchaskin 0:10faa982ae23 46 rgb_sensor.setIntegrationTime(100); // Set integration time of sensor
nbchaskin 0:10faa982ae23 47 int rgb_data[4]; // declare a 4 element array to store RGB sensor readings
nbchaskin 0:10faa982ae23 48 float PWMbrightness = 1.0; // float specifying brightness of LED (between 0.0 and 1.0)
m202346 4:b23a2400c78f 49 pc.baud(921600);
nbchaskin 0:10faa982ae23 50
nbchaskin 0:10faa982ae23 51 while(1) {
m202346 5:c47b952fbd58 52 float son[6];
m202346 5:c47b952fbd58 53 for (int count = 0;count<6;count++){
m202346 5:c47b952fbd58 54 son[count] = sonar;
m202346 5:c47b952fbd58 55 wait(0.01);
m202346 5:c47b952fbd58 56 }
m202346 5:c47b952fbd58 57
m202346 5:c47b952fbd58 58 float a;
m202346 5:c47b952fbd58 59 int i;
m202346 5:c47b952fbd58 60 int j;
m202346 5:c47b952fbd58 61 int n = 6;
m202346 5:c47b952fbd58 62 for (i = 0; i < n; i++)
m202346 5:c47b952fbd58 63 {
m202346 5:c47b952fbd58 64 for (j = i + 1; j < n; j++)
m202346 5:c47b952fbd58 65 {
m202346 5:c47b952fbd58 66 if (son[i] > son[j])
m202346 5:c47b952fbd58 67 {
m202346 5:c47b952fbd58 68 a = son[i];
m202346 5:c47b952fbd58 69 son[i] = son[j];
m202346 5:c47b952fbd58 70 son[j] = a;
m202346 5:c47b952fbd58 71 }
m202346 5:c47b952fbd58 72 }
m202346 5:c47b952fbd58 73 }
m202346 5:c47b952fbd58 74 pc.printf("%f\n",son[2]);
m202346 5:c47b952fbd58 75 distance = 10;//inches;
m202346 5:c47b952fbd58 76 distance = 0.003*distance - 0.0057;
m202346 5:c47b952fbd58 77 wait(0.05);
m202346 5:c47b952fbd58 78
nbchaskin 0:10faa982ae23 79 LB = PWMbrightness; // set brightness of sensor LED
nbchaskin 0:10faa982ae23 80 rgb_sensor.getAllColors( rgb_data ); // read the sensor to get red, green, and blue color data along with overall brightness
m202346 4:b23a2400c78f 81 wait(0.1);
nbchaskin 0:10faa982ae23 82
nbchaskin 2:141024530b2f 83 speedL = tLeft.getSpeed();
nbchaskin 2:141024530b2f 84 speedR = tRight.getSpeed();
nbchaskin 2:141024530b2f 85 l = PIpwmL(0.3, speedL);
nbchaskin 2:141024530b2f 86 r = PIpwmR(0.3, speedR);
m202346 5:c47b952fbd58 87 l = l - 0.41;
m202346 5:c47b952fbd58 88 r = r - 0.15;
nbchaskin 2:141024530b2f 89
nbchaskin 0:10faa982ae23 90 if(hall == 1){
m202346 5:c47b952fbd58 91 led4 = 1;
nbchaskin 0:10faa982ae23 92 wait(0.4);
nbchaskin 0:10faa982ae23 93 toggle = 0;
nbchaskin 0:10faa982ae23 94 wait(0.1);
nbchaskin 0:10faa982ae23 95 toggle = 1;
m202346 5:c47b952fbd58 96 led4 = 0;
nbchaskin 0:10faa982ae23 97 counter = counter +1;
nbchaskin 0:10faa982ae23 98 printf("Counter=%d\n",counter);
nbchaskin 0:10faa982ae23 99 }
m202346 5:c47b952fbd58 100 if(son[2] > 0.026){
nbchaskin 2:141024530b2f 101 left.speed(l);
nbchaskin 2:141024530b2f 102 right.speed(-r);
m202346 5:c47b952fbd58 103
m202346 5:c47b952fbd58 104 if (rgb_data[0] < 1900){//add value for too dark
m202346 5:c47b952fbd58 105 float rn = PropR(rgb_data[0],2100);
m202346 5:c47b952fbd58 106 rn = rn + r;
m202346 5:c47b952fbd58 107 right.speed(-rn);
nbchaskin 3:2cdd1e2df380 108 }
m202346 5:c47b952fbd58 109 else if (rgb_data[0] > 2900){//add value for too light
m202346 5:c47b952fbd58 110 float ln = PropL(rgb_data[0],2100);
m202346 4:b23a2400c78f 111 left.speed(ln);
nbchaskin 3:2cdd1e2df380 112 }
m202346 5:c47b952fbd58 113 else if (rgb_data[0] < 2900 && rgb_data[0] > 1900){
m202346 5:c47b952fbd58 114 right.speed(-r);
m202346 5:c47b952fbd58 115 left.speed(l);
m202346 5:c47b952fbd58 116 }
nbchaskin 0:10faa982ae23 117 }
nbchaskin 0:10faa982ae23 118 else {
nbchaskin 3:2cdd1e2df380 119 left.stop();
nbchaskin 3:2cdd1e2df380 120 right.stop();
nbchaskin 0:10faa982ae23 121 break;
nbchaskin 0:10faa982ae23 122 }
nbchaskin 3:2cdd1e2df380 123 }
nbchaskin 3:2cdd1e2df380 124 wait(0.05);
nbchaskin 0:10faa982ae23 125 }
m202346 5:c47b952fbd58 126
nbchaskin 2:141024530b2f 127
nbchaskin 3:2cdd1e2df380 128 float PIpwmL(float desired_speed,float speed){
nbchaskin 2:141024530b2f 129 float integral_errorL = 0.0;
nbchaskin 2:141024530b2f 130 float sampling_per = 0.05;
nbchaskin 2:141024530b2f 131 float errorL = desired_speed - speed;
nbchaskin 2:141024530b2f 132 integral_errorL += (errorL*sampling_per);
nbchaskin 2:141024530b2f 133 float left = (0.07*integral_errorL) + (0.08*errorL) + 0.48;
nbchaskin 2:141024530b2f 134 return left;
nbchaskin 2:141024530b2f 135 }
nbchaskin 3:2cdd1e2df380 136 float PIpwmR(float desired_speed,float speed){
nbchaskin 2:141024530b2f 137 float integral_errorR = 0.0;
nbchaskin 2:141024530b2f 138 float sampling_per = 0.05;
nbchaskin 2:141024530b2f 139 float errorR = desired_speed - speed;
nbchaskin 2:141024530b2f 140 integral_errorR += (errorR*sampling_per);
nbchaskin 2:141024530b2f 141 float right = (0.07*integral_errorR) + (0.08*errorR) + 0.25;
nbchaskin 2:141024530b2f 142 return right;
m202346 4:b23a2400c78f 143 }
m202346 4:b23a2400c78f 144 float PropR(float actual_rgb, float desired_rgb){
m202346 4:b23a2400c78f 145 float integral_errorr = 0.0;
m202346 4:b23a2400c78f 146 float sampling_periodr = 0.05;
m202346 4:b23a2400c78f 147 float errorr = desired_rgb - actual_rgb;
m202346 4:b23a2400c78f 148 integral_errorr += (errorr*sampling_periodr);
m202346 5:c47b952fbd58 149 float changer = (0.00006*integral_errorr) + (0.00009*errorr);
m202346 4:b23a2400c78f 150 return changer;
m202346 4:b23a2400c78f 151 }
m202346 4:b23a2400c78f 152 float PropL(float actual_rgb, float desired_rgb){
m202346 4:b23a2400c78f 153 float integral_error = 0.0;
m202346 4:b23a2400c78f 154 float sampling_period = 0.05;
m202346 4:b23a2400c78f 155 float error = desired_rgb - actual_rgb;
m202346 4:b23a2400c78f 156 integral_error += (error*sampling_period);
m202346 5:c47b952fbd58 157 float change = (0.00000015*integral_error) + (0.00000017*error)- 0.18;
m202346 4:b23a2400c78f 158 return -change;
m202346 4:b23a2400c78f 159 }