PID motor controll for the biorobotics project.
Dependents: PID_example Motor_calibration Demo_mode Demo_mode ... more
Diff: motor.h
- Revision:
- 0:009e84d7af32
- Child:
- 2:b30a467e90d3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor.h Fri Oct 19 10:57:39 2018 +0000 @@ -0,0 +1,56 @@ +#pragma once + +#include "mbed.h" +#include "FastPWM.h" +#include "MODSERIAL.h" +#include "QEI.h" + +#include "pid.h" + +const float PI = 3.14159265359; + +// Amount of motor encoder pulses per rotation. When using X4 encoding. +const int PULSES_PER_ROTATION = 6533; + +const double MOTOR_THRESHOLD_RPS = 0.1; // Rad/s under which we send 0 to the motor, to prevent it from jittering around. +const double MOTOR_STALL_PWM = 0.45; // PWM fraction above which the motor starts to move. + +class Motor { + /// Rotates a motor to a given angle using PID control. + /// Sets the pwm frequency to 60 HZ. +private: + Ticker motor_ticker; + + FastPWM pwm_out; + DigitalOut dir_out; + QEI encoder; + PID pid; + + double target_angle; + + // For debugging purposes; + Serial* pc; + int printcount; + float pid_period; +public: + // Initializes the motor with the two driver pins, and the two encoder pins. + Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b, Serial* pc); + // Set the angle the motor needs to rotate to. In radians. + void set_target_angle(double angle); + void set_pid_k_values(double k_p, double k_i, double k_d); + // Starts the motor controller with the specified update period. + void start(float period); + + // Returns the current angle of the motor. + double get_current_angle(); + +private: + void update(); + // Updates the motor with the given speed. + // The speed can be both positive and negative, in the range [-1, 1]. + void update_motor_speed(double speed); + double encoder_pulses_to_radians(int pulses); + // Converts radians/s values into PWM values for motor controll. + // Both positive and negative values. + double radians_per_second_to_pwm(double rps); +}; \ No newline at end of file