Diff: main.cpp

Revision:

0:f9723e66263e

Child:

1:b19f21fdf69f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 25 03:08:02 2018 +0000
@@ -0,0 +1,33 @@
+#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
+    }
+}
\ No newline at end of file