File content as of revision 0:4cf5365df85c:

#include "mbed.h"
#include <iostream>

using namespace std;

//constants
const float scalar = 0.6;

//ticker to compute output
Ticker sample;

//System IO
AnalogIn setPoint(PTB0);
AnalogIn speed(PTB2);
PwmOut gateDrive(PTB1);

//moving average filter
float* samples;

//PI controller
double Input, Output, Setpoint;
double errSum;
double ITerm;
double kp, ki;

//saturation limits
double outMin;
double outMax;


double sampling_frequency;
double ti;

void set_tunings(double KP, double KI)
{
    kp = KP;
    ki = KI;
}


/*
Length 8 moving average filter for noisy inputs
*/
float filter_input(AnalogIn input)
{  
    int filterLength = 8;
   for(int i = filterLength -1; i > 0; i--)
   {
      samples[i] = samples[i-1]; 
   }
   samples[0] = input.read();
   float sum = 0.0;
   for(int j = 0; j < filterLength; j++)
   {
       sum += samples[j];
   }
   return sum/filterLength;   
}

void update_states()
{
    Input = speed.read();
    Setpoint = filter_input(setPoint);
//    Setpoint = setPoint.read();
//   test step response
//  Setpoint = 1/3.3;
}

void compute()
{
    //proportional
    double error = Setpoint-Input;
    
    //integral
    errSum +=(error * ti);//potentially the problem
    ITerm = ki*errSum;
    if(ITerm > outMax) ITerm = outMax;
    if(ITerm < outMin) ITerm = outMin; 
    
    //compute output
    Output = kp*error + ITerm;
    if(Output > outMax) Output = outMax;
    if(Output < outMin) Output = outMin;    
}

int main()
{
   outMin = 0.0;
   outMax = 1.0;
   
   sampling_frequency = 1e3;
   ti = 1/sampling_frequency;
   
   set_tunings(0.1414,19.7408);//Kp, Ki
   
   samples = new float[8];
   
   sample.attach(&compute, ti);
   
   while(true)
   {
    update_states();
    gateDrive = scalar*Output;
   }
   
}