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

Committer:
brass_phoenix
Date:
Wed Oct 31 10:38:38 2018 +0000
Revision:
10:d50c4957a193
Parent:
7:eb8787e7a5f5
Child:
12:2c65bcbebf8a
+ UNTESTED: Added a way to set the motor reference angle.

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