Diff: main.cpp

Revision:

0:29e8568563e4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 26 14:07:38 2018 +0000
@@ -0,0 +1,65 @@
+#include "mbed.h"
+#include "ContinuousServo.h"
+#include "Tach.h"
+
+Tach tLeft(p17,64);
+Tach tRight(p13,64);
+
+ContinuousServo left(p23);
+ContinuousServo right(p26);
+
+float l;
+float r;
+float speedL;
+float speedR;
+float errorL;
+float errorR;
+float sampling_per;
+
+Serial pc(USBTX,USBRX);
+
+float PIpwmL(float desired_speed,float speed); 
+float PIpwmR(float desired_speed,float speed); 
+
+int main() {
+    wait(5);
+    //go straight
+    while(1){
+        
+        speedL = tLeft.getSpeed();
+        speedR = tRight.getSpeed();
+        l = PIpwmL(0.3, speedL);
+        r = PIpwmR(0.3, speedR); 
+        l = r + 0.1;
+        left.speed(l);
+        right.speed(-r);
+        
+        pc.printf("%f,%f\n",speedL,speedR);
+        wait(0.05);
+        r = l + 0.1;
+        speedL = tLeft.getSpeed();
+        speedR = tRight.getSpeed();
+        left.speed(l);
+        right.speed(-r);
+        pc.printf("%f,%f\n",speedL,speedR);
+    }
+}
+
+float PIpwmL(float desired_speed,float speed)
+{
+    float integral_errorL = 0.0;  
+    float sampling_per = 0.05;    
+    float errorL = desired_speed - speed;
+    integral_errorL += (errorL*sampling_per);   
+    float left = (0.07*integral_errorL) + (0.08*errorL) + 0.48;   
+    return left;
+    }
+float PIpwmR(float desired_speed,float speed)
+{
+        float integral_errorR = 0.0;
+        float sampling_per = 0.05;
+        float errorR = desired_speed - speed;
+        integral_errorR += (errorR*sampling_per);
+        float right = (0.07*integral_errorR) + (0.08*errorR) + 0.25;
+        return right;
+    }
\ No newline at end of file