jiexuan fan / Mbed 2 deprecated ServoController

mit

Dependencies:   mbed QEI

main.cpp@5:75b5c24e36ad, 2018-12-24 (annotated)

Committer:
coldplay
Date:
Mon Dec 24 03:46:36 2018 +0000
Revision:
5:75b5c24e36ad
Parent:
4:4e7b392ed0aa 
mit

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