not sure on the if statement at the bottom
Dependencies: ContinuousServo Tach mbed TCS3472_I2C
Diff: color.cpp
- Revision:
- 0:7d319151aaa0
- Child:
- 1:1dd468e2ad40
diff -r 000000000000 -r 7d319151aaa0 color.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/color.cpp Wed Apr 25 13:33:13 2018 +0000 @@ -0,0 +1,52 @@ +#include "mbed.h" +#include "ContinuousServo.h" +#include "Tach.h" +#include "TCS3472_I2C.h" + +DigitalIn hall(p21); +InterruptIn hall(p21); +BusOut flash(LED1, LED2, LED3, LED4); +TCS3472_I2C rgb_sensor(p9, p10); +ContinuousServo left(p23); //Set up left wheel driver +ContinuousServo right(p26); //Set up right wheel driver +Tach tleft(p17,64); //Set up left tachometer +Tach tright(p13, 64); //Set up right tachometer + +float wl; //Left wheel velocity +float wr; //Right wheel velocity +float e; //Error + +float r = 1.3125; //radius of wheels +float l = 4.0625; //width of wheel base +float omega = 0.0; //angular velocity of wheels +float v = 0.5; //translational velocity of wheels +float kp = 1.0; //Proportional control gain + +float vl = ((omega*l)+(2*v))/(2*r); //Calculates left wheel velocity +float vr = ((omega*l)-(2*v))/(2*r); //Calculates right wheel velocity + +int counter = 0.0; + +void mine() { + flash = 16.0; +} + +int main() { + + hall.rise(&mine); + hall.fall(&mine); + + while(1) { // wait around, interrupts will interrupt this! + left.speed(vl); //Move the left wheel + right.speed(vr); //Move the right wheel + wl = tleft.getSpeed(); //Read the left tachometer + wr = tright.getSpeed(); //Read the right tachometer + + e = wl - (-wr); //Calculate the error + vr = vr - (kp*e); //Readjust the right wheel velocity + + if(rgb_sensor){ + flash = counter; + } + } +} \ No newline at end of file