![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
not sure on the if statement at the bottom
Dependencies: ContinuousServo Tach mbed TCS3472_I2C
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?
User | Revision | Line number | New 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 | } |