PID motor controll for the biorobotics project.
Dependents: PID_example Motor_calibration Demo_mode Demo_mode ... more
motor.cpp
00001 #include "motor.h" 00002 00003 Motor::Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b): 00004 pwm_out(pwm_pin), 00005 dir_out(dir_pin), 00006 encoder(encoder_a, encoder_b, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING) 00007 { 00008 pid = PID(); 00009 00010 target_angle = 0; 00011 extra_reduction_ratio = 1.0; 00012 00013 max_speed = 1.0; 00014 00015 printcount = 0; 00016 pid_period = 0; 00017 serial_debugging = false; 00018 00019 // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to be done once) 00020 pwm_out.period_us(60.0); 00021 } 00022 00023 Motor::Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b, Serial* pc): 00024 pwm_out(pwm_pin), 00025 dir_out(dir_pin), 00026 encoder(encoder_a, encoder_b, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING) 00027 { 00028 pid = PID(); 00029 00030 target_angle = 0; 00031 extra_reduction_ratio = 1.0; 00032 00033 max_speed = 1.0; 00034 00035 printcount = 0; 00036 pid_period = 0; 00037 this->pc = pc; 00038 serial_debugging = true; 00039 00040 // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to be done once) 00041 pwm_out.period_us(60.0); 00042 } 00043 00044 void Motor::start(float period) { 00045 pid_period = period; 00046 pid.set_period(period); 00047 motor_ticker.attach(callback(this, &Motor::update), period); 00048 } 00049 00050 void Motor::stop() { 00051 motor_ticker.detach(); 00052 target_angle = get_current_angle(); 00053 // Stop the actual motor. 00054 pwm_out = 0.0; 00055 dir_out = 0; 00056 00057 pid.clear_state(); 00058 00059 printcount = 0; 00060 } 00061 00062 void Motor::define_current_angle_as_x_radians(double radians) { 00063 encoder.reset(); 00064 target_angle = 0; 00065 rotation_frame_offset = radians; 00066 } 00067 00068 void Motor::set_pid_k_values(double k_p, double k_i, double k_d) { 00069 pid.set_k_values(k_p, k_i, k_d); 00070 } 00071 00072 void Motor::set_extra_reduction_ratio(double ratio) { 00073 extra_reduction_ratio = ratio; 00074 } 00075 00076 void Motor::set_target_angle(double angle) { 00077 // Preform calculation between motor angles and robot angles. 00078 target_angle = (angle - rotation_frame_offset) / extra_reduction_ratio; 00079 } 00080 00081 double Motor::get_current_angle() { 00082 // Preform calculation between motor angles and robot angles. 00083 return (encoder_pulses_to_radians(encoder.getPulses()) * extra_reduction_ratio) + rotation_frame_offset; 00084 } 00085 00086 void Motor::update() { 00087 int pulses = encoder.getPulses(); 00088 double current_angle = encoder_pulses_to_radians(pulses); 00089 00090 double error = current_angle - target_angle; 00091 // PID controll. 00092 double speed_rps = pid.update(error); 00093 00094 double speed_pwm = radians_per_second_to_pwm(speed_rps); 00095 00096 if (serial_debugging) { 00097 00098 printcount++; 00099 if (printcount >= 0.1L/pid_period) { 00100 pc->printf("c_angle: %f, d_angle: %f, error: %f, rps: %f, speed: %f\n", current_angle, target_angle, error, speed_rps, speed_pwm); 00101 printcount = 0; 00102 } 00103 } 00104 00105 update_motor_speed(speed_pwm); 00106 } 00107 00108 00109 void Motor::update_motor_speed(double speed) { 00110 // Limit the output speed. 00111 double requested_speed = speed; 00112 if (requested_speed > max_speed) { 00113 speed = max_speed; 00114 } 00115 00116 if (speed < 1.0 && speed > 0) { 00117 // Speed is in the range [0, 1] but the motor only moves 00118 // in the range [0.5, 1]. Rescale for this. 00119 speed = (speed * (1-MOTOR_STALL_PWM)) + MOTOR_STALL_PWM; 00120 } 00121 if (speed > -1.0 && speed < 0) { 00122 // Speed is in the range [-1, 0] but the motor only moves 00123 // in the range [-1, -0.5]. Rescale for this. 00124 speed = (speed * (1-MOTOR_STALL_PWM)) - MOTOR_STALL_PWM; 00125 } 00126 00127 // either true or false, determines direction (0 or 1) 00128 dir_out = speed > 0; 00129 // pwm duty cycle can only be positive, floating point absolute value (if value is >0, the there still will be a positive value). 00130 00131 00132 pwm_out = fabs(speed); 00133 } 00134 00135 double Motor::encoder_pulses_to_radians(int pulses) { 00136 return (pulses/float(PULSES_PER_ROTATION)) * 2.0f*PI; 00137 } 00138 00139 00140 // Converts radians/s values into PWM values for motor controll. 00141 // Both positive and negative values. 00142 double Motor::radians_per_second_to_pwm(double rps) { 00143 // If the rad/s is below the anti-jitter treshold, it is simply 0. 00144 if (rps > 0 && rps < MOTOR_THRESHOLD_RPS) { 00145 rps = 0; 00146 } 00147 if (rps < 0 && rps > -MOTOR_THRESHOLD_RPS) { 00148 rps = 0; 00149 } 00150 00151 00152 // With our specific motor, full PWM is equal to 1 round per second. 00153 // Or 2PI radians per second. 00154 double pwm_speed = rps / (2*PI); 00155 00156 // PWM speeds can only go between [-1, 1] 00157 if (pwm_speed > 1) { pwm_speed = 1; } 00158 if (pwm_speed < -1) { pwm_speed = -1; } 00159 return pwm_speed; 00160 }
Generated on Wed Aug 10 2022 00:15:42 by
1.7.2