Diff: PID.cpp

Revision:

4:5ae9f8b3a16f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Tue Nov 24 03:56:22 2015 +0000
@@ -0,0 +1,91 @@
+#include "PID.h"
+
+PIDController::PIDController(float p_gain, float d_gain, float i_gain) {
+    kp = p_gain;
+    kd = d_gain;
+    ki = i_gain;
+
+    command = 0.0;
+    error = 0.0;
+    old_error = 0.0;
+    integral_error = 0.0;
+    
+    current_torque = 0.0;
+    torque_command = 0.0;
+    torque_error = 0.0;
+    torque_integral_error = 0.0;
+    
+    direction = 0;
+}
+
+void PIDController::set_command(float command) {
+    this->command = command;
+    this->torque_command = command;
+    this->integral_error = 0.0;
+    this->torque_integral_error = 0.0;
+}
+
+//voltage mode position control
+//This function is called to set the desired position of the servo
+float PIDController::command_position(float current_position) {
+    this->error = (this->command - current_position);
+    this->integral_error += this->error;
+    if (this->integral_error > 1.0)
+        this->integral_error = 1.0;
+    else if (this->integral_error < -1.0)
+        this->integral_error = -1.0;
+    float output = (this->kp)*(this->error) + (this->kd)*(this->error - this->old_error) + (this->ki)*(this->integral_error);
+    this->old_error = this->error;
+    return output;
+}
+
+// Torque Mode position control
+float PIDController::command_position_tm(float current_position, float current_current) {
+    float output = this->command_position(current_position);
+    output += this->command_torque(current_current);
+    return output;
+}
+
+// Given a current current in abs(Amps), produce a PWM command between -1.0 (full reverse) and 1.0 (full forward)
+float PIDController::command_torque(float current_current) {
+    if (this->direction != 0){
+        this->current_torque = this->direction*(current_current);
+    } else {
+        this->current_torque = current_current;
+    }
+    
+    float average_torque = 0;
+   
+    for (int i = 0; i < 4; i++){
+        this->torque_history[i] = this->torque_history[i+1];
+        average_torque += this->torque_history[i];
+    }
+    average_torque += current_torque;
+    average_torque = average_torque/5;
+    //average_torque = current_torque; // Does this just overwrite the average?
+    this->torque_history[4] = current_torque;
+    
+    this->torque_error = (this->torque_command - average_torque);
+
+    this->torque_integral_error += this->torque_error;
+    
+    float output = 4.0*this->torque_command + this->kp*this->torque_error + this->ki*this->torque_integral_error;
+    
+    // Set direction based on output
+    if (output > 0){
+        this->direction = 1;
+    } else if(output < 0){
+        this->direction = -1;
+    } else{
+        output = 0;
+    }
+   
+    // Coerce ouptut to be between -1 and 1
+    if (output > 1){
+        output = 1;
+    } else if (output < -1){
+        output = -1;
+    }
+    
+    return output;
+}
\ No newline at end of file