![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Turn Code
Dependencies: ContinuousServo Tach mbed
main.cpp@1:b19f21fdf69f, 2018-04-25 (annotated)
- Committer:
- georgezolovick
- Date:
- Wed Apr 25 03:09:37 2018 +0000
- Revision:
- 1:b19f21fdf69f
- Parent:
- 0:f9723e66263e
24APR2018
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
georgezolovick | 0:f9723e66263e | 1 | #include "mbed.h" |
georgezolovick | 0:f9723e66263e | 2 | #include "ContinuousServo.h" |
georgezolovick | 0:f9723e66263e | 3 | #include "Tach.h" |
georgezolovick | 0:f9723e66263e | 4 | |
georgezolovick | 0:f9723e66263e | 5 | ContinuousServo left(p23); //Set up left wheel driver |
georgezolovick | 0:f9723e66263e | 6 | ContinuousServo right(p26); //Set up right wheel driver |
georgezolovick | 0:f9723e66263e | 7 | Tach tleft(p17,64); //Set up left tachometer |
georgezolovick | 0:f9723e66263e | 8 | Tach tright(p13, 64); //Set up right tachometer |
georgezolovick | 0:f9723e66263e | 9 | |
georgezolovick | 0:f9723e66263e | 10 | float wl; //Left wheel velocity |
georgezolovick | 0:f9723e66263e | 11 | float wr; //Right wheel velocity |
georgezolovick | 0:f9723e66263e | 12 | float e; //Error |
georgezolovick | 0:f9723e66263e | 13 | |
georgezolovick | 0:f9723e66263e | 14 | float r = 1.3125; //radius of wheels |
georgezolovick | 0:f9723e66263e | 15 | float l = 4.0625; //width of wheel base |
georgezolovick | 1:b19f21fdf69f | 16 | //For these robots, a positive omega is a right turn and a negative is left |
georgezolovick | 0:f9723e66263e | 17 | float omega = 0.2; //angular velocity of wheels |
georgezolovick | 0:f9723e66263e | 18 | float v = 0.0; //translational velocity of wheels |
georgezolovick | 0:f9723e66263e | 19 | float kp = 1.0; //Proportional control gain |
georgezolovick | 0:f9723e66263e | 20 | |
georgezolovick | 0:f9723e66263e | 21 | float vl = ((omega*l)+(2*v))/(2*r); //Calculates left wheel velocity |
georgezolovick | 0:f9723e66263e | 22 | float vr = ((omega*l)-(2*v))/(2*r); //Calculates right wheel velocity |
georgezolovick | 0:f9723e66263e | 23 | |
georgezolovick | 0:f9723e66263e | 24 | int main() { |
georgezolovick | 0:f9723e66263e | 25 | while(1){ //Forever |
georgezolovick | 0:f9723e66263e | 26 | left.speed(vl); //Move the left wheel |
georgezolovick | 0:f9723e66263e | 27 | right.speed(vr); //Move the right wheel |
georgezolovick | 0:f9723e66263e | 28 | wl = tleft.getSpeed(); //Read the left tachometer |
georgezolovick | 0:f9723e66263e | 29 | wr = tright.getSpeed(); //Read the right tachometer |
georgezolovick | 0:f9723e66263e | 30 | |
georgezolovick | 0:f9723e66263e | 31 | e = wl - wr; //Calculate the error |
georgezolovick | 0:f9723e66263e | 32 | vr = vr + (kp*e); //Readjust the right wheel velocity |
georgezolovick | 0:f9723e66263e | 33 | } |
georgezolovick | 0:f9723e66263e | 34 | } |