Zolovick_Larrea_Mouaffek / Mbed 2 deprecated ES202_Final_ColorSensor

not sure on the if statement at the bottom

Dependencies:   ContinuousServo TCS3472_I2C Tach mbed

Fork of ES202_Final_ColorSensor by Thomas Larrea

color.cpp@1:1dd468e2ad40, 2018-04-25 (annotated)

Committer:
PlayaLarrea
Date:
Wed Apr 25 14:13:24 2018 +0000
Revision:
1:1dd468e2ad40
Parent:
0:7d319151aaa0
Child:
2:b5a85ba07057
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
PlayaLarrea 0:7d319151aaa0 1 #include "mbed.h"
PlayaLarrea 0:7d319151aaa0 2 #include "ContinuousServo.h"
PlayaLarrea 0:7d319151aaa0 3 #include "Tach.h"
PlayaLarrea 0:7d319151aaa0 4 #include "TCS3472_I2C.h"
PlayaLarrea 0:7d319151aaa0 5
PlayaLarrea 0:7d319151aaa0 6 InterruptIn hall(p21);
PlayaLarrea 0:7d319151aaa0 7 BusOut flash(LED1, LED2, LED3, LED4);
PlayaLarrea 0:7d319151aaa0 8 TCS3472_I2C rgb_sensor(p9, p10);
PlayaLarrea 0:7d319151aaa0 9 ContinuousServo left(p23); //Set up left wheel driver
PlayaLarrea 0:7d319151aaa0 10 ContinuousServo right(p26); //Set up right wheel driver
PlayaLarrea 0:7d319151aaa0 11 Tach tleft(p17,64); //Set up left tachometer
PlayaLarrea 0:7d319151aaa0 12 Tach tright(p13, 64); //Set up right tachometer
PlayaLarrea 0:7d319151aaa0 13
PlayaLarrea 0:7d319151aaa0 14 float wl; //Left wheel velocity
PlayaLarrea 0:7d319151aaa0 15 float wr; //Right wheel velocity
PlayaLarrea 0:7d319151aaa0 16 float e; //Error
PlayaLarrea 0:7d319151aaa0 17
PlayaLarrea 0:7d319151aaa0 18 float r = 1.3125; //radius of wheels
PlayaLarrea 0:7d319151aaa0 19 float l = 4.0625; //width of wheel base
PlayaLarrea 0:7d319151aaa0 20 float omega = 0.0; //angular velocity of wheels
PlayaLarrea 0:7d319151aaa0 21 float v = 0.5; //translational velocity of wheels
PlayaLarrea 0:7d319151aaa0 22 float kp = 1.0; //Proportional control gain
PlayaLarrea 0:7d319151aaa0 23
PlayaLarrea 0:7d319151aaa0 24 float vl = ((omega*l)+(2*v))/(2*r); //Calculates left wheel velocity
PlayaLarrea 0:7d319151aaa0 25 float vr = ((omega*l)-(2*v))/(2*r); //Calculates right wheel velocity
PlayaLarrea 0:7d319151aaa0 26
PlayaLarrea 1:1dd468e2ad40 27 int rgb_data[4];
PlayaLarrea 1:1dd468e2ad40 28 float PWMbrightness = 1.0;
PlayaLarrea 1:1dd468e2ad40 29 float LB = 0.0;
PlayaLarrea 0:7d319151aaa0 30
PlayaLarrea 0:7d319151aaa0 31 void mine() {
PlayaLarrea 1:1dd468e2ad40 32 flash = 8.0;
PlayaLarrea 0:7d319151aaa0 33 }
PlayaLarrea 0:7d319151aaa0 34
PlayaLarrea 0:7d319151aaa0 35 int main() {
PlayaLarrea 0:7d319151aaa0 36
PlayaLarrea 0:7d319151aaa0 37 hall.rise(&mine);
PlayaLarrea 0:7d319151aaa0 38 hall.fall(&mine);
PlayaLarrea 0:7d319151aaa0 39
PlayaLarrea 1:1dd468e2ad40 40 rgb_sensor.enablePowerAndRGBC();
PlayaLarrea 1:1dd468e2ad40 41 rgb_sensor.setIntegrationTime(100);
PlayaLarrea 1:1dd468e2ad40 42
PlayaLarrea 0:7d319151aaa0 43 while(1) { // wait around, interrupts will interrupt this!
PlayaLarrea 0:7d319151aaa0 44 left.speed(vl); //Move the left wheel
PlayaLarrea 0:7d319151aaa0 45 right.speed(vr); //Move the right wheel
PlayaLarrea 0:7d319151aaa0 46 wl = tleft.getSpeed(); //Read the left tachometer
PlayaLarrea 0:7d319151aaa0 47 wr = tright.getSpeed(); //Read the right tachometer
PlayaLarrea 0:7d319151aaa0 48
PlayaLarrea 0:7d319151aaa0 49 e = wl - (-wr); //Calculate the error
PlayaLarrea 0:7d319151aaa0 50 vr = vr - (kp*e); //Readjust the right wheel velocity
PlayaLarrea 0:7d319151aaa0 51
PlayaLarrea 1:1dd468e2ad40 52 LB = PWMbrightness;
PlayaLarrea 1:1dd468e2ad40 53 rgb_sensor.getAllColors(rgb_data);
PlayaLarrea 1:1dd468e2ad40 54
PlayaLarrea 1:1dd468e2ad40 55 if(rgb_data[1]>999 && rgb_data[2]<999 && rgb_data[3]<999){
PlayaLarrea 1:1dd468e2ad40 56 flash = 1.0;
PlayaLarrea 0:7d319151aaa0 57 }
PlayaLarrea 1:1dd468e2ad40 58 else if(rgb_data[1]<999 && rgb_data[2]>999 && rgb_data[3]<999){
PlayaLarrea 1:1dd468e2ad40 59 flash = 2.0;
PlayaLarrea 1:1dd468e2ad40 60 }
PlayaLarrea 1:1dd468e2ad40 61 else if(rgb_data[1]<999 && rgb_data[2]<999 && rgb_data[3]>999){
PlayaLarrea 1:1dd468e2ad40 62 flash = 4.0;
PlayaLarrea 1:1dd468e2ad40 63 }
PlayaLarrea 0:7d319151aaa0 64 }
PlayaLarrea 0:7d319151aaa0 65 }