Differential drive succeed (Ver. 1.0)

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Differential_Drive.h Source File

Differential_Drive.h

00001 // A motion control for differential drive mobile robot
00002 // Designed by Chun-Feng Huang (Benson516)
00003 
00004 #ifndef DIFFERENTIL_DRIVE_H
00005 #define DIFFERENTIL_DRIVE_H
00006 // Begine the library
00007 ////////////////////////
00008 
00009 // #include "mbed.h"
00010 #include <vector>
00011 #include <cmath> // for abs()
00012 #include "PID.h"
00013 
00014 using std::abs;
00015 using std::vector;
00016 
00017 class Diff_Drive{
00018     float Ts; // sec., sampling time
00019     
00020     /////////////////////////
00021     
00022     // Controller parameters
00023     // Control gains
00024     // K.Size = 8
00025     // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2
00026     vector<float> K;
00027     
00028     // Driver parameters
00029     // Output limit
00030     float Vdc; // Volt.
00031     float voltage_limit_H; // +Vdc
00032     float voltage_limit_L; // -Vdc
00033     // Input limit
00034     float W_max,W_max_inv; // rad/s
00035     
00036     // Motor parameters
00037     float Ke; // Speed constant, volt.-sec./rad <- This may be repalced by a nonlinear function f_e(w)
00038     float Kt; // Torque Constant, Nt./Amp. (Ke >=Kt)
00039     
00040     // Robot parameters
00041     float b, b_inv; // m, half of robot's wheel axle
00042     float r, r_half, r_inv;        // m, wheel radious
00043     float r_half_b_inv; // r_half*b_inv
00044     
00045     // States(feedback signals), commands, outputs, 
00046     // States(feedback signals)
00047     vector<float> V_W;     // Speed-rotaion domain
00048     vector<float> W1_W2;   // Two-single-wheel domain, W1: right wheel, W2: left wheel
00049     // Commands
00050     vector<float> Vd_Wd;     // Input from higher-level commander
00051     vector<float> W1d_W2d;
00052     vector<float> W1dS_W2dS; // Saturated command for W1 and W2
00053     vector<float> VdS_WdS;   // Saturated command for V  and W
00054     // Outputs (voltage command)
00055     vector<float> UV_UW;    // Controller output, speed-rotaion domain
00056     vector<float> U1_U2;    // Controller output, two-single-wheel domain, U1: right wheel, U2: left wheel
00057     vector<float> V1_V2;    // Voltage compensated with back-emf, two-single-wheel domain, V1: right wheel, V2: left wheel
00058     vector<float> V1S_V2S;  // Saturated voltage, two-single-wheel domain
00059     
00060     vector<float> delta_V1_V2, delta_VV_VW; // The difference between saturated and original voltage command
00061                                // deltaVi = VSi - Vi
00062 
00063     // PID controller
00064     PID PID_1,PID_2;                           
00065     
00066 public:
00067     Diff_Drive(float Ts_in, bool Diff_drive_in);
00068     
00069     //
00070     bool Diff_drive; // Ture: differential drive; false: separated control
00071     //
00072     
00073     // T: two-single-wheel domain |-> speed-rotaion domain
00074     void Transform(const vector<float> &V_in, vector<float> &V_out);
00075     void Transform_inv(const vector<float> &V_in, vector<float> &V_out); // Inverse transformation
00076     
00077     // Main process
00078     void compute(float Vd, float Wd, float W1, float W2);
00079     void compute_SeparatedMethod(float Vd, float Wd, float W1, float W2);
00080     void compute_DiffDriveMethod(float Vd, float Wd, float W1, float W2);
00081     // Get results 
00082     float get_V1S();
00083     float get_V2S();
00084     
00085     // Saturation
00086     void Saturation_input(const vector<float> &in, vector<float> &out, bool enable);
00087     void Saturation_output(const vector<float> &in, vector<float> &out,vector<float> &delta_out, bool enable);
00088     
00089     // Back emf as the function of rotational speed
00090     float func_back_emf(float W_in);
00091 
00092 };
00093 
00094 
00095 //////////////////////// end Begine the library
00096 #endif