Chun Feng Huang / Differential_Drive

Differential drive succeed (Ver. 1.0)

Differential_Drive.h@0:644a119c9d8a, 2016-10-26 (annotated)

Committer:
benson516
Date:
Wed Oct 26 17:56:15 2016 +0000
Revision:
0:644a119c9d8a
Differential drive succeed (Ver. 1.0)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:644a119c9d8a 1 // A motion control for differential drive mobile robot
benson516 0:644a119c9d8a 2 // Designed by Chun-Feng Huang (Benson516)
benson516 0:644a119c9d8a 3
benson516 0:644a119c9d8a 4 #ifndef DIFFERENTIL_DRIVE_H
benson516 0:644a119c9d8a 5 #define DIFFERENTIL_DRIVE_H
benson516 0:644a119c9d8a 6 // Begine the library
benson516 0:644a119c9d8a 7 ////////////////////////
benson516 0:644a119c9d8a 8
benson516 0:644a119c9d8a 9 // #include "mbed.h"
benson516 0:644a119c9d8a 10 #include <vector>
benson516 0:644a119c9d8a 11 #include <cmath> // for abs()
benson516 0:644a119c9d8a 12 #include "PID.h"
benson516 0:644a119c9d8a 13
benson516 0:644a119c9d8a 14 using std::abs;
benson516 0:644a119c9d8a 15 using std::vector;
benson516 0:644a119c9d8a 16
benson516 0:644a119c9d8a 17 class Diff_Drive{
benson516 0:644a119c9d8a 18 float Ts; // sec., sampling time
benson516 0:644a119c9d8a 19
benson516 0:644a119c9d8a 20 /////////////////////////
benson516 0:644a119c9d8a 21
benson516 0:644a119c9d8a 22 // Controller parameters
benson516 0:644a119c9d8a 23 // Control gains
benson516 0:644a119c9d8a 24 // K.Size = 8
benson516 0:644a119c9d8a 25 // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2
benson516 0:644a119c9d8a 26 vector<float> K;
benson516 0:644a119c9d8a 27
benson516 0:644a119c9d8a 28 // Driver parameters
benson516 0:644a119c9d8a 29 // Output limit
benson516 0:644a119c9d8a 30 float Vdc; // Volt.
benson516 0:644a119c9d8a 31 float voltage_limit_H; // +Vdc
benson516 0:644a119c9d8a 32 float voltage_limit_L; // -Vdc
benson516 0:644a119c9d8a 33 // Input limit
benson516 0:644a119c9d8a 34 float W_max,W_max_inv; // rad/s
benson516 0:644a119c9d8a 35
benson516 0:644a119c9d8a 36 // Motor parameters
benson516 0:644a119c9d8a 37 float Ke; // Speed constant, volt.-sec./rad <- This may be repalced by a nonlinear function f_e(w)
benson516 0:644a119c9d8a 38 float Kt; // Torque Constant, Nt./Amp. (Ke >=Kt)
benson516 0:644a119c9d8a 39
benson516 0:644a119c9d8a 40 // Robot parameters
benson516 0:644a119c9d8a 41 float b, b_inv; // m, half of robot's wheel axle
benson516 0:644a119c9d8a 42 float r, r_half, r_inv; // m, wheel radious
benson516 0:644a119c9d8a 43 float r_half_b_inv; // r_half*b_inv
benson516 0:644a119c9d8a 44
benson516 0:644a119c9d8a 45 // States(feedback signals), commands, outputs,
benson516 0:644a119c9d8a 46 // States(feedback signals)
benson516 0:644a119c9d8a 47 vector<float> V_W; // Speed-rotaion domain
benson516 0:644a119c9d8a 48 vector<float> W1_W2; // Two-single-wheel domain, W1: right wheel, W2: left wheel
benson516 0:644a119c9d8a 49 // Commands
benson516 0:644a119c9d8a 50 vector<float> Vd_Wd; // Input from higher-level commander
benson516 0:644a119c9d8a 51 vector<float> W1d_W2d;
benson516 0:644a119c9d8a 52 vector<float> W1dS_W2dS; // Saturated command for W1 and W2
benson516 0:644a119c9d8a 53 vector<float> VdS_WdS; // Saturated command for V and W
benson516 0:644a119c9d8a 54 // Outputs (voltage command)
benson516 0:644a119c9d8a 55 vector<float> UV_UW; // Controller output, speed-rotaion domain
benson516 0:644a119c9d8a 56 vector<float> U1_U2; // Controller output, two-single-wheel domain, U1: right wheel, U2: left wheel
benson516 0:644a119c9d8a 57 vector<float> V1_V2; // Voltage compensated with back-emf, two-single-wheel domain, V1: right wheel, V2: left wheel
benson516 0:644a119c9d8a 58 vector<float> V1S_V2S; // Saturated voltage, two-single-wheel domain
benson516 0:644a119c9d8a 59
benson516 0:644a119c9d8a 60 vector<float> delta_V1_V2, delta_VV_VW; // The difference between saturated and original voltage command
benson516 0:644a119c9d8a 61 // deltaVi = VSi - Vi
benson516 0:644a119c9d8a 62
benson516 0:644a119c9d8a 63 // PID controller
benson516 0:644a119c9d8a 64 PID PID_1,PID_2;
benson516 0:644a119c9d8a 65
benson516 0:644a119c9d8a 66 public:
benson516 0:644a119c9d8a 67 Diff_Drive(float Ts_in, bool Diff_drive_in);
benson516 0:644a119c9d8a 68
benson516 0:644a119c9d8a 69 //
benson516 0:644a119c9d8a 70 bool Diff_drive; // Ture: differential drive; false: separated control
benson516 0:644a119c9d8a 71 //
benson516 0:644a119c9d8a 72
benson516 0:644a119c9d8a 73 // T: two-single-wheel domain |-> speed-rotaion domain
benson516 0:644a119c9d8a 74 void Transform(const vector<float> &V_in, vector<float> &V_out);
benson516 0:644a119c9d8a 75 void Transform_inv(const vector<float> &V_in, vector<float> &V_out); // Inverse transformation
benson516 0:644a119c9d8a 76
benson516 0:644a119c9d8a 77 // Main process
benson516 0:644a119c9d8a 78 void compute(float Vd, float Wd, float W1, float W2);
benson516 0:644a119c9d8a 79 void compute_SeparatedMethod(float Vd, float Wd, float W1, float W2);
benson516 0:644a119c9d8a 80 void compute_DiffDriveMethod(float Vd, float Wd, float W1, float W2);
benson516 0:644a119c9d8a 81 // Get results
benson516 0:644a119c9d8a 82 float get_V1S();
benson516 0:644a119c9d8a 83 float get_V2S();
benson516 0:644a119c9d8a 84
benson516 0:644a119c9d8a 85 // Saturation
benson516 0:644a119c9d8a 86 void Saturation_input(const vector<float> &in, vector<float> &out, bool enable);
benson516 0:644a119c9d8a 87 void Saturation_output(const vector<float> &in, vector<float> &out,vector<float> &delta_out, bool enable);
benson516 0:644a119c9d8a 88
benson516 0:644a119c9d8a 89 // Back emf as the function of rotational speed
benson516 0:644a119c9d8a 90 float func_back_emf(float W_in);
benson516 0:644a119c9d8a 91
benson516 0:644a119c9d8a 92 };
benson516 0:644a119c9d8a 93
benson516 0:644a119c9d8a 94
benson516 0:644a119c9d8a 95 //////////////////////// end Begine the library
benson516 0:644a119c9d8a 96 #endif