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@7:eb8787e7a5f5, 2018-10-31 (annotated)

Committer:
brass_phoenix
Date:
Wed Oct 31 08:47:16 2018 +0000
Revision:
7:eb8787e7a5f5
Parent:
5:5537072b0e2e
Child:
10:d50c4957a193
+ UNTESTED: Added the possibility to add an extra gearbox on top of the internal motor one.

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 0:009e84d7af32 33 double target_angle;
brass_phoenix 0:009e84d7af32 34
brass_phoenix 0:009e84d7af32 35 // For debugging purposes;
brass_phoenix 0:009e84d7af32 36 Serial* pc;
brass_phoenix 2:b30a467e90d3 37 bool serial_debugging;
brass_phoenix 0:009e84d7af32 38 int printcount;
brass_phoenix 0:009e84d7af32 39 float pid_period;
brass_phoenix 0:009e84d7af32 40 public:
brass_phoenix 0:009e84d7af32 41 // Initializes the motor with the two driver pins, and the two encoder pins.
brass_phoenix 0:009e84d7af32 42 Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b, Serial* pc);
brass_phoenix 2:b30a467e90d3 43 // Initialize the motor without serial output.
brass_phoenix 2:b30a467e90d3 44 Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b);
brass_phoenix 0:009e84d7af32 45 // Set the angle the motor needs to rotate to. In radians.
brass_phoenix 0:009e84d7af32 46 void set_target_angle(double angle);
brass_phoenix 0:009e84d7af32 47 void set_pid_k_values(double k_p, double k_i, double k_d);
brass_phoenix 7:eb8787e7a5f5 48 // Reduction ratio of an extra gear box.
brass_phoenix 7:eb8787e7a5f5 49 // On top of the gear box in the motor.
brass_phoenix 7:eb8787e7a5f5 50 void set_extra_reduction_ratio(double ratio);
brass_phoenix 5:5537072b0e2e 51
brass_phoenix 0:009e84d7af32 52 // Starts the motor controller with the specified update period.
brass_phoenix 0:009e84d7af32 53 void start(float period);
brass_phoenix 4:5353c5d0d2ed 54 // Stops the motor immediately.
brass_phoenix 4:5353c5d0d2ed 55 void stop();
brass_phoenix 4:5353c5d0d2ed 56
brass_phoenix 5:5537072b0e2e 57 // Makes the current angle the new 0 point.
brass_phoenix 5:5537072b0e2e 58 void set_current_angle_as_zero();
brass_phoenix 5:5537072b0e2e 59
brass_phoenix 0:009e84d7af32 60 // Returns the current angle of the motor.
brass_phoenix 0:009e84d7af32 61 double get_current_angle();
brass_phoenix 0:009e84d7af32 62
brass_phoenix 0:009e84d7af32 63 private:
brass_phoenix 0:009e84d7af32 64 void update();
brass_phoenix 0:009e84d7af32 65 // Updates the motor with the given speed.
brass_phoenix 0:009e84d7af32 66 // The speed can be both positive and negative, in the range [-1, 1].
brass_phoenix 0:009e84d7af32 67 void update_motor_speed(double speed);
brass_phoenix 0:009e84d7af32 68 double encoder_pulses_to_radians(int pulses);
brass_phoenix 0:009e84d7af32 69 // Converts radians/s values into PWM values for motor controll.
brass_phoenix 0:009e84d7af32 70 // Both positive and negative values.
brass_phoenix 0:009e84d7af32 71 double radians_per_second_to_pwm(double rps);
brass_phoenix 0:009e84d7af32 72 };