Differential drive succeed (Ver. 1.0)
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Sat Jul 23 2022 02:02:34 by
1.7.2