File content as of revision 0:058dc42250db:

#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();
}