PID.cpp

00001 #include "PID.h"
00002 
00003 PIDController::PIDController(float p_gain, float d_gain, float i_gain) {
00004     kp = p_gain;
00005     kd = d_gain;
00006     ki = i_gain;
00007 
00008     command = 0.0;
00009     error = 0.0;
00010     old_error = 0.0;
00011     integral_error = 0.0;
00012     
00013     current_torque = 0.0;
00014     torque_command = 0.0;
00015     torque_error = 0.0;
00016     torque_integral_error = 0.0;
00017     
00018     direction = 0;
00019 }
00020 
00021 void PIDController::set_command(float command) {
00022     this->command = command;
00023     this->torque_command = command;
00024     this->integral_error = 0.0;
00025     this->torque_integral_error = 0.0;
00026 }
00027 
00028 //voltage mode position control
00029 //This function is called to set the desired position of the servo
00030 float PIDController::command_position(float current_position) {
00031     this->error = (this->command - current_position);
00032     this->integral_error += this->error;
00033     if (this->integral_error > 1.0)
00034         this->integral_error = 1.0;
00035     else if (this->integral_error < -1.0)
00036         this->integral_error = -1.0;
00037     float output = (this->kp)*(this->error) + (this->kd)*(this->error - this->old_error) + (this->ki)*(this->integral_error);
00038     this->old_error = this->error;
00039     return output;
00040 }
00041 
00042 // Torque Mode position control
00043 float PIDController::command_position_tm(float current_position, float current_current) {
00044     float output = this->command_position(current_position);
00045     output += this->command_torque(current_current);
00046     return output;
00047 }
00048 
00049 // Given a current current in abs(Amps), produce a PWM command between -1.0 (full reverse) and 1.0 (full forward)
00050 float PIDController::command_torque(float current_current) {
00051     if (this->direction != 0){
00052         this->current_torque = this->direction*(current_current);
00053     } else {
00054         this->current_torque = current_current;
00055     }
00056     
00057     float average_torque = 0;
00058    
00059     for (int i = 0; i < 4; i++){
00060         this->torque_history[i] = this->torque_history[i+1];
00061         average_torque += this->torque_history[i];
00062     }
00063     average_torque += current_torque;
00064     average_torque = average_torque/5;
00065     //average_torque = current_torque; // Does this just overwrite the average?
00066     this->torque_history[4] = current_torque;
00067     
00068     this->torque_error = (this->torque_command - average_torque);
00069 
00070     this->torque_integral_error += this->torque_error;
00071     
00072     float output = 4.0*this->torque_command + this->kp*this->torque_error + this->ki*this->torque_integral_error;
00073     
00074     // Set direction based on output
00075     if (output > 0){
00076         this->direction = 1;
00077     } else if(output < 0){
00078         this->direction = -1;
00079     } else{
00080         output = 0;
00081     }
00082    
00083     // Coerce ouptut to be between -1 and 1
00084     if (output > 1){
00085         output = 1;
00086     } else if (output < -1){
00087         output = -1;
00088     }
00089     
00090     return output;
00091 }