File content as of revision 2:f78e083a20c4:

#include "mbed.h"
#include "Classic_PID.h"

// Create object
Classic_PID::Classic_PID(float Kp, float Ki, float Kd, float kvelff)
{

    // Save paramaters
    _Kp = Kp;
    _Ki = Ki;
    _Kd = Kd;
    _kvelff = kvelff;
}

// Set Methods
void Classic_PID::setSetPoint(float sp)
{

    _setPoint = sp;

}

void Classic_PID::setProcessValue(float pv)
{

    _processVariable = pv;

}

float Classic_PID::compute(void)
{
    int _error = _setPoint - _processVariable;                      // Work out the instantaneous error
    
    // Calculate the individual parts of the PID algorithm
    float Proportional = _Kp * (_error - _lastError);               // Calculate the Proportional part
    //_sumError += _error;                                          // Add this error to the sum of errors 
    float Intergral = _Ki * _error;                                 // Calculate the Intergral part
    
    // Calculate the output
    _output = _lastOutput + Proportional + Intergral; 
        
    // Check for wind-up
    if(_output > _maxLimit)
        _output = _maxLimit;              
    if(_output < _minLimit)
        _output = _minLimit; 
        
    // Save the outputs for the next time round the loop
    _lastOutput = _output;                                          // Save the ouput 
    _lastError = _error;

    // Return the new output
    return(_output);                                                // Return the PI loop output (duty cycle)
}

void Classic_PID::setKp(float Kp)
{

    _Kp = Kp;

}

void Classic_PID::setKi(float Ki)
{

    _Ki = Ki;
    _sumError = 0;

}

float Classic_PID::getKp(void)
{

    return(_Kp);

}

float Classic_PID::getKi(void)
{

    return(_Ki);

}

float Classic_PID::getKvelff(void)
{

    return(_kvelff);

}

float Classic_PID::compute_ff(void)
{
    int _error = _setPoint - _processVariable;                      // Work out the instantaneous error
    
    // Calculate the individual parts of the PID algorithm
    float Proportional = _Kp * _error;                              // Calculate the Proportional part
    _sumError += _error;                                            // Add this error to the sum of errors 
    if(_sumError > 1000)
        _sumError = 1000;
    float Intergral = _Ki * _sumError;                                 // Calculate the Intergral part
    
    // Calculate the output
    _output = ((1 / _setPoint) * _kvelff) + Proportional + Intergral; 
        
    // Check for wind-up
    if(_output > _maxLimit)
        _output = _maxLimit;              
    if(_output < _minLimit)
        _output = _minLimit; 
        
    // Return the new output
    return(_output);                                                // Return the PI loop output (duty cycle)
}

void Classic_PID::setKvelff(float Kvelff)
{

    _kvelff = Kvelff / 500000;

}

void Classic_PID::setProcessLimits(float maxLimit, float minLimit)
{

    _maxLimit = maxLimit;
    _minLimit = minLimit;

}

float Classic_PID::getSetPoint(void)
{

    return(_setPoint);

}