File content as of revision 6:0832c6c6bcba:

#include "mbed.h"
#include "encoder.h"
//#include "MODSERIAL.h"
#include "HIDScope.h"


#define TSAMP 0.01
#define K_P (10     *TSAMP)
#define K_I (0.00   *TSAMP)
#define K_D (5      *TSAMP)

#define POT_AVG 50

void coerce(float * in, float min, float max);
float pid(float setpoint, float measurement);
AnalogIn potmeter(PTC2);
volatile bool looptimerflag;
float potsamples[POT_AVG];
HIDScope scope(4);


void setlooptimerflag(void)
{
    looptimerflag = true;
}

void potAverager(void)
{
    static uint16_t sample_index = 0;
    float voltage = potmeter.read()-.5;

    potsamples[sample_index] = voltage;

    sample_index++;
    if(sample_index >= POT_AVG)
        sample_index = 0;
}

float getpotAverage(void)
{
    uint16_t valuecounter;
    float sum = 0;
    for(valuecounter = 0 ; valuecounter < POT_AVG ; valuecounter++)
     {
         sum += potsamples[valuecounter];
     }
    return sum / (POT_AVG*1.);
}

int main()
{
    //start Encoder
    Encoder motor1(PTD0,PTC9);
    //Ticker to average potmeter values
    Ticker potaverage;
    //MODSERIAL pc(USBTX,USBRX);
    PwmOut pwm_motor(PTA12);
    //10kHz PWM frequency
    pwm_motor.period_us(100);
    DigitalOut motordir(PTD3);
    potaverage.attach(potAverager,0.002);
    //pc.baud(921600);
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,TSAMP);
    while(1) {
        float setpoint;
        float new_pwm;
        while(!looptimerflag);
        looptimerflag = false;
        setpoint = (getpotAverage())*2000;
        //new_pwm = (setpoint - motor1.getPosition())*.001;
        new_pwm = pid(setpoint, motor1.getPosition());
        coerce(&new_pwm, -1,1);
        scope.set(0, setpoint);
        scope.set(1,new_pwm);
        // ch 2 and 3 set in pid controller */
        scope.send();
        //pc.printf("%f %f %f 0\n", setpoint/2000.+0.5, motor1.getPosition()/2000.+.5,new_pwm/2.+0.5);
        if(new_pwm > 0)
            motordir = 1;
        else
            motordir = 0;
        pwm_motor.write(abs(new_pwm));
    }
}


//coerces 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 coerce(float * in, float min, float max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}


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;
    coerce(&out_i,-0.5,0.5);
    prev_error = error;
    scope.set(2,error);
    scope.set(3,out_p);
    return out_p + out_i + out_d;
}