File content as of revision 0:e89ba43e3f05:

#include "mbed.h"
#include "encoder.h"

#include <iostream>

Encoder encoderA(PTD0,PTD2);

PwmOut pwm(PTA5);
DigitalOut dir(PTA4);


int32_t enca = 0,enca1 =0,enca2 =0,counts=0, encb;
float speed = 0,v=0;
int n=0,a =1 ,b =0;

float pid(float setpoint, float measurement)
{
    float error;
    static float prev_error = 0;
    float           out_p = 0;
    static float    out_i = 0;
    float           out_d = 0;
    error  = setpoint-measurement;
    out_p  = error*K_P;
    out_i += error*K_I;
    out_d  = (error-prev_error)*K_D;
    clamp(&out_i,-I_LIMIT,I_LIMIT);
    prev_error = error;
    scope.set(1,out_p);
    scope.set(2,out_i);
    scope.set(3,out_d);
    return out_p + out_i + out_d;
}
/*
int main()
{
    speed = 0.09;
    pwm = speed;
    while(1){
        wait(0.2);
        enca = encoderA.getPosition();
        cout<<enca<<endl;
        }
}
*/

int main()
{
    speed = 0.3;
    pwm = speed;
   
    while(1){
    wait(0.5);
    enca = encoderA.getPosition();
   
    enca2 = enca1;
    enca1 = enca;
    
    counts = (enca1 - enca2)/0.5;
    v = (counts)*((2*3.14159265359)/1550);
    
    cout<<"v: "<<v<<endl;
    

    
    }
   
    
    
    

}
//clamps value 'in' to min or max when exceeding those values
//if you'd like to understand the statement below take a google for
//'ternary operators'.
void clamp(float * in, float min, float max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}