File content as of revision 20:559db13dc85a:

/*
#ifndef PI
#define PI = 3.1415926
#endif
*/

#ifndef __CURRENT_CONTROL_H__
#define __CURRENT_CONTROL_H__

#include "mbed.h"
#include "PID.h"
#include "QEI.h"
//
#include "FILTER_LIB.h" // Filter library



/*
class LPF
{public:
    float output;

    LPF(float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz"
    float filter(float input);
    void reset(float input);

private:
    float Ts;
    float cutOff_freq_Hz; // Hz
    float alpha_Ts;
    float One_alpha_Ts;

    // Flag
    bool Flag_Init;
};
*/

class CURRENT_CONTROL
{
public:
    typedef enum {
        PWM1,
        PWM2
    } PWMIndex;

    //Current-input limit
    float current_limit_H; // +1.3
    float current_limit_L; // -1.3

    //                      AD_pin,             PWM_p,               PWM_n,       pwmIndex  ,                   Ts
    // CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, float samplingTime);
    //                      AD_pin,             PWM_p,               PWM_n,                pwmIndex  ,       QEI_A        , QEI_B      , pulsesPerRev    , arraysize ,  Ts
    CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, PinName QEI_A, PinName QEI_B, float pulsesPerRev, int arraysize, float samplingTime);

    // Setting parameters
    void SetParams(float Analog2Cur_in, float angSpeed2BackEmf, float voltage2Duty_in);
    void SetReversal(bool reverse_current_in, bool reverse_rotationalSpeed_in, bool reverse_voltage_in);
    void SetGain(float Kp, float Ki, float Kd, float Ka);
    void setInputLimits(float current_limit_H_in, float current_limit_L_in);

    // Initialization
    void OffsetInit(void);
    // Utilities
    float saturation(float input_value, const float &limit_H, const float &limit_L);
    float saturation(float input_value, float &delta, const float &limit_H, const float &limit_L);
    // Control
    void TorqueControl(float TorqueRef, bool enable);
    void Control(float curRef, bool enable);

    // Back emf as the function of rotational speed
    float func_back_emf(const float &W_in);

    // Elementary function (building block)
    void ChangePwmPeriod(float microSeconds);
    void SetPWMDuty(float ratio);
    void SetVoltage(float volt);
    float GetAnalogIn(void);
    float GetCurrent(void);


    //////////////////////////
    PWMIndex pwmIndex_;
    PID pid;
    PwmOut MotorPlus;
    PwmOut MotorMinus;
    QEI wheelSpeed; //(pin_A, pin_B, pin_Z, pulsesPerRev, arraysize, sampletime, pulses)

    //
    // float controlOutput;
    float currentOffset;
    float curFeedBack;
    float curFeedBack_filter;
    float curCommand;
    //
    float voltage_out;
    float delta_output;

    //
    AnalogIn currentAnalogIn;
    float analogInValue;

    // Ratio for units transformation
    float analog2Cur;
    float voltage2Duty;

    //
    float Ke;
    float Kt;
    float Kt_inv;



    // Initialization
    bool Flag_Init;
    int Init_count;
    float Accumulated_offset;

    // Speed
    bool Flag_SpeedCal_Iterated;
    float angularSpeed;
    float Speed_IterateOnce(void);
    float getAngularSpeed(void);
    float getAngularSpeed_deg_s(void);

private:

    float Ts;
    bool reverse_current;
    bool reverse_rotationalSpeed;
    bool reverse_voltage;

    // Low-pass filters
    LPF lpf_current; // For motor current
    LPF_nthOrderCritical lpf_wheelSpeed; // For wheel rotational speed

};


#endif