![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Turn Code
Dependencies: ContinuousServo Tach mbed
main.cpp
- Committer:
- georgezolovick
- Date:
- 2018-04-25
- Revision:
- 0:f9723e66263e
- Child:
- 1:b19f21fdf69f
File content as of revision 0:f9723e66263e:
#include "mbed.h" #include "ContinuousServo.h" #include "Tach.h" 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.2; //angular velocity of wheels float v = 0.0; //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 main() { while(1){ //Forever 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 } }