ES202_Final / Mbed 2 deprecated 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

main.cpp@4:b23a2400c78f, 2018-04-30 (annotated)

Committer:
m202346
Date:
Mon Apr 30 15:47:32 2018 +0000
Revision:
4:b23a2400c78f
Parent:
3:2cdd1e2df380
Child:
5:c47b952fbd58
after monday class

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