File content as of revision 0:a859fb8b0f6b:

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


}