Zolovick_Larrea_Mouaffek / Mbed 2 deprecated ES202_Final_Turn

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?

UserRevisionLine numberNew 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 }