PID motor controll for the biorobotics project.
Dependents: PID_example Motor_calibration Demo_mode Demo_mode ... more
motor.h@13:9be1857401f8, 2018-11-01 (annotated)
- Committer:
- brass_phoenix
- Date:
- Thu Nov 01 12:55:32 2018 +0000
- Revision:
- 13:9be1857401f8
- Parent:
- 12:2c65bcbebf8a
- Child:
- 14:e5fc69651b1d
* Fixed max speed bug.
Who changed what in which revision?
User | Revision | Line number | New 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 | 13:9be1857401f8 | 39 | // Maximum speed [0 - 1.0]. |
brass_phoenix | 13:9be1857401f8 | 40 | double max_speed; |
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 | 13:9be1857401f8 | 59 | void set_max_speed(double fraction) { |
brass_phoenix | 13:9be1857401f8 | 60 | max_speed = 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 | }; |