jiexuan fan / Mbed 2 deprecated ServoController

mit

Dependencies:   mbed QEI

main.cpp@4:4e7b392ed0aa, 2015-03-18 (annotated)

Committer:
matthieulap
Date:
Wed Mar 18 17:17:58 2015 +0000
Revision:
4:4e7b392ed0aa
Parent:
3:cae0b305d54c
Child:
5:75b5c24e36ad
small refactor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 2:89bb6272869b 1 //Ben Katz, 2013
benkatz 2:89bb6272869b 2
aberk 0:bcff39fac858 3 #include "QEI.h"
benkatz 2:89bb6272869b 4 #include "PID.h"
benkatz 2:89bb6272869b 5
benkatz 2:89bb6272869b 6 #define pi 3.14159265358979323846
benkatz 2:89bb6272869b 7
aberk 0:bcff39fac858 8
aberk 0:bcff39fac858 9 Serial pc(USBTX, USBRX);
benkatz 3:cae0b305d54c 10
benkatz 3:cae0b305d54c 11 AnalogIn p_pot (p15);
benkatz 3:cae0b305d54c 12 AnalogIn d_pot (p16);
benkatz 3:cae0b305d54c 13 AnalogIn a_pot (p17);
benkatz 2:89bb6272869b 14
benkatz 2:89bb6272869b 15 PwmOut Motor1(p21); //Ouput pin to h-bridge 1
benkatz 2:89bb6272869b 16 PwmOut Motor2 (p22); //Output pin to h-bridge 2
benkatz 2:89bb6272869b 17 AnalogIn Current (p20); //Current sense pin
benkatz 2:89bb6272869b 18
benkatz 2:89bb6272869b 19 int steps_per_rev = 1200; //Encoder CPR * gearbox ratio
benkatz 2:89bb6272869b 20 Timer timer;
benkatz 2:89bb6272869b 21
benkatz 2:89bb6272869b 22
matthieulap 4:4e7b392ed0aa 23 // Setup parameters
matthieulap 4:4e7b392ed0aa 24 float volt2amp = 3.3 / .525; //(0-3.3V input) * (voltage divider muiltiplier) / (.525 V/A)
matthieulap 4:4e7b392ed0aa 25 float PWM_frequency = 20000.0; // MC33926 h-bridge limited 20 kHz (.00005 seconds)
matthieulap 4:4e7b392ed0aa 26
matthieulap 4:4e7b392ed0aa 27 //Use X4 encoding.
matthieulap 4:4e7b392ed0aa 28 QEI wheel(p13, p14, NC, steps_per_rev, QEI::X4_ENCODING); //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2))
matthieulap 4:4e7b392ed0aa 29
benkatz 2:89bb6272869b 30
benkatz 2:89bb6272869b 31 float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians
benkatz 2:89bb6272869b 32 {
benkatz 2:89bb6272869b 33 return ((pulses/steps_per_rev)*(2*pi));
benkatz 2:89bb6272869b 34 }
benkatz 2:89bb6272869b 35
benkatz 2:89bb6272869b 36 float pulsesToDegrees(float pulses) //Encoder steps to revolutions of output shaft in degrees
benkatz 2:89bb6272869b 37 {
benkatz 3:cae0b305d54c 38
benkatz 2:89bb6272869b 39 return ((pulses/steps_per_rev)*360);
benkatz 2:89bb6272869b 40 }
benkatz 2:89bb6272869b 41
benkatz 2:89bb6272869b 42
benkatz 2:89bb6272869b 43 void signal_to_hbridge( float signal) //Input of -1 means full reverse, 1 means full forward
benkatz 2:89bb6272869b 44 { //An input of magnitude > 1 gets reduced to 1 (or -1) by the pwm.write() function
benkatz 2:89bb6272869b 45 if (signal < 0) {
benkatz 2:89bb6272869b 46 Motor1.write(0);
benkatz 2:89bb6272869b 47 Motor2.write(signal * -1);
benkatz 2:89bb6272869b 48 } else if (signal > 0) {
benkatz 2:89bb6272869b 49 Motor1.write(signal);
benkatz 2:89bb6272869b 50 Motor2.write(0);
benkatz 2:89bb6272869b 51 } else {
benkatz 2:89bb6272869b 52 Motor1.write(0);
benkatz 2:89bb6272869b 53 Motor2.write(0);
benkatz 2:89bb6272869b 54 }
benkatz 2:89bb6272869b 55 }
aberk 0:bcff39fac858 56
benkatz 3:cae0b305d54c 57
benkatz 3:cae0b305d54c 58
benkatz 2:89bb6272869b 59 void pulse_motor(float pulse_time, float pulse_interval) //Usefull for testing peak torque.
benkatz 2:89bb6272869b 60 //pulse_time = motor on time. pulse_interval = motor off time
matthieulap 4:4e7b392ed0aa 61 {
matthieulap 4:4e7b392ed0aa 62 timer.start();
benkatz 3:cae0b305d54c 63
benkatz 2:89bb6272869b 64 while (timer.read() < pulse_time){
matthieulap 4:4e7b392ed0aa 65 Motor1.write(1);
matthieulap 4:4e7b392ed0aa 66 Motor2.write(0);
matthieulap 4:4e7b392ed0aa 67 //Prints current draw to PC
matthieulap 4:4e7b392ed0aa 68 printf("%F", (Current.read())*volt2amp); //Arithmetic converts the analog input to Amps
matthieulap 4:4e7b392ed0aa 69 printf("\n");
benkatz 2:89bb6272869b 70 }
matthieulap 4:4e7b392ed0aa 71
benkatz 2:89bb6272869b 72 timer.stop();
benkatz 2:89bb6272869b 73 timer.reset();
matthieulap 4:4e7b392ed0aa 74
benkatz 2:89bb6272869b 75 wait(pulse_time);
matthieulap 4:4e7b392ed0aa 76
benkatz 2:89bb6272869b 77 Motor1.write(0);
benkatz 2:89bb6272869b 78 Motor2.write(0);
matthieulap 4:4e7b392ed0aa 79
benkatz 2:89bb6272869b 80 wait(pulse_interval);
matthieulap 4:4e7b392ed0aa 81 }
benkatz 2:89bb6272869b 82
benkatz 2:89bb6272869b 83
aberk 0:bcff39fac858 84
benkatz 2:89bb6272869b 85 int main()
benkatz 2:89bb6272869b 86 {
benkatz 3:cae0b305d54c 87
matthieulap 4:4e7b392ed0aa 88 Motor1.period(1 / PWM_frequency); //Set PWM frequency. MC33926 h-bridge limited 20 kHz (.00005 seconds)
matthieulap 4:4e7b392ed0aa 89 Motor2.period(1 / PWM_frequency);
matthieulap 4:4e7b392ed0aa 90
matthieulap 4:4e7b392ed0aa 91 float desired_position = 40;
matthieulap 4:4e7b392ed0aa 92 float torque = 0.1;
matthieulap 4:4e7b392ed0aa 93 float position_P_gain = 0.1;
matthieulap 4:4e7b392ed0aa 94 float position_D_gain = 3;
matthieulap 4:4e7b392ed0aa 95 float position_I_gain = 0;
matthieulap 4:4e7b392ed0aa 96 float torque_P_gain = 0.0007;
matthieulap 4:4e7b392ed0aa 97 float torque_I_gain = 0.0000001;
matthieulap 4:4e7b392ed0aa 98
matthieulap 4:4e7b392ed0aa 99
matthieulap 4:4e7b392ed0aa 100 PIDController controller(desired_position, torque, position_P_gain, position_D_gain, position_I_gain, torque_P_gain, torque_I_gain);
benkatz 3:cae0b305d54c 101 //timer.start();
matthieulap 4:4e7b392ed0aa 102
matthieulap 4:4e7b392ed0aa 103 wait(1.5);
matthieulap 4:4e7b392ed0aa 104
benkatz 3:cae0b305d54c 105 printf("%F", pulsesToDegrees(wheel.getPulses()));
benkatz 3:cae0b305d54c 106 printf("\n\r");
matthieulap 4:4e7b392ed0aa 107
benkatz 3:cae0b305d54c 108 while(1){
benkatz 3:cae0b305d54c 109 signal_to_hbridge(controller.command_position_tm());
matthieulap 4:4e7b392ed0aa 110 printf("%F", pulsesToDegrees(wheel.getPulses()));
benkatz 3:cae0b305d54c 111 }
matthieulap 4:4e7b392ed0aa 112
benkatz 2:89bb6272869b 113 }
benkatz 2:89bb6272869b 114
benkatz 2:89bb6272869b 115
benkatz 3:cae0b305d54c 116 PIDController::PIDController(float desired_position, float desired_torque, float p_gain_p, float d_gain_p, float i_gain_p, float p_gain_c, float i_gain_c)
benkatz 2:89bb6272869b 117 {
benkatz 3:cae0b305d54c 118 kp_p = p_gain_p;
benkatz 3:cae0b305d54c 119 kd_p = d_gain_p;
benkatz 3:cae0b305d54c 120 ki_p = i_gain_p;
benkatz 3:cae0b305d54c 121 kp_c = p_gain_c;
benkatz 3:cae0b305d54c 122 ki_c = i_gain_c;
matthieulap 4:4e7b392ed0aa 123
benkatz 3:cae0b305d54c 124 torque = desired_torque;
matthieulap 4:4e7b392ed0aa 125 goal_position = desired_position;
matthieulap 4:4e7b392ed0aa 126
benkatz 3:cae0b305d54c 127 current_position = 0;
benkatz 3:cae0b305d54c 128 torque_command = 0;
benkatz 3:cae0b305d54c 129 c_torque = 0;
benkatz 2:89bb6272869b 130 error = 0;
benkatz 2:89bb6272869b 131 old_error = 0;
benkatz 3:cae0b305d54c 132 error_sum = 0;
benkatz 3:cae0b305d54c 133 direction = 0;
benkatz 3:cae0b305d54c 134 counter = 0;
matthieulap 4:4e7b392ed0aa 135
benkatz 3:cae0b305d54c 136 timer.start();
benkatz 3:cae0b305d54c 137 command = 0;
benkatz 3:cae0b305d54c 138 }
benkatz 3:cae0b305d54c 139
benkatz 3:cae0b305d54c 140 //voltage mode position control
benkatz 3:cae0b305d54c 141 float PIDController::command_position(void) //This function is called to set the desired position of the servo
benkatz 3:cae0b305d54c 142 {
matthieulap 4:4e7b392ed0aa 143 wait(.0004); //This delay allows enough time to register a position difference between cycles.
benkatz 3:cae0b305d54c 144 //Without the delay, velocity is read as 0, so there is no D feedback.
benkatz 3:cae0b305d54c 145 //The delay can be decreased when adding more servos, as the computation time becomes more significant.
matthieulap 4:4e7b392ed0aa 146
benkatz 3:cae0b305d54c 147 float desired_position = this->goal_position; //All value are stored in the PIDController object created, and can be changed at any time
benkatz 3:cae0b305d54c 148 float p_gain = this->kp_p;
benkatz 3:cae0b305d54c 149 float d_gain = this->kd_p;
benkatz 3:cae0b305d54c 150 float i_gain = this->ki_p;
benkatz 3:cae0b305d54c 151 if (this->timer.read_us() > 400){
benkatz 3:cae0b305d54c 152 float currentPosition = pulsesToDegrees(wheel.getPulses()); //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians
benkatz 3:cae0b305d54c 153
benkatz 3:cae0b305d54c 154 this->error = (currentPosition - desired_position);
benkatz 3:cae0b305d54c 155 this->integral_error += this->error;
benkatz 3:cae0b305d54c 156
benkatz 3:cae0b305d54c 157 this->command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error);
benkatz 3:cae0b305d54c 158
benkatz 3:cae0b305d54c 159 this->old_error = error;
benkatz 3:cae0b305d54c 160 }
benkatz 3:cae0b305d54c 161
benkatz 3:cae0b305d54c 162 return this->command;
aberk 0:bcff39fac858 163
aberk 0:bcff39fac858 164 }
benkatz 2:89bb6272869b 165
benkatz 2:89bb6272869b 166
benkatz 3:cae0b305d54c 167 float PIDController::command_position_tm(void){ //Torque Mode position control
matthieulap 4:4e7b392ed0aa 168 wait(.0003);
matthieulap 4:4e7b392ed0aa 169
matthieulap 4:4e7b392ed0aa 170 float desired_position = this->goal_position;
benkatz 3:cae0b305d54c 171 float p_gain = this->kp_p;
benkatz 3:cae0b305d54c 172 float d_gain = this->kd_p;
benkatz 3:cae0b305d54c 173 float i_gain = this->ki_p;
matthieulap 4:4e7b392ed0aa 174
benkatz 3:cae0b305d54c 175 if (this->timer.read_us() > 400){
benkatz 3:cae0b305d54c 176 this->current_position = pulsesToDegrees(wheel.getPulses()); //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians
benkatz 3:cae0b305d54c 177 this->old_error = this->error;
benkatz 3:cae0b305d54c 178 this->error = (this->current_position - desired_position);
benkatz 3:cae0b305d54c 179 this-> command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error);
benkatz 3:cae0b305d54c 180 this->integral_error += this->error;
matthieulap 4:4e7b392ed0aa 181 this->torque_command = command;
matthieulap 4:4e7b392ed0aa 182 this->timer.reset();
matthieulap 4:4e7b392ed0aa 183 }
benkatz 2:89bb6272869b 184
benkatz 2:89bb6272869b 185
benkatz 3:cae0b305d54c 186 return this->command_torque();
matthieulap 4:4e7b392ed0aa 187
benkatz 2:89bb6272869b 188
benkatz 2:89bb6272869b 189 }
benkatz 3:cae0b305d54c 190
benkatz 3:cae0b305d54c 191
benkatz 3:cae0b305d54c 192
benkatz 3:cae0b305d54c 193 float PIDController::command_torque(void){
matthieulap 4:4e7b392ed0aa 194 wait(.0004);
benkatz 3:cae0b305d54c 195 float current_torque;
matthieulap 4:4e7b392ed0aa 196
benkatz 3:cae0b305d54c 197 if (this->direction != 0){
matthieulap 4:4e7b392ed0aa 198 current_torque = this->direction*(Current.read() * volt2amp);
benkatz 3:cae0b305d54c 199 }
benkatz 3:cae0b305d54c 200 else{
matthieulap 4:4e7b392ed0aa 201 current_torque = (Current.read() * volt2amp);
benkatz 3:cae0b305d54c 202 }
benkatz 3:cae0b305d54c 203 float average = 0;
matthieulap 4:4e7b392ed0aa 204
benkatz 3:cae0b305d54c 205 for (int i = 0; i < 4; i++){
benkatz 3:cae0b305d54c 206 this->past_currents[i] = this->past_currents[i+1];
benkatz 3:cae0b305d54c 207 average += past_currents[i];
benkatz 3:cae0b305d54c 208 }
benkatz 3:cae0b305d54c 209 average += current_torque;
benkatz 3:cae0b305d54c 210 average = average/5;
benkatz 3:cae0b305d54c 211 average = current_torque;
benkatz 3:cae0b305d54c 212 this->past_currents[4] = current_torque;
matthieulap 4:4e7b392ed0aa 213
benkatz 3:cae0b305d54c 214 this->c_torque = average;
benkatz 3:cae0b305d54c 215 this->c_error = (this->torque - average);
benkatz 3:cae0b305d54c 216
benkatz 3:cae0b305d54c 217 this->error_sum += this->c_error;
matthieulap 4:4e7b392ed0aa 218
benkatz 3:cae0b305d54c 219 this->torque_command += -1*(this->kp_c*this->c_error + this->ki_c*this->error_sum);
benkatz 3:cae0b305d54c 220 if (this->torque_command > 0){
benkatz 3:cae0b305d54c 221 this->direction = -1;
benkatz 3:cae0b305d54c 222 }
benkatz 3:cae0b305d54c 223 else if(this->torque_command < 0){
benkatz 3:cae0b305d54c 224 this->direction = 1;
benkatz 3:cae0b305d54c 225 }
benkatz 3:cae0b305d54c 226 else{
benkatz 3:cae0b305d54c 227 this->direction = 0;
benkatz 3:cae0b305d54c 228 }
benkatz 3:cae0b305d54c 229 if (this->torque_command > 1){
benkatz 3:cae0b305d54c 230 this->torque_command = 1;
benkatz 3:cae0b305d54c 231 }
benkatz 3:cae0b305d54c 232 else if (this->torque_command < -1){
benkatz 3:cae0b305d54c 233 this->torque_command = -1;
benkatz 3:cae0b305d54c 234 }
matthieulap 4:4e7b392ed0aa 235
benkatz 3:cae0b305d54c 236 return this->torque_command;
benkatz 3:cae0b305d54c 237 }