Biorobotics 7 / BioroboticsMotorControl

PID motor controll for the biorobotics project.

Dependencies:   FastPWM QEI

Dependents:   PID_example Motor_calibration Demo_mode Demo_mode ... more

motor.cpp@13:9be1857401f8, 2018-11-01 (annotated)

Committer:
brass_phoenix
Date:
Thu Nov 01 12:55:32 2018 +0000
Revision:
13:9be1857401f8
Parent:
12:2c65bcbebf8a 
* Fixed max speed bug.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
brass_phoenix 0:009e84d7af32 1 #include "motor.h"
brass_phoenix 0:009e84d7af32 2
brass_phoenix 2:b30a467e90d3 3 Motor::Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b):
brass_phoenix 2:b30a467e90d3 4 pwm_out(pwm_pin),
brass_phoenix 2:b30a467e90d3 5 dir_out(dir_pin),
brass_phoenix 2:b30a467e90d3 6 encoder(encoder_a, encoder_b, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING)
brass_phoenix 2:b30a467e90d3 7 {
brass_phoenix 2:b30a467e90d3 8 pid = PID();
brass_phoenix 2:b30a467e90d3 9
brass_phoenix 2:b30a467e90d3 10 target_angle = 0;
brass_phoenix 7:eb8787e7a5f5 11 extra_reduction_ratio = 1.0;
brass_phoenix 2:b30a467e90d3 12
brass_phoenix 13:9be1857401f8 13 max_speed = 1.0;
brass_phoenix 12:2c65bcbebf8a 14
brass_phoenix 2:b30a467e90d3 15 printcount = 0;
brass_phoenix 2:b30a467e90d3 16 pid_period = 0;
brass_phoenix 2:b30a467e90d3 17 serial_debugging = false;
brass_phoenix 2:b30a467e90d3 18
brass_phoenix 2:b30a467e90d3 19 // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to be done once)
brass_phoenix 2:b30a467e90d3 20 pwm_out.period_us(60.0);
brass_phoenix 2:b30a467e90d3 21 }
brass_phoenix 2:b30a467e90d3 22
brass_phoenix 0:009e84d7af32 23 Motor::Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b, Serial* pc):
brass_phoenix 0:009e84d7af32 24 pwm_out(pwm_pin),
brass_phoenix 0:009e84d7af32 25 dir_out(dir_pin),
brass_phoenix 0:009e84d7af32 26 encoder(encoder_a, encoder_b, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING)
brass_phoenix 0:009e84d7af32 27 {
brass_phoenix 0:009e84d7af32 28 pid = PID();
brass_phoenix 0:009e84d7af32 29
brass_phoenix 0:009e84d7af32 30 target_angle = 0;
brass_phoenix 9:48a0b4a67283 31 extra_reduction_ratio = 1.0;
brass_phoenix 0:009e84d7af32 32
brass_phoenix 13:9be1857401f8 33 max_speed = 1.0;
brass_phoenix 12:2c65bcbebf8a 34
brass_phoenix 0:009e84d7af32 35 printcount = 0;
brass_phoenix 0:009e84d7af32 36 pid_period = 0;
brass_phoenix 0:009e84d7af32 37 this->pc = pc;
brass_phoenix 2:b30a467e90d3 38 serial_debugging = true;
brass_phoenix 0:009e84d7af32 39
brass_phoenix 0:009e84d7af32 40 // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to be done once)
brass_phoenix 0:009e84d7af32 41 pwm_out.period_us(60.0);
brass_phoenix 0:009e84d7af32 42 }
brass_phoenix 0:009e84d7af32 43
brass_phoenix 0:009e84d7af32 44 void Motor::start(float period) {
brass_phoenix 0:009e84d7af32 45 pid_period = period;
brass_phoenix 0:009e84d7af32 46 pid.set_period(period);
brass_phoenix 4:5353c5d0d2ed 47 motor_ticker.attach(callback(this, &Motor::update), period);
brass_phoenix 4:5353c5d0d2ed 48 }
brass_phoenix 4:5353c5d0d2ed 49
brass_phoenix 4:5353c5d0d2ed 50 void Motor::stop() {
brass_phoenix 4:5353c5d0d2ed 51 motor_ticker.detach();
brass_phoenix 6:d83c79716ea1 52 target_angle = get_current_angle();
brass_phoenix 4:5353c5d0d2ed 53 // Stop the actual motor.
brass_phoenix 4:5353c5d0d2ed 54 pwm_out = 0.0;
brass_phoenix 4:5353c5d0d2ed 55 dir_out = 0;
brass_phoenix 4:5353c5d0d2ed 56
brass_phoenix 4:5353c5d0d2ed 57 pid.clear_state();
brass_phoenix 4:5353c5d0d2ed 58
brass_phoenix 4:5353c5d0d2ed 59 printcount = 0;
brass_phoenix 0:009e84d7af32 60 }
brass_phoenix 0:009e84d7af32 61
brass_phoenix 11:34bd7f42c9db 62 void Motor::define_current_angle_as_x_radians(double radians) {
brass_phoenix 5:5537072b0e2e 63 encoder.reset();
brass_phoenix 5:5537072b0e2e 64 target_angle = 0;
brass_phoenix 10:d50c4957a193 65 rotation_frame_offset = radians;
brass_phoenix 5:5537072b0e2e 66 }
brass_phoenix 5:5537072b0e2e 67
brass_phoenix 0:009e84d7af32 68 void Motor::set_pid_k_values(double k_p, double k_i, double k_d) {
brass_phoenix 0:009e84d7af32 69 pid.set_k_values(k_p, k_i, k_d);
brass_phoenix 0:009e84d7af32 70 }
brass_phoenix 0:009e84d7af32 71
brass_phoenix 7:eb8787e7a5f5 72 void Motor::set_extra_reduction_ratio(double ratio) {
brass_phoenix 7:eb8787e7a5f5 73 extra_reduction_ratio = ratio;
brass_phoenix 7:eb8787e7a5f5 74 }
brass_phoenix 7:eb8787e7a5f5 75
brass_phoenix 0:009e84d7af32 76 void Motor::set_target_angle(double angle) {
brass_phoenix 10:d50c4957a193 77 // Preform calculation between motor angles and robot angles.
brass_phoenix 10:d50c4957a193 78 target_angle = (angle - rotation_frame_offset) / extra_reduction_ratio;
brass_phoenix 0:009e84d7af32 79 }
brass_phoenix 0:009e84d7af32 80
brass_phoenix 0:009e84d7af32 81 double Motor::get_current_angle() {
brass_phoenix 10:d50c4957a193 82 // Preform calculation between motor angles and robot angles.
brass_phoenix 10:d50c4957a193 83 return (encoder_pulses_to_radians(encoder.getPulses()) * extra_reduction_ratio) + rotation_frame_offset;
brass_phoenix 0:009e84d7af32 84 }
brass_phoenix 0:009e84d7af32 85
brass_phoenix 0:009e84d7af32 86 void Motor::update() {
brass_phoenix 0:009e84d7af32 87 int pulses = encoder.getPulses();
brass_phoenix 0:009e84d7af32 88 double current_angle = encoder_pulses_to_radians(pulses);
brass_phoenix 0:009e84d7af32 89
brass_phoenix 0:009e84d7af32 90 double error = current_angle - target_angle;
brass_phoenix 0:009e84d7af32 91 // PID controll.
brass_phoenix 0:009e84d7af32 92 double speed_rps = pid.update(error);
brass_phoenix 0:009e84d7af32 93
brass_phoenix 0:009e84d7af32 94 double speed_pwm = radians_per_second_to_pwm(speed_rps);
brass_phoenix 0:009e84d7af32 95
brass_phoenix 2:b30a467e90d3 96 if (serial_debugging) {
brass_phoenix 2:b30a467e90d3 97
brass_phoenix 2:b30a467e90d3 98 printcount++;
brass_phoenix 2:b30a467e90d3 99 if (printcount >= 0.1L/pid_period) {
brass_phoenix 2:b30a467e90d3 100 pc->printf("c_angle: %f, d_angle: %f, error: %f, rps: %f, speed: %f\n", current_angle, target_angle, error, speed_rps, speed_pwm);
brass_phoenix 2:b30a467e90d3 101 printcount = 0;
brass_phoenix 2:b30a467e90d3 102 }
brass_phoenix 0:009e84d7af32 103 }
brass_phoenix 0:009e84d7af32 104
brass_phoenix 0:009e84d7af32 105 update_motor_speed(speed_pwm);
brass_phoenix 0:009e84d7af32 106 }
brass_phoenix 0:009e84d7af32 107
brass_phoenix 0:009e84d7af32 108
brass_phoenix 0:009e84d7af32 109 void Motor::update_motor_speed(double speed) {
brass_phoenix 13:9be1857401f8 110 // Limit the output speed.
brass_phoenix 13:9be1857401f8 111 double requested_speed = speed;
brass_phoenix 13:9be1857401f8 112 if (requested_speed > max_speed) {
brass_phoenix 13:9be1857401f8 113 speed = max_speed;
brass_phoenix 13:9be1857401f8 114 }
brass_phoenix 13:9be1857401f8 115
brass_phoenix 0:009e84d7af32 116 if (speed < 1.0 && speed > 0) {
brass_phoenix 0:009e84d7af32 117 // Speed is in the range [0, 1] but the motor only moves
brass_phoenix 0:009e84d7af32 118 // in the range [0.5, 1]. Rescale for this.
brass_phoenix 0:009e84d7af32 119 speed = (speed * (1-MOTOR_STALL_PWM)) + MOTOR_STALL_PWM;
brass_phoenix 0:009e84d7af32 120 }
brass_phoenix 0:009e84d7af32 121 if (speed > -1.0 && speed < 0) {
brass_phoenix 0:009e84d7af32 122 // Speed is in the range [-1, 0] but the motor only moves
brass_phoenix 0:009e84d7af32 123 // in the range [-1, -0.5]. Rescale for this.
brass_phoenix 0:009e84d7af32 124 speed = (speed * (1-MOTOR_STALL_PWM)) - MOTOR_STALL_PWM;
brass_phoenix 0:009e84d7af32 125 }
brass_phoenix 0:009e84d7af32 126
brass_phoenix 0:009e84d7af32 127 // either true or false, determines direction (0 or 1)
brass_phoenix 0:009e84d7af32 128 dir_out = speed > 0;
brass_phoenix 0:009e84d7af32 129 // pwm duty cycle can only be positive, floating point absolute value (if value is >0, the there still will be a positive value).
brass_phoenix 12:2c65bcbebf8a 130
brass_phoenix 13:9be1857401f8 131
brass_phoenix 13:9be1857401f8 132 pwm_out = fabs(speed);
brass_phoenix 0:009e84d7af32 133 }
brass_phoenix 0:009e84d7af32 134
brass_phoenix 0:009e84d7af32 135 double Motor::encoder_pulses_to_radians(int pulses) {
brass_phoenix 0:009e84d7af32 136 return (pulses/float(PULSES_PER_ROTATION)) * 2.0f*PI;
brass_phoenix 0:009e84d7af32 137 }
brass_phoenix 0:009e84d7af32 138
brass_phoenix 0:009e84d7af32 139
brass_phoenix 0:009e84d7af32 140 // Converts radians/s values into PWM values for motor controll.
brass_phoenix 0:009e84d7af32 141 // Both positive and negative values.
brass_phoenix 0:009e84d7af32 142 double Motor::radians_per_second_to_pwm(double rps) {
brass_phoenix 0:009e84d7af32 143 // If the rad/s is below the anti-jitter treshold, it is simply 0.
brass_phoenix 0:009e84d7af32 144 if (rps > 0 && rps < MOTOR_THRESHOLD_RPS) {
brass_phoenix 0:009e84d7af32 145 rps = 0;
brass_phoenix 0:009e84d7af32 146 }
brass_phoenix 0:009e84d7af32 147 if (rps < 0 && rps > -MOTOR_THRESHOLD_RPS) {
brass_phoenix 0:009e84d7af32 148 rps = 0;
brass_phoenix 0:009e84d7af32 149 }
brass_phoenix 0:009e84d7af32 150
brass_phoenix 0:009e84d7af32 151
brass_phoenix 0:009e84d7af32 152 // With our specific motor, full PWM is equal to 1 round per second.
brass_phoenix 0:009e84d7af32 153 // Or 2PI radians per second.
brass_phoenix 0:009e84d7af32 154 double pwm_speed = rps / (2*PI);
brass_phoenix 0:009e84d7af32 155
brass_phoenix 0:009e84d7af32 156 // PWM speeds can only go between [-1, 1]
brass_phoenix 0:009e84d7af32 157 if (pwm_speed > 1) { pwm_speed = 1; }
brass_phoenix 0:009e84d7af32 158 if (pwm_speed < -1) { pwm_speed = -1; }
brass_phoenix 0:009e84d7af32 159 return pwm_speed;
brass_phoenix 0:009e84d7af32 160 }