PID motor controll for the biorobotics project.

Dependencies:   FastPWM QEI

Dependents:   PID_example Motor_calibration Demo_mode Demo_mode ... more

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers motor.h Source File

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 };