![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
mit
main.cpp
- Committer:
- matthieulap
- Date:
- 2015-03-18
- Revision:
- 4:4e7b392ed0aa
- Parent:
- 3:cae0b305d54c
- Child:
- 5:75b5c24e36ad
File content as of revision 4:4e7b392ed0aa:
//Ben Katz, 2013 #include "QEI.h" #include "PID.h" #define pi 3.14159265358979323846 Serial pc(USBTX, USBRX); AnalogIn p_pot (p15); AnalogIn d_pot (p16); AnalogIn a_pot (p17); PwmOut Motor1(p21); //Ouput pin to h-bridge 1 PwmOut Motor2 (p22); //Output pin to h-bridge 2 AnalogIn Current (p20); //Current sense pin int steps_per_rev = 1200; //Encoder CPR * gearbox ratio Timer timer; // Setup parameters float volt2amp = 3.3 / .525; //(0-3.3V input) * (voltage divider muiltiplier) / (.525 V/A) float PWM_frequency = 20000.0; // MC33926 h-bridge limited 20 kHz (.00005 seconds) //Use X4 encoding. 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)) float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians { return ((pulses/steps_per_rev)*(2*pi)); } float pulsesToDegrees(float pulses) //Encoder steps to revolutions of output shaft in degrees { return ((pulses/steps_per_rev)*360); } void signal_to_hbridge( float signal) //Input of -1 means full reverse, 1 means full forward { //An input of magnitude > 1 gets reduced to 1 (or -1) by the pwm.write() function if (signal < 0) { Motor1.write(0); Motor2.write(signal * -1); } else if (signal > 0) { Motor1.write(signal); Motor2.write(0); } else { Motor1.write(0); Motor2.write(0); } } void pulse_motor(float pulse_time, float pulse_interval) //Usefull for testing peak torque. //pulse_time = motor on time. pulse_interval = motor off time { timer.start(); while (timer.read() < pulse_time){ Motor1.write(1); Motor2.write(0); //Prints current draw to PC printf("%F", (Current.read())*volt2amp); //Arithmetic converts the analog input to Amps printf("\n"); } timer.stop(); timer.reset(); wait(pulse_time); Motor1.write(0); Motor2.write(0); wait(pulse_interval); } int main() { Motor1.period(1 / PWM_frequency); //Set PWM frequency. MC33926 h-bridge limited 20 kHz (.00005 seconds) Motor2.period(1 / PWM_frequency); float desired_position = 40; float torque = 0.1; float position_P_gain = 0.1; float position_D_gain = 3; float position_I_gain = 0; float torque_P_gain = 0.0007; float torque_I_gain = 0.0000001; PIDController controller(desired_position, torque, position_P_gain, position_D_gain, position_I_gain, torque_P_gain, torque_I_gain); //timer.start(); wait(1.5); printf("%F", pulsesToDegrees(wheel.getPulses())); printf("\n\r"); while(1){ signal_to_hbridge(controller.command_position_tm()); printf("%F", pulsesToDegrees(wheel.getPulses())); } } 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) { kp_p = p_gain_p; kd_p = d_gain_p; ki_p = i_gain_p; kp_c = p_gain_c; ki_c = i_gain_c; torque = desired_torque; goal_position = desired_position; current_position = 0; torque_command = 0; c_torque = 0; error = 0; old_error = 0; error_sum = 0; direction = 0; counter = 0; timer.start(); command = 0; } //voltage mode position control float PIDController::command_position(void) //This function is called to set the desired position of the servo { wait(.0004); //This delay allows enough time to register a position difference between cycles. //Without the delay, velocity is read as 0, so there is no D feedback. //The delay can be decreased when adding more servos, as the computation time becomes more significant. float desired_position = this->goal_position; //All value are stored in the PIDController object created, and can be changed at any time float p_gain = this->kp_p; float d_gain = this->kd_p; float i_gain = this->ki_p; if (this->timer.read_us() > 400){ float currentPosition = pulsesToDegrees(wheel.getPulses()); //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians this->error = (currentPosition - desired_position); this->integral_error += this->error; this->command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error); this->old_error = error; } return this->command; } float PIDController::command_position_tm(void){ //Torque Mode position control wait(.0003); float desired_position = this->goal_position; float p_gain = this->kp_p; float d_gain = this->kd_p; float i_gain = this->ki_p; if (this->timer.read_us() > 400){ this->current_position = pulsesToDegrees(wheel.getPulses()); //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians this->old_error = this->error; this->error = (this->current_position - desired_position); this-> command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error); this->integral_error += this->error; this->torque_command = command; this->timer.reset(); } return this->command_torque(); } float PIDController::command_torque(void){ wait(.0004); float current_torque; if (this->direction != 0){ current_torque = this->direction*(Current.read() * volt2amp); } else{ current_torque = (Current.read() * volt2amp); } float average = 0; for (int i = 0; i < 4; i++){ this->past_currents[i] = this->past_currents[i+1]; average += past_currents[i]; } average += current_torque; average = average/5; average = current_torque; this->past_currents[4] = current_torque; this->c_torque = average; this->c_error = (this->torque - average); this->error_sum += this->c_error; this->torque_command += -1*(this->kp_c*this->c_error + this->ki_c*this->error_sum); if (this->torque_command > 0){ this->direction = -1; } else if(this->torque_command < 0){ this->direction = 1; } else{ this->direction = 0; } if (this->torque_command > 1){ this->torque_command = 1; } else if (this->torque_command < -1){ this->torque_command = -1; } return this->torque_command; }