Diff: main.cpp

Revision:

0:058dc42250db

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 11 15:08:27 2019 +0000
@@ -0,0 +1,150 @@
+#include "mbed.h"
+#include "QEI.h"
+
+QEI turn(p26,p27,NC,1600);
+Serial pc(USBTX,USBRX);   // serial comms with PC
+Timer t;
+Ticker tick;
+PwmOut in1(p25);
+DigitalOut in2(p24);
+
+int pulse_count;
+int j;
+float dc = 0.0;
+float dir;
+float ang_curr;
+float ang_old = 0.0;
+float ctrl_period = 0.01;
+float ang_des;
+float dc_old = 0.0;
+float current_time;
+float speed;
+int idx = 0; // counter for figuring out when you reach 20 values of velocity
+float e_dot_arr[20]; //stores 20 values of the error derivative
+float e_dot; //this is your error derivative
+int first = 0;
+float omega;
+float err_int;
+float err_int_old = 0.0;
+float err_ang;
+float err_ang_old = 0.0;
+float deg_ang_curr;
+float deg_err_ang;
+
+void measure()
+{
+    pulse_count = 1.0*turn.getPulses();
+    current_time = t.read();
+    //3200 pulses is one rev for x2
+    ang_curr = (pulse_count /(2.0 * 1600)*(2*3.14159265));
+    //Estimate speed
+
+    omega     = (ang_curr - ang_old)/ctrl_period; //this is change in theta over delta time
+
+//Velocity filter for derivative term
+    if(idx <= 19) {
+        e_dot_arr[idx] = omega; //keep storing values in your array
+        idx = idx+1;
+    } else {
+        idx = 0; //start over with the first entry (rewrite old ones)
+        e_dot_arr[idx] = omega;
+    }
+    if (first == 0) {
+
+        if (idx >= 1) {
+            e_dot = 0.0; //this checks to see if it's the first time the array is populated
+            for (j=0; j <= idx; j++) {
+                e_dot = e_dot + e_dot_arr[j];
+            }
+
+        } else {
+            e_dot = e_dot_arr[idx];
+        }
+    } else {
+        first = 1;  //set the value to first to 1 so that it starts to just overwrite old values in the array
+    }
+    for (j=0; j<=19; j++) {
+        e_dot = e_dot + e_dot_arr[j];
+    }
+
+    e_dot = 0.0 - e_dot;
+
+    //ERRORS//
+
+    err_ang = -ang_curr + ang_des;
+
+    err_int = err_int_old + ((err_ang_old + err_ang)/2)*ctrl_period;
+
+    //err_der = (err_ang- err_ang_old)/ctrl_period
+
+    //compute duty cycle for motor (if necessary)
+
+    ////PID////
+    dc = 1.6*err_ang + (0.75*err_int) + (0.000*e_dot); //Kp = .43 0.0418 .35
+    //dc= -dc;
+
+    
+    //Age variable
+    err_ang_old = err_ang;
+    err_int_old = err_int;
+
+    
+
+    //send duty cycle to motor
+   if(dc>0.0) {
+        dir = 1.0;
+    } else if(dc<0.0) {
+        dir = 0.0;
+    }
+    if(dc>1.0) {
+        dc = 1.0;
+    } else if(dc<-1.0) {
+        dc = 1.0;
+    } else if (dc<0.0){
+        dc = abs(dc);
+        }
+    
+    //Age variable
+    //dc_old = dc;
+    in1 = dc;
+    in2 = dir;
+    
+    deg_ang_curr = ang_curr*180.0/3.14159265;
+    deg_err_ang= err_ang*180.0/3.14159265;
+
+    printf("%f,%f,%f,%f,%f\n\r", current_time,deg_ang_curr,deg_err_ang,dc,dir);
+
+
+}
+
+int main()
+{
+    in1.period(1.0/20.0e3);
+    pc.baud(115200); // increase baud rate
+    pc.format(8,SerialBase::Even,1);
+    turn.reset();
+
+    in1 = dc;
+    in2 = dir;
+
+    printf("Enter a Position:");
+    pc.scanf("%f",&ang_des); //take in desired position
+
+    //printf("Enter a direction 0 or 1:");
+    //pc.scanf("%f",&dir); //take in direction
+
+    in1 = dc;
+    in2 = dir;
+
+    tick.attach(&measure, 0.01);
+
+
+
+    while(t.read()<10.0) {
+        t.start();
+
+
+    }
+    t.stop();
+    tick.detach();
+}