not sure on the if statement at the bottom
Dependencies: ContinuousServo Tach mbed TCS3472_I2C
color.cpp@0:7d319151aaa0, 2018-04-25 (annotated)
- Committer:
- PlayaLarrea
- Date:
- Wed Apr 25 13:33:13 2018 +0000
- Revision:
- 0:7d319151aaa0
- Child:
- 1:1dd468e2ad40
uh
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 | DigitalIn hall(p21); |
PlayaLarrea | 0:7d319151aaa0 | 7 | InterruptIn hall(p21); |
PlayaLarrea | 0:7d319151aaa0 | 8 | BusOut flash(LED1, LED2, LED3, LED4); |
PlayaLarrea | 0:7d319151aaa0 | 9 | TCS3472_I2C rgb_sensor(p9, p10); |
PlayaLarrea | 0:7d319151aaa0 | 10 | ContinuousServo left(p23); //Set up left wheel driver |
PlayaLarrea | 0:7d319151aaa0 | 11 | ContinuousServo right(p26); //Set up right wheel driver |
PlayaLarrea | 0:7d319151aaa0 | 12 | Tach tleft(p17,64); //Set up left tachometer |
PlayaLarrea | 0:7d319151aaa0 | 13 | Tach tright(p13, 64); //Set up right tachometer |
PlayaLarrea | 0:7d319151aaa0 | 14 | |
PlayaLarrea | 0:7d319151aaa0 | 15 | float wl; //Left wheel velocity |
PlayaLarrea | 0:7d319151aaa0 | 16 | float wr; //Right wheel velocity |
PlayaLarrea | 0:7d319151aaa0 | 17 | float e; //Error |
PlayaLarrea | 0:7d319151aaa0 | 18 | |
PlayaLarrea | 0:7d319151aaa0 | 19 | float r = 1.3125; //radius of wheels |
PlayaLarrea | 0:7d319151aaa0 | 20 | float l = 4.0625; //width of wheel base |
PlayaLarrea | 0:7d319151aaa0 | 21 | float omega = 0.0; //angular velocity of wheels |
PlayaLarrea | 0:7d319151aaa0 | 22 | float v = 0.5; //translational velocity of wheels |
PlayaLarrea | 0:7d319151aaa0 | 23 | float kp = 1.0; //Proportional control gain |
PlayaLarrea | 0:7d319151aaa0 | 24 | |
PlayaLarrea | 0:7d319151aaa0 | 25 | float vl = ((omega*l)+(2*v))/(2*r); //Calculates left wheel velocity |
PlayaLarrea | 0:7d319151aaa0 | 26 | float vr = ((omega*l)-(2*v))/(2*r); //Calculates right wheel velocity |
PlayaLarrea | 0:7d319151aaa0 | 27 | |
PlayaLarrea | 0:7d319151aaa0 | 28 | int counter = 0.0; |
PlayaLarrea | 0:7d319151aaa0 | 29 | |
PlayaLarrea | 0:7d319151aaa0 | 30 | void mine() { |
PlayaLarrea | 0:7d319151aaa0 | 31 | flash = 16.0; |
PlayaLarrea | 0:7d319151aaa0 | 32 | } |
PlayaLarrea | 0:7d319151aaa0 | 33 | |
PlayaLarrea | 0:7d319151aaa0 | 34 | int main() { |
PlayaLarrea | 0:7d319151aaa0 | 35 | |
PlayaLarrea | 0:7d319151aaa0 | 36 | hall.rise(&mine); |
PlayaLarrea | 0:7d319151aaa0 | 37 | hall.fall(&mine); |
PlayaLarrea | 0:7d319151aaa0 | 38 | |
PlayaLarrea | 0:7d319151aaa0 | 39 | while(1) { // wait around, interrupts will interrupt this! |
PlayaLarrea | 0:7d319151aaa0 | 40 | left.speed(vl); //Move the left wheel |
PlayaLarrea | 0:7d319151aaa0 | 41 | right.speed(vr); //Move the right wheel |
PlayaLarrea | 0:7d319151aaa0 | 42 | wl = tleft.getSpeed(); //Read the left tachometer |
PlayaLarrea | 0:7d319151aaa0 | 43 | wr = tright.getSpeed(); //Read the right tachometer |
PlayaLarrea | 0:7d319151aaa0 | 44 | |
PlayaLarrea | 0:7d319151aaa0 | 45 | e = wl - (-wr); //Calculate the error |
PlayaLarrea | 0:7d319151aaa0 | 46 | vr = vr - (kp*e); //Readjust the right wheel velocity |
PlayaLarrea | 0:7d319151aaa0 | 47 | |
PlayaLarrea | 0:7d319151aaa0 | 48 | if(rgb_sensor){ |
PlayaLarrea | 0:7d319151aaa0 | 49 | flash = counter; |
PlayaLarrea | 0:7d319151aaa0 | 50 | } |
PlayaLarrea | 0:7d319151aaa0 | 51 | } |
PlayaLarrea | 0:7d319151aaa0 | 52 | } |