Biorobotics 7 / BioroboticsMotorControl

PID motor controll for the biorobotics project.

Dependencies:   FastPWM QEI

Dependents:   PID_example Motor_calibration Demo_mode Demo_mode ... more

motor.h@12:2c65bcbebf8a, 2018-10-31 (annotated)

Committer:
brass_phoenix
Date:
Wed Oct 31 16:19:09 2018 +0000
Revision:
12:2c65bcbebf8a
Parent:
10:d50c4957a193
Child:
13:9be1857401f8
+ Added maximum PWM limiter for the motors.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
brass_phoenix 0:009e84d7af32 1 #pragma once
brass_phoenix 0:009e84d7af32 2
brass_phoenix 0:009e84d7af32 3 #include "mbed.h"
brass_phoenix 0:009e84d7af32 4 #include "FastPWM.h"
brass_phoenix 0:009e84d7af32 5 #include "MODSERIAL.h"
brass_phoenix 0:009e84d7af32 6 #include "QEI.h"
brass_phoenix 0:009e84d7af32 7
brass_phoenix 0:009e84d7af32 8 #include "pid.h"
brass_phoenix 0:009e84d7af32 9
brass_phoenix 0:009e84d7af32 10 // Amount of motor encoder pulses per rotation. When using X4 encoding.
brass_phoenix 0:009e84d7af32 11 const int PULSES_PER_ROTATION = 6533;
brass_phoenix 0:009e84d7af32 12
brass_phoenix 0:009e84d7af32 13 const double MOTOR_THRESHOLD_RPS = 0.1; // Rad/s under which we send 0 to the motor, to prevent it from jittering around.
brass_phoenix 0:009e84d7af32 14 const double MOTOR_STALL_PWM = 0.45; // PWM fraction above which the motor starts to move.
brass_phoenix 0:009e84d7af32 15
brass_phoenix 0:009e84d7af32 16 class Motor {
brass_phoenix 0:009e84d7af32 17 /// Rotates a motor to a given angle using PID control.
brass_phoenix 0:009e84d7af32 18 /// Sets the pwm frequency to 60 HZ.
brass_phoenix 0:009e84d7af32 19 private:
brass_phoenix 4:5353c5d0d2ed 20 const float PI = 3.14159265359;
brass_phoenix 4:5353c5d0d2ed 21
brass_phoenix 0:009e84d7af32 22 Ticker motor_ticker;
brass_phoenix 0:009e84d7af32 23
brass_phoenix 0:009e84d7af32 24 FastPWM pwm_out;
brass_phoenix 0:009e84d7af32 25 DigitalOut dir_out;
brass_phoenix 0:009e84d7af32 26 QEI encoder;
brass_phoenix 0:009e84d7af32 27 PID pid;
brass_phoenix 0:009e84d7af32 28
brass_phoenix 7:eb8787e7a5f5 29 // Reduction ratio of an extra gear box.
brass_phoenix 7:eb8787e7a5f5 30 // On top of the gear box in the motor.
brass_phoenix 7:eb8787e7a5f5 31 double extra_reduction_ratio;
brass_phoenix 7:eb8787e7a5f5 32
brass_phoenix 10:d50c4957a193 33 // How much our rotation frame is turned with respect to the "0" value
brass_phoenix 10:d50c4957a193 34 // of the encoder.
brass_phoenix 10:d50c4957a193 35 double rotation_frame_offset;
brass_phoenix 10:d50c4957a193 36
brass_phoenix 0:009e84d7af32 37 double target_angle;
brass_phoenix 0:009e84d7af32 38
brass_phoenix 12:2c65bcbebf8a 39 // Maximum pwm fraction that this motor is allowed to output.
brass_phoenix 12:2c65bcbebf8a 40 double max_pwm_fraction;
brass_phoenix 12:2c65bcbebf8a 41
brass_phoenix 0:009e84d7af32 42 // For debugging purposes;
brass_phoenix 0:009e84d7af32 43 Serial* pc;
brass_phoenix 2:b30a467e90d3 44 bool serial_debugging;
brass_phoenix 0:009e84d7af32 45 int printcount;
brass_phoenix 0:009e84d7af32 46 float pid_period;
brass_phoenix 0:009e84d7af32 47 public:
brass_phoenix 0:009e84d7af32 48 // Initializes the motor with the two driver pins, and the two encoder pins.
brass_phoenix 0:009e84d7af32 49 Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b, Serial* pc);
brass_phoenix 2:b30a467e90d3 50 // Initialize the motor without serial output.
brass_phoenix 2:b30a467e90d3 51 Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b);
brass_phoenix 0:009e84d7af32 52 // Set the angle the motor needs to rotate to. In radians.
brass_phoenix 0:009e84d7af32 53 void set_target_angle(double angle);
brass_phoenix 0:009e84d7af32 54 void set_pid_k_values(double k_p, double k_i, double k_d);
brass_phoenix 7:eb8787e7a5f5 55 // Reduction ratio of an extra gear box.
brass_phoenix 7:eb8787e7a5f5 56 // On top of the gear box in the motor.
brass_phoenix 7:eb8787e7a5f5 57 void set_extra_reduction_ratio(double ratio);
brass_phoenix 5:5537072b0e2e 58
brass_phoenix 12:2c65bcbebf8a 59 void set_max_pwm_fraction(double fraction) {
brass_phoenix 12:2c65bcbebf8a 60 max_pwm_fraction = fraction;
brass_phoenix 12:2c65bcbebf8a 61 };
brass_phoenix 12:2c65bcbebf8a 62
brass_phoenix 0:009e84d7af32 63 // Starts the motor controller with the specified update period.
brass_phoenix 0:009e84d7af32 64 void start(float period);
brass_phoenix 4:5353c5d0d2ed 65 // Stops the motor immediately.
brass_phoenix 4:5353c5d0d2ed 66 void stop();
brass_phoenix 4:5353c5d0d2ed 67
brass_phoenix 10:d50c4957a193 68 // Defines the current angle to be the given amount of radians.
brass_phoenix 10:d50c4957a193 69 void define_current_angle_as_x_radians(double radians);
brass_phoenix 5:5537072b0e2e 70
brass_phoenix 0:009e84d7af32 71 // Returns the current angle of the motor.
brass_phoenix 0:009e84d7af32 72 double get_current_angle();
brass_phoenix 0:009e84d7af32 73
brass_phoenix 0:009e84d7af32 74 private:
brass_phoenix 0:009e84d7af32 75 void update();
brass_phoenix 0:009e84d7af32 76 // Updates the motor with the given speed.
brass_phoenix 0:009e84d7af32 77 // The speed can be both positive and negative, in the range [-1, 1].
brass_phoenix 0:009e84d7af32 78 void update_motor_speed(double speed);
brass_phoenix 0:009e84d7af32 79 double encoder_pulses_to_radians(int pulses);
brass_phoenix 0:009e84d7af32 80 // Converts radians/s values into PWM values for motor controll.
brass_phoenix 0:009e84d7af32 81 // Both positive and negative values.
brass_phoenix 0:009e84d7af32 82 double radians_per_second_to_pwm(double rps);
brass_phoenix 0:009e84d7af32 83 };