Diff: Differential_Drive.h

Revision:

0:644a119c9d8a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Differential_Drive.h	Wed Oct 26 17:56:15 2016 +0000
@@ -0,0 +1,96 @@
+// A motion control for differential drive mobile robot
+// Designed by Chun-Feng Huang (Benson516)
+
+#ifndef DIFFERENTIL_DRIVE_H
+#define DIFFERENTIL_DRIVE_H
+// Begine the library
+////////////////////////
+
+// #include "mbed.h"
+#include <vector>
+#include <cmath> // for abs()
+#include "PID.h"
+
+using std::abs;
+using std::vector;
+
+class Diff_Drive{
+    float Ts; // sec., sampling time
+    
+    /////////////////////////
+    
+    // Controller parameters
+    // Control gains
+    // K.Size = 8
+    // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2
+    vector<float> K;
+    
+    // Driver parameters
+    // Output limit
+    float Vdc; // Volt.
+    float voltage_limit_H; // +Vdc
+    float voltage_limit_L; // -Vdc
+    // Input limit
+    float W_max,W_max_inv; // rad/s
+    
+    // Motor parameters
+    float Ke; // Speed constant, volt.-sec./rad <- This may be repalced by a nonlinear function f_e(w)
+    float Kt; // Torque Constant, Nt./Amp. (Ke >=Kt)
+    
+    // Robot parameters
+    float b, b_inv; // m, half of robot's wheel axle
+    float r, r_half, r_inv;        // m, wheel radious
+    float r_half_b_inv; // r_half*b_inv
+    
+    // States(feedback signals), commands, outputs, 
+    // States(feedback signals)
+    vector<float> V_W;     // Speed-rotaion domain
+    vector<float> W1_W2;   // Two-single-wheel domain, W1: right wheel, W2: left wheel
+    // Commands
+    vector<float> Vd_Wd;     // Input from higher-level commander
+    vector<float> W1d_W2d;
+    vector<float> W1dS_W2dS; // Saturated command for W1 and W2
+    vector<float> VdS_WdS;   // Saturated command for V  and W
+    // Outputs (voltage command)
+    vector<float> UV_UW;    // Controller output, speed-rotaion domain
+    vector<float> U1_U2;    // Controller output, two-single-wheel domain, U1: right wheel, U2: left wheel
+    vector<float> V1_V2;    // Voltage compensated with back-emf, two-single-wheel domain, V1: right wheel, V2: left wheel
+    vector<float> V1S_V2S;  // Saturated voltage, two-single-wheel domain
+    
+    vector<float> delta_V1_V2, delta_VV_VW; // The difference between saturated and original voltage command
+                               // deltaVi = VSi - Vi
+
+    // PID controller
+    PID PID_1,PID_2;                           
+    
+public:
+    Diff_Drive(float Ts_in, bool Diff_drive_in);
+    
+    //
+    bool Diff_drive; // Ture: differential drive; false: separated control
+    //
+    
+    // T: two-single-wheel domain |-> speed-rotaion domain
+    void Transform(const vector<float> &V_in, vector<float> &V_out);
+    void Transform_inv(const vector<float> &V_in, vector<float> &V_out); // Inverse transformation
+    
+    // Main process
+    void compute(float Vd, float Wd, float W1, float W2);
+    void compute_SeparatedMethod(float Vd, float Wd, float W1, float W2);
+    void compute_DiffDriveMethod(float Vd, float Wd, float W1, float W2);
+    // Get results 
+    float get_V1S();
+    float get_V2S();
+    
+    // Saturation
+    void Saturation_input(const vector<float> &in, vector<float> &out, bool enable);
+    void Saturation_output(const vector<float> &in, vector<float> &out,vector<float> &delta_out, bool enable);
+    
+    // Back emf as the function of rotational speed
+    float func_back_emf(float W_in);
+
+};
+
+
+//////////////////////// end Begine the library
+#endif