mit
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