jiexuan fan / ServoController_2

mit

Dependencies:   QEI mbed-src

PID.cpp@4:5ae9f8b3a16f, 2015-11-24 (annotated)

Committer:
abuchan
Date:
Tue Nov 24 03:56:22 2015 +0000
Revision:
4:5ae9f8b3a16f
Decoupled position and current control working.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
abuchan 4:5ae9f8b3a16f 1 #include "PID.h"
abuchan 4:5ae9f8b3a16f 2
abuchan 4:5ae9f8b3a16f 3 PIDController::PIDController(float p_gain, float d_gain, float i_gain) {
abuchan 4:5ae9f8b3a16f 4 kp = p_gain;
abuchan 4:5ae9f8b3a16f 5 kd = d_gain;
abuchan 4:5ae9f8b3a16f 6 ki = i_gain;
abuchan 4:5ae9f8b3a16f 7
abuchan 4:5ae9f8b3a16f 8 command = 0.0;
abuchan 4:5ae9f8b3a16f 9 error = 0.0;
abuchan 4:5ae9f8b3a16f 10 old_error = 0.0;
abuchan 4:5ae9f8b3a16f 11 integral_error = 0.0;
abuchan 4:5ae9f8b3a16f 12
abuchan 4:5ae9f8b3a16f 13 current_torque = 0.0;
abuchan 4:5ae9f8b3a16f 14 torque_command = 0.0;
abuchan 4:5ae9f8b3a16f 15 torque_error = 0.0;
abuchan 4:5ae9f8b3a16f 16 torque_integral_error = 0.0;
abuchan 4:5ae9f8b3a16f 17
abuchan 4:5ae9f8b3a16f 18 direction = 0;
abuchan 4:5ae9f8b3a16f 19 }
abuchan 4:5ae9f8b3a16f 20
abuchan 4:5ae9f8b3a16f 21 void PIDController::set_command(float command) {
abuchan 4:5ae9f8b3a16f 22 this->command = command;
abuchan 4:5ae9f8b3a16f 23 this->torque_command = command;
abuchan 4:5ae9f8b3a16f 24 this->integral_error = 0.0;
abuchan 4:5ae9f8b3a16f 25 this->torque_integral_error = 0.0;
abuchan 4:5ae9f8b3a16f 26 }
abuchan 4:5ae9f8b3a16f 27
abuchan 4:5ae9f8b3a16f 28 //voltage mode position control
abuchan 4:5ae9f8b3a16f 29 //This function is called to set the desired position of the servo
abuchan 4:5ae9f8b3a16f 30 float PIDController::command_position(float current_position) {
abuchan 4:5ae9f8b3a16f 31 this->error = (this->command - current_position);
abuchan 4:5ae9f8b3a16f 32 this->integral_error += this->error;
abuchan 4:5ae9f8b3a16f 33 if (this->integral_error > 1.0)
abuchan 4:5ae9f8b3a16f 34 this->integral_error = 1.0;
abuchan 4:5ae9f8b3a16f 35 else if (this->integral_error < -1.0)
abuchan 4:5ae9f8b3a16f 36 this->integral_error = -1.0;
abuchan 4:5ae9f8b3a16f 37 float output = (this->kp)*(this->error) + (this->kd)*(this->error - this->old_error) + (this->ki)*(this->integral_error);
abuchan 4:5ae9f8b3a16f 38 this->old_error = this->error;
abuchan 4:5ae9f8b3a16f 39 return output;
abuchan 4:5ae9f8b3a16f 40 }
abuchan 4:5ae9f8b3a16f 41
abuchan 4:5ae9f8b3a16f 42 // Torque Mode position control
abuchan 4:5ae9f8b3a16f 43 float PIDController::command_position_tm(float current_position, float current_current) {
abuchan 4:5ae9f8b3a16f 44 float output = this->command_position(current_position);
abuchan 4:5ae9f8b3a16f 45 output += this->command_torque(current_current);
abuchan 4:5ae9f8b3a16f 46 return output;
abuchan 4:5ae9f8b3a16f 47 }
abuchan 4:5ae9f8b3a16f 48
abuchan 4:5ae9f8b3a16f 49 // Given a current current in abs(Amps), produce a PWM command between -1.0 (full reverse) and 1.0 (full forward)
abuchan 4:5ae9f8b3a16f 50 float PIDController::command_torque(float current_current) {
abuchan 4:5ae9f8b3a16f 51 if (this->direction != 0){
abuchan 4:5ae9f8b3a16f 52 this->current_torque = this->direction*(current_current);
abuchan 4:5ae9f8b3a16f 53 } else {
abuchan 4:5ae9f8b3a16f 54 this->current_torque = current_current;
abuchan 4:5ae9f8b3a16f 55 }
abuchan 4:5ae9f8b3a16f 56
abuchan 4:5ae9f8b3a16f 57 float average_torque = 0;
abuchan 4:5ae9f8b3a16f 58
abuchan 4:5ae9f8b3a16f 59 for (int i = 0; i < 4; i++){
abuchan 4:5ae9f8b3a16f 60 this->torque_history[i] = this->torque_history[i+1];
abuchan 4:5ae9f8b3a16f 61 average_torque += this->torque_history[i];
abuchan 4:5ae9f8b3a16f 62 }
abuchan 4:5ae9f8b3a16f 63 average_torque += current_torque;
abuchan 4:5ae9f8b3a16f 64 average_torque = average_torque/5;
abuchan 4:5ae9f8b3a16f 65 //average_torque = current_torque; // Does this just overwrite the average?
abuchan 4:5ae9f8b3a16f 66 this->torque_history[4] = current_torque;
abuchan 4:5ae9f8b3a16f 67
abuchan 4:5ae9f8b3a16f 68 this->torque_error = (this->torque_command - average_torque);
abuchan 4:5ae9f8b3a16f 69
abuchan 4:5ae9f8b3a16f 70 this->torque_integral_error += this->torque_error;
abuchan 4:5ae9f8b3a16f 71
abuchan 4:5ae9f8b3a16f 72 float output = 4.0*this->torque_command + this->kp*this->torque_error + this->ki*this->torque_integral_error;
abuchan 4:5ae9f8b3a16f 73
abuchan 4:5ae9f8b3a16f 74 // Set direction based on output
abuchan 4:5ae9f8b3a16f 75 if (output > 0){
abuchan 4:5ae9f8b3a16f 76 this->direction = 1;
abuchan 4:5ae9f8b3a16f 77 } else if(output < 0){
abuchan 4:5ae9f8b3a16f 78 this->direction = -1;
abuchan 4:5ae9f8b3a16f 79 } else{
abuchan 4:5ae9f8b3a16f 80 output = 0;
abuchan 4:5ae9f8b3a16f 81 }
abuchan 4:5ae9f8b3a16f 82
abuchan 4:5ae9f8b3a16f 83 // Coerce ouptut to be between -1 and 1
abuchan 4:5ae9f8b3a16f 84 if (output > 1){
abuchan 4:5ae9f8b3a16f 85 output = 1;
abuchan 4:5ae9f8b3a16f 86 } else if (output < -1){
abuchan 4:5ae9f8b3a16f 87 output = -1;
abuchan 4:5ae9f8b3a16f 88 }
abuchan 4:5ae9f8b3a16f 89
abuchan 4:5ae9f8b3a16f 90 return output;
abuchan 4:5ae9f8b3a16f 91 }