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