![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
not sure on the if statement at the bottom
Dependencies: ContinuousServo TCS3472_I2C Tach mbed
Fork of ES202_Final_ColorSensor by
color.cpp
- Committer:
- PlayaLarrea
- Date:
- 2018-04-25
- Revision:
- 0:7d319151aaa0
- Child:
- 1:1dd468e2ad40
File content as of revision 0:7d319151aaa0:
#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; } } }