File content as of revision 0:72ae4ee22e56:

#include "mbed.h"
#include "Motor.h"
#include "QEI.h"
#define pi (3.14159265)
#define logfreq (100)
#define data_pts (500)
Serial pc(USBTX, USBRX);
Motor m(p25, p23, p27);

QEI enc(p16,p17,NC,1600);



float motspeed;
float x;
float rad;
int count;
Timer t;
float rad_one=0;
float time_one=0;
float omega;
float old_E;
float E;
float PI=3.14159265;
float old_t;
float DC;
float old_DC;
float theta;
//float theta_desired=x;
float Ts=0.01;

int main()
{
    t.reset();
    enc.reset();
    pc.printf("enter an angle\r\n");
    pc.scanf("%f", &x);




    while(1) {
        t.start();
        wait(1/logfreq);
        //if(t.read()-old_t>=Ts)   

        count = enc.getPulses() ;
        theta = -count*((2*pi)/3200) ;
        // Calculate position error
        E=(x*(PI/180))-theta;
        
            // Control algorithm
        DC = ((0.9841*old_DC) + (0.2348*E) - (0.2335*old_E));
        old_E  = E;
        m.speed(DC+0.15);
        old_DC = DC;
        
        if(DC>0 && DC<0.7)
        m.speed(DC+0.1);
        if(E>-1.0)
        m.speed(DC+0.15);
        if (E<0.015 && E>-0.01)
        m.speed(0.0);
        else if(DC<0 && DC>-0.7){
        m.speed(DC-0.34);
        if(E>-1.0)
        m.speed(DC-0.5);
        if (E>-0.015&& E<0.01)
        m.speed(0.0);}
       
        // Age variables
        
        
        old_t  = t.read();


        //omega=(rad-rad_one)/(t-time_one);
        //right is negative, left is positive theta
        
        //if (x<0){
        //  m.speed(0.25);  
        //if(rad==x){
        //m.speed(0);}}
        //if (x>0){
        // m.speed(-0.5);
        //  if(rad==x){
        //m.speed(0);}}
        //rad_one=rad;
        //time_one=t; 
        printf("time: %f  angle: %f  error: %f  DC: %f\n\r", t.read(), theta, E,DC);
    }


    //0.2 for forward torque
    //-0.5 for backward


    //for (servopos=0; servopos<=1.0; servopos+=0.1){
    //for (motspeed= 0.2; motspeed <= 1.0; motspeed += 0.1) {
    //myservo1 = servopos;
    //myservo2 = 1.0 - servopos;
    //printf("Servo 1 position %.2f  Servo 2 position %.3f\n", servopos, (1-servopos));

    //m.speed(motspeed);
    //printf("motor speed is %.2f\n", motspeed);
    //wait(0.0002);
    //m.speed(-1.0);
    //wait(0.002);
    //m.speed(1.0);
}