Diff: Classic_PID.cpp

Revision:

0:3a0d53c1740f

Child:

1:f85ca5649d55

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Classic_PID.cpp	Tue Nov 04 11:57:02 2014 +0000
@@ -0,0 +1,119 @@
+#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;
+
+}
\ No newline at end of file