PID motor controll for the biorobotics project.
Dependents: PID_example Motor_calibration Demo_mode Demo_mode ... more
Diff: motor.cpp
- Revision:
- 0:009e84d7af32
- Child:
- 2:b30a467e90d3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor.cpp Fri Oct 19 10:57:39 2018 +0000 @@ -0,0 +1,101 @@ +#include "motor.h" + +Motor::Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b, Serial* pc): + pwm_out(pwm_pin), + dir_out(dir_pin), + encoder(encoder_a, encoder_b, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING) + { + pid = PID(); + + target_angle = 0; + + printcount = 0; + pid_period = 0; + this->pc = pc; + + // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to be done once) + pwm_out.period_us(60.0); +} + +void Motor::start(float period) { + pid_period = period; + pid.set_period(period); + motor_ticker.attach(this, &Motor::update, period); +} + +void Motor::set_pid_k_values(double k_p, double k_i, double k_d) { + pid.set_k_values(k_p, k_i, k_d); +} + +void Motor::set_target_angle(double angle) { + target_angle = angle; +} + +double Motor::get_current_angle() { + return encoder_pulses_to_radians(encoder.getPulses()); +} + +void Motor::update() { + int pulses = encoder.getPulses(); + double current_angle = encoder_pulses_to_radians(pulses); + + double error = current_angle - target_angle; + // PID controll. + double speed_rps = pid.update(error); + + double speed_pwm = radians_per_second_to_pwm(speed_rps); + + printcount++; + if (printcount >= 0.1L/pid_period) { + pc->printf("c_angle: %f, d_angle: %f, error: %f, rps: %f, speed: %f\n", current_angle, target_angle, error, speed_rps, speed_pwm); + printcount = 0; + } + + update_motor_speed(speed_pwm); +} + + +void Motor::update_motor_speed(double speed) { + if (speed < 1.0 && speed > 0) { + // Speed is in the range [0, 1] but the motor only moves + // in the range [0.5, 1]. Rescale for this. + speed = (speed * (1-MOTOR_STALL_PWM)) + MOTOR_STALL_PWM; + } + if (speed > -1.0 && speed < 0) { + // Speed is in the range [-1, 0] but the motor only moves + // in the range [-1, -0.5]. Rescale for this. + speed = (speed * (1-MOTOR_STALL_PWM)) - MOTOR_STALL_PWM; + } + + // either true or false, determines direction (0 or 1) + dir_out = speed > 0; + // pwm duty cycle can only be positive, floating point absolute value (if value is >0, the there still will be a positive value). + pwm_out = fabs(speed); +} + +double Motor::encoder_pulses_to_radians(int pulses) { + return (pulses/float(PULSES_PER_ROTATION)) * 2.0f*PI; +} + + +// Converts radians/s values into PWM values for motor controll. +// Both positive and negative values. +double Motor::radians_per_second_to_pwm(double rps) { + // If the rad/s is below the anti-jitter treshold, it is simply 0. + if (rps > 0 && rps < MOTOR_THRESHOLD_RPS) { + rps = 0; + } + if (rps < 0 && rps > -MOTOR_THRESHOLD_RPS) { + rps = 0; + } + + + // With our specific motor, full PWM is equal to 1 round per second. + // Or 2PI radians per second. + double pwm_speed = rps / (2*PI); + + // PWM speeds can only go between [-1, 1] + if (pwm_speed > 1) { pwm_speed = 1; } + if (pwm_speed < -1) { pwm_speed = -1; } + return pwm_speed; +} \ No newline at end of file