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@2:b30a467e90d3, 2018-10-29 (annotated)

Committer:
brass_phoenix
Date:
Mon Oct 29 14:15:57 2018 +0000
Revision:
2:b30a467e90d3
Parent:
0:009e84d7af32
Child:
4:5353c5d0d2ed
+ Added constructor without serial debuging.

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 const float PI = 3.14159265359;
brass_phoenix 0:009e84d7af32 11
brass_phoenix 0:009e84d7af32 12 // Amount of motor encoder pulses per rotation. When using X4 encoding.
brass_phoenix 0:009e84d7af32 13 const int PULSES_PER_ROTATION = 6533;
brass_phoenix 0:009e84d7af32 14
brass_phoenix 0:009e84d7af32 15 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 16 const double MOTOR_STALL_PWM = 0.45; // PWM fraction above which the motor starts to move.
brass_phoenix 0:009e84d7af32 17
brass_phoenix 0:009e84d7af32 18 class Motor {
brass_phoenix 0:009e84d7af32 19 /// Rotates a motor to a given angle using PID control.
brass_phoenix 0:009e84d7af32 20 /// Sets the pwm frequency to 60 HZ.
brass_phoenix 0:009e84d7af32 21 private:
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 0:009e84d7af32 29 double target_angle;
brass_phoenix 0:009e84d7af32 30
brass_phoenix 0:009e84d7af32 31 // For debugging purposes;
brass_phoenix 0:009e84d7af32 32 Serial* pc;
brass_phoenix 2:b30a467e90d3 33 bool serial_debugging;
brass_phoenix 0:009e84d7af32 34 int printcount;
brass_phoenix 0:009e84d7af32 35 float pid_period;
brass_phoenix 0:009e84d7af32 36 public:
brass_phoenix 0:009e84d7af32 37 // Initializes the motor with the two driver pins, and the two encoder pins.
brass_phoenix 0:009e84d7af32 38 Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b, Serial* pc);
brass_phoenix 2:b30a467e90d3 39 // Initialize the motor without serial output.
brass_phoenix 2:b30a467e90d3 40 Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b);
brass_phoenix 0:009e84d7af32 41 // Set the angle the motor needs to rotate to. In radians.
brass_phoenix 0:009e84d7af32 42 void set_target_angle(double angle);
brass_phoenix 0:009e84d7af32 43 void set_pid_k_values(double k_p, double k_i, double k_d);
brass_phoenix 0:009e84d7af32 44 // Starts the motor controller with the specified update period.
brass_phoenix 0:009e84d7af32 45 void start(float period);
brass_phoenix 0:009e84d7af32 46
brass_phoenix 0:009e84d7af32 47 // Returns the current angle of the motor.
brass_phoenix 0:009e84d7af32 48 double get_current_angle();
brass_phoenix 0:009e84d7af32 49
brass_phoenix 0:009e84d7af32 50 private:
brass_phoenix 0:009e84d7af32 51 void update();
brass_phoenix 0:009e84d7af32 52 // Updates the motor with the given speed.
brass_phoenix 0:009e84d7af32 53 // The speed can be both positive and negative, in the range [-1, 1].
brass_phoenix 0:009e84d7af32 54 void update_motor_speed(double speed);
brass_phoenix 0:009e84d7af32 55 double encoder_pulses_to_radians(int pulses);
brass_phoenix 0:009e84d7af32 56 // Converts radians/s values into PWM values for motor controll.
brass_phoenix 0:009e84d7af32 57 // Both positive and negative values.
brass_phoenix 0:009e84d7af32 58 double radians_per_second_to_pwm(double rps);
brass_phoenix 0:009e84d7af32 59 };