jiexuan fan / ServoController_2

mit

Dependencies:   QEI mbed-src

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:5ae9f8b3a16f
now with current control;

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
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 }