Diff: main.cpp

Revision:

0:e6c8377a69d0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 04 18:01:56 2019 +0000
@@ -0,0 +1,81 @@
+#include "mbed.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <QEI.h>
+
+
+DigitalOut led1(LED1);
+PwmOut pwm(p25);
+DigitalOut dir(p27);
+Serial pc(USBTX,USBRX);
+QEI turret(p17, p18, NC, 1600);
+Timer t;
+
+float pulses;
+float revs;
+float dc;
+float theta;
+float err = 0.0;
+float err_old = 0.0;
+float err_ancient = 0.0;
+float dc_old = 0.0;
+float dc_ancient = 0.0;
+float des_theta=0.0;
+
+//float t;
+//float t_2;
+int yes;
+
+Ticker Controller;
+void CtrCode() {
+    pulses = turret.getPulses();
+    revs = -pulses / 3200.0;
+    theta = revs*360.0;
+
+    err = des_theta-theta;
+
+    dc = 1.9703*dc_old - 0.9703*dc_ancient + .1471*err- 0.293*err_old + 0.146*err_ancient;    //1.9703*dc_old - 0.9703*dc_ancient + .1471*err - 0.293*err_old + 0.146*err_ancient;
+
+    err_ancient = err_old;
+    err_old = err;
+    dc_ancient = dc_old;
+    dc_old = dc;
+
+    if (dc>0)
+    {
+        dir = 1;
+    } else
+    {
+        dir = 0;
+    }
+    
+    pwm.write(abs(dc));
+
+}
+
+
+
+int main()
+{
+    float pwm_now;
+
+    pc.baud(9600); // set baud rate
+
+    dir=1;
+    pwm.period(1.0/(20*10^3));
+    pc.printf("start?");
+    pc.scanf("%d",&yes);
+    Controller.attach(&CtrCode,.0083);
+    des_theta = 90.0;
+
+    while(1) {
+        pc.printf("Error: %.3f, DC: %.3f\n", err, dc);
+        
+        
+        
+    }
+
+
+
+
+}
\ No newline at end of file