PID motor controll for the biorobotics project.
Dependents: PID_example Motor_calibration Demo_mode Demo_mode ... more
motor.h
00001 #pragma once // Include only once 00002 00003 #include "mbed.h" 00004 #include "FastPWM.h" 00005 #include "MODSERIAL.h" 00006 #include "QEI.h" 00007 00008 #include "pid.h" 00009 00010 // Amount of motor encoder pulses per rotation. When using X4 encoding. 00011 const int PULSES_PER_ROTATION = 6533; 00012 00013 const double MOTOR_THRESHOLD_RPS = 0.1; // Motor speed fraction under which we send 0 to the motor, to prevent it from jittering around. 00014 const double MOTOR_STALL_PWM = 0.45; // PWM fraction above which the motor starts to move (below it motor will start to stall). 00015 00016 class Motor { 00017 /// Rotates a motor to a given angle using PID control. 00018 /// Sets the pwm frequency to 60 HZ. 00019 private: 00020 const float PI = 3.14159265359; 00021 00022 Ticker motor_ticker; 00023 00024 FastPWM pwm_out; 00025 DigitalOut dir_out; 00026 QEI encoder; 00027 PID pid; 00028 00029 // Reduction ratio of an extra gear box. 00030 // On top of the gear box in the motor. 00031 double extra_reduction_ratio; 00032 00033 // How much our rotation frame is turned with respect to the "0" value 00034 // of the encoder. 00035 double rotation_frame_offset; 00036 00037 double target_angle; 00038 00039 // Maximum speed [0 - 1.0]. 00040 double max_speed; 00041 00042 // For debugging purposes; 00043 Serial* pc; 00044 bool serial_debugging; 00045 int printcount; 00046 float pid_period; 00047 public: 00048 // Initializes the motor with the two driver pins, and the two encoder pins. 00049 Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b, Serial* pc); 00050 // Initialize the motor without serial output. 00051 Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b); 00052 // Set the angle the motor needs to rotate to. In radians. 00053 void set_target_angle(double angle); 00054 void set_pid_k_values(double k_p, double k_i, double k_d); 00055 // Reduction ratio of an extra gear box. 00056 // On top of the gear box in the motor. 00057 void set_extra_reduction_ratio(double ratio); 00058 00059 void set_max_speed(double fraction) { 00060 max_speed = fraction; 00061 }; 00062 00063 // Starts the motor controller with the specified update period. 00064 void start(float period); 00065 // Stops the motor immediately. 00066 void stop(); 00067 00068 // Defines the current angle to be the given amount of radians. 00069 void define_current_angle_as_x_radians(double radians); 00070 00071 // Returns the current angle of the motor. 00072 double get_current_angle(); 00073 00074 private: 00075 void update(); 00076 // Updates the motor with the given speed. 00077 // The speed can be both positive and negative, in the range [-1, 1]. 00078 void update_motor_speed(double speed); 00079 double encoder_pulses_to_radians(int pulses); 00080 // Converts radians/s values into PWM values for motor controll. 00081 // Both positive and negative values. 00082 double radians_per_second_to_pwm(double rps); 00083 };
Generated on Wed Aug 10 2022 00:15:42 by
1.7.2