Diff: main.cpp

Revision:

0:a859fb8b0f6b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 04 17:53:54 2019 +0000
@@ -0,0 +1,103 @@
+#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(){
+ 
+ 
+ pc.printf("Pulses is: %i\n\r", turret.getPulses());
+ pc.printf("State is: %i\n\r", turret.getCurrentState());
+ pc.printf("State is: %i\n\r", turret.getRevolutions());
+    }*/
+
+int main()
+{
+    float t_now;
+    float pwm_now;
+
+    pc.baud(9600); // set baud rate
+
+    dir=1;
+    pwm.period(1.0/(20*10^3));
+    //pwm.write(abs(dc));
+    //reader.attach(&reading, 0.5);
+    while(1) {
+        pc.printf("start?");
+        pc.scanf("%d",&yes);
+
+        t.start();
+        //pwm_now = 0.5;
+        //pwm.write(pwm_now);
+        while( t.read() <= 20 ) {
+            /*if (t.read() <= 5 ){
+                des_angle = 45;
+                }
+            if (t.read() <= 10 && t.read() >5 ){
+                des_angle = 90;
+                }
+            if (t.read() <= 15 && t.read()>10){
+                des_angle = 125;
+                }
+             if (t.read()<= 20 && t.read() >15){
+                des_angle = 180;
+                }*/
+            des_theta = 45.0;
+            pulses = turret.getPulses();
+            revs = -pulses / 3200.0;
+            theta = revs*360.0;
+
+            err = des_theta-theta;
+
+            dc = 1.9703*dc_old - 0.9703*dc_ancient + 0.01471*err - 0.0293*err_old + 0.0146*err_ancient;
+                        
+            err_ancient = err_old;
+            err_old = err;
+            dc_ancient = dc_old;
+            dc_old = dc;
+
+
+            t_now = t.read();
+
+            if (dc>0) {
+                dir = 1;
+            } else {
+                dir = 0;
+            }
+            pc.printf("Error: %.3f, DC: %.3f\n", err, dc);
+            pwm.write(abs(dc));
+            
+        }
+
+
+
+
+
+        wait(5);
+    }
+
+
+}
\ No newline at end of file