File content as of revision 0:3a0d53c1740f:

#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 > 1.0)
        _output = 1.0;              
    if(_output < -1.0)
        _output = -1.0; 
        
    // 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 
    float Intergral = _Ki * _sumError;                                 // Calculate the Intergral part
    
    // Calculate the output
    _output = (_setPoint * _kvelff) + Proportional + Intergral; 
        
    // Check for wind-up
    if(_output > 1.0)
        _output = 1.0;              
    if(_output < -1.0)
        _output = -1.0; 

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

void Classic_PID::setKvelff(float Kvelff)
{

    _kvelff = Kvelff / 500000;

}