PID motor controll for the biorobotics project.

Dependencies:   FastPWM QEI

Dependents:   PID_example Motor_calibration Demo_mode Demo_mode ... more

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers motor.cpp Source File

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 }