File content as of revision 0:644a119c9d8a:

// 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