Jacob Kang
/
EW309_turrettest
EW309
Diff: main.cpp
- Revision:
- 0:058dc42250db
diff -r 000000000000 -r 058dc42250db main.cpp --- /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(); +}