File content as of revision 0:b5f9b0530f25:

/*
This is a program for localized controller of one leg
Endpoint space Impedence Control and Force/Torque feedforward
Two input was acceptable
q_0_E[3] Neutral point for Impedence reference
T_0[3]   Feedforward of Force/Torque

---------o------↺ q_act_est[0]
        /|
    L1 / | ⤸ q_act_est[1]
      o  |
 L2 ／/
  ／ / ⤸ q_act_est[2]
o   /
MCU target: F303K8
ADC config: 3.3 Vadc common source
*/
//**************** Wiring Map ********************//
//      |---------------|           |   |---------------|
//   Srl| PA_9  | D1    |Cmd Serail |   | VIN   | VIN   |
//   Srl| PA_10 | D0    |Cmd Serial |   | GND   | GND   |
//      | NRST  | NRST  |           |   | NRST  | NRST  |
//      | GND   | GND   |           |   | 5V    | 5V    |
//      | PA_12 | D2    |           |Srl| A7    | PA_2  |PC Serial
//   PWM| PB_0  | D3    |Shing Drv  |Dou| A6    | PA_7  |CSG
//   Dou| PB_7  | D4    |Task1      |Din| A5    | PA_6  |CS2 Stretch Cmd
//   Dou| PB_6  | D5    |Task2      |Din| A4    | PA_5  |CS1 Serial Sync
//   PWM| PB_1  | D6    |Tigh Drv   |Ain| A3    | PA_4  |Shing sense
//      | PF_0  | D7    |           |Ain| A2    | PA_3  |Tigh sense
//      | PF_1  | D8    |           |Ain| A1    | PA_1  |Hip sense
//   PWM| PA_8  | D9    |Hip Drv    |   | A0    | PA_0  |
//      | PA_11 | D10   |           |   | AREF  | AREF  |VR source
//      | PB_5  | D11   |           |   | 3V3   | 3V3   |
//      | PB_4  | D12   |           |Dou| D13   | PB_3  |led / Serial error
//      |---------------|           |   |---------------|
#include "mbed.h"
//#include "LSM9DS0_SH.h"
//#define DEBUG       1
#define DEBUG       0
#define pi          3.141592f
#define d2r         0.01745329f

#define Rms         5000            //TT rate
#define dt          0.015f
#define Task_1_NN   2
#define Task_2_NN   199

//Torque-Controller gain
#define G1_rad_dc   0.00157f
#define Kqff        636.7f          //ffoward gain of q_act_est = 1 / G1_rad_dc
#define KTff        60.408f          //ffoward gain of T_ref_WD = 1 / (K_q * G1_rad_dc)
#define Kpt         22.9f
#define Ki          151.9f

//ADC reference constant
#define Kadc        5.817f          //adc value to q_spring
#define K_spring    13180f         //sping constant of single spring
#define K_q         10.544f         //2*K_spring*R^2
#define R           0.02f           //wheely's radius

//Endpoint Impedence Contorller coefficcient
#define Kpi_Hip     0
#define Kpi_Thigh   2000
#define Kpi_Shin    8
#define Kd_Hip      0.0
#define Kd_Thigh    0.0
#define Kd_Shin     0.0
#define KN          20
#define L1          0.120f          //Tigh length
#define L2          0.1480f          //Shing lenth
#define LA          (L1*L1+L2*L2)   //L1^2 + L2^2
#define LB          (L1*L2)         //L1*L2
#define LC          (L1/L2)         //L1/L2
#define L0          0.2f           //Length of the leg

//Structure
#define PWM_Hip     1550            //900~1550~2200 2050
#define PWM_Thigh   1730            //900~1550~2200 1300
#define PWM_Shin    1490            //900~1550~2200 1450 1740

#define Buff_size   16              //Serial Buffer

#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
//╔═════════════════╗
//║    Structure    ║
//╚═════════════════╝
DigitalOut  led(D13);               //detection
DigitalOut  led2(D12);              //detection
DigitalOut  Task_1(D4);             //Task indicate
DigitalOut  Task_2(D5);
DigitalOut  CSG(A6);                //Stretch interrupt pull down

AnalogIn    adc1(A1);               //adc
AnalogIn    adc2(A2);               //adc
AnalogIn    adc3(A3);               //adc

InterruptIn CS1(D2);                //Serial synchronizor
DigitalIn   CS2(A5);                //Stretch interrupt


//╔═════════════════╗
//║    Servo out    ║
//╚═════════════════╝
PwmOut      Drive1(D9);             //PWM 1/1
PwmOut      Drive2(D6);             //PWM 1/2N
PwmOut      Drive3(D3);             //PWM 1/3N

//╔═════════════════╗
//║     Serial      ║
//╚═════════════════╝
Serial      Debug(PA_2, PA_15);     //Serial reg(TX RX) USB port
Serial      Cmd(D1, D0);            //Serial reg(TX RX)
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of GPIO registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Varible registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
//╔═════════════════╗
//║    Structure    ║
//╚═════════════════╝
Ticker  TT;                         //call a timer
uint8_t Task_1_count = 0;           //1st prior task count
uint8_t Task_2_count = 0;           //2nd prior task count
uint8_t Flag_1 = 0;                 //1st prior task flag
uint8_t Flag_2 = 0;                 //2nd prior task flag

//╔═════════════════╗
//║       ADC       ║
//╚═════════════════╝

//╔═════════════════╗
//║       PWM       ║
//╚═════════════════╝
const int16_t  PWM_base[3] = {      //reference command at 0, -45, 45 deg
    PWM_Hip,PWM_Thigh,PWM_Shin
};
int16_t PWM[3] = {                  //command out PWM_base + q_ref
    0, 0, 0                         //transfer: 10us to 1 deg
};

//╔═════════════════╗
//║ q_actEstimator  ║
//╚═════════════════╝
/*
q_act_est = q_ref*G1_rad_TUT(z) - q_spring

G1_rad_TUT(z) is shown below

 y(z)     1.316 + 2.633*z^-1 + 1.316*z^-2               b0 + b1*z^-1 + b2*z^-2
----- = ----------------------------------- * 1e-04 =  ------------------------
 x(z)      1 - 0.8447*z^-1 + 0.1799*z^-2                1 - a1*z^-1 - a2*z^-2

q_ref═>[Σ]════════╦═>[b0]═>[Σ]═[1e-04]═>y
        ⇑         ⇓w0       ⇑
        ║       [1/z]       ║
       [Σ]<═[a1]<═╬═>[b1]═>[Σ]
        ⇑         ⇓w1       ⇑
        ║       [1/z]       ║
        ╚═══[a2]<═╩═>[b2]═══╝
                   w2
*/
float   q_act_est[3] = {            //output of filter
    0, 0, 0
};

float   L_act_est = 0.0;
float   P_act_est = 0.0;

float   G1_w_n[3][3] = {            //G1_rad_TUT's
    {0, 0, 0},                      //{w0, w1, w2}
    {0, 0, 0},                      //second sea
    {0, 0, 0},
};
const float   G1_a[3] = {           //{a0, a1, a2}
    1.00, 0.8447, -0.1799
};
const float   G1_b[3] = {           //{b0, b1, b2}
    1.316, 2.633, 1.316
};

//╔═════════════════╗//╔══════════════════╗
//║     Comp_ref    ║//║ Controller Input ║
//╚═════════════════╝//╚══════════════════╝
/*

---------o------↺ q_act_est[0]
        /|
    L1 / | ↻ q_act_est[1]
      o  |
 L2 ／/
  ／ / ↻ q_act_est[2]
o   /

                        1 - z^-1
PDn(z) = P + D*N*---------------------
                  1 - (1 - N*Ts)*z^-1

 y(z)    b0 + b1*z^-1      1 + (-1)*z^-1
----- = -------------- = ------------------
 x(z)    1 - a1*z^-1       1 - 0.7*z^-1

x═>[Σ]═══════╦═>[b0]═>[Σ]═>[D*N]═>y
    ⇑        ⇓w0       ⇑
    ║      [1/z]       ║
    ╚══[a1]<═╩═>[b1]═══╝
              w1
*/

float K_c[3] = {                    //K_c reference impedence            
    Kpi_Hip, Kpi_Thigh, Kpi_Shin     //{Hip, Thigh, Shin}      
};
float B_c[3] = {                    //B_c = D*N (*20)reference damping (Dmax~0.4)(B_c_max~5) in joint!!
    Kd_Hip*KN, Kd_Thigh*KN, Kd_Shin*KN                //{Hip, Thigh, Shin}
};
float q_0_E[3] = {                  //"Controller Input" from upper controller in endpoint level (Operational Space)
    0, 0.2, 0                         //{Hip, leg length, leg angle} original point
};

float q_0_E_1_gain = 0.2f/255.0f;
float q_0_E_2_gain = (pi/4)/128.0f;

float T_0[3] = {                    //"Controller Input" from upper controller in joint level
    0, 0, 0                         //{T_0_Hip, T_0_Tigh, F_0_Stretch} force feed foward
};
float q_err_E[3] = {                //error in endpoint level
    0, 0, 0
};
float q_err_D_E[3] = {              //derivative error in endpoint level
    0, 0, 0
};
float Cref_w[3][2] = {              //filter storage
    {0, 0},
    {0, 0},
    {0, 0},
};
const float Cref_a[2] = {           //{a0, a1}
    1.00, 0.70
};
const float Cref_b[2] = {           //{b0, b1}
    1.00, -1.00
};
float       J32 = 0.0;
float       J33 = 0.0;
float       Phi1_0 = 0.0;
float       Phi2_0 = 0.0;

//╔═════════════════╗
//║   Torque_reg    ║
//╚═════════════════╝
/*
         q_act_est             T_ref_WD
q_ref = ----------- + -------------------------- + PI( T_ref - 2*K_spring*R^2*q_spring )
         G1_rad_dc     2*K_spring*R^2*G1_rad_dc

PI(z) = P + I*dt / (z-1)
*/
const float Kp[3] = {               //P gain for Torque REG
    Kpt, Kpt, Kpt             //{Hip, Tigh, Shin}
};
const float Kit[3] = {              //I gain for Torque REG Kit = Ki*Ts
    Ki*dt, Ki*dt, Ki*dt            //{Hip, Tigh, Shin}
};
float T_ref[3] = {                  //command to T_reg in joint level
    0, 0, 0                         //{Hip, Tigh, Shin}
};
float T_ref_E[3] = {                //command to T_reg in end point level
    0, 0, 0                         //{Hip, Len, Phi}
};
float T_ref_WD[3] = {               //command without damping reference in joint level
    0, 0, 0
};
float T_ref_WD_E[3] = {             //command without damping reference in endpoint level
    0, 0, 0
};
float T_err[3] = {                  //T_ref - T_act_est, T_act_est = q_spring * K_q
    0, 0, 0                         //{Hip, Tigh, Shin}
};
float q_ref[3] = {                  //output of T_reg
    0, 0, 0                         //{Hip, Thigh, Shin}
};
float q_spring[3] = {               //read in spring travel
    0, 0, 0
};
float q_spring_0[3] = {             //neutral point of VR
    0, 0, 0
};
float T_reg_I[3] = {                //PI_w
    0, 0, 0
};

//╔═════════════════╗
//║   I/O Serial    ║
//╚═════════════════╝
uint8_t Buff[Buff_size];
uint8_t Recieve_index = 0;
uint8_t Cmd_Flag = 0;
uint8_t Rx_enable = 0;
char buffer[128];
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Function registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void    init_TIMER();           //set TT_main() rate
void    TT_main();              //timebase function rated by TT
void    init_q_spring_0();      //calibrating
void    init_IO();              //initialize IO state

void    q_spring_Est();
void    q_act_Est();
void    Impedence_Ref();
void    Torque_Reg();

void    Rx_irq();
void    Rx_srt();
void    Rx_end();
//void    NVIC_DisableIRQ(USART1_IRQn);
//void    NVIC_EnableIRQ(USART1_IRQn);
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡main funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
int main()
{
    Debug.baud(115200);                     //set baud rate
    Cmd.baud(115200);                       //set baud rate

    wait_ms(10);
    init_q_spring_0();                      //calibrating q_spring reference point

    wait_ms(600);
    init_IO();                              //initialized value
    init_TIMER();                           //start TT_main
    
    Cmd.attach(&Rx_irq,Serial::RxIrq);      //start recieving message
//    NVIC_DisableIRQ(USART1_IRQn);           //stay down before CS1 signal
    
    CS1.fall(&Rx_srt);                      //attach the address of Rx_srt to falling edge as start signal of communication
    CS1.rise(&Rx_end);                      //attach the address of Rx_end to rise edge
    
    //for joint endpoint transform
    Phi1_0 = (acos((L0*L0+L1*L1-L2*L2)/(2*L0*L1)));
    Phi2_0 = (Phi1_0 + acos((L0*L0+L2*L2-L1*L1)/(2*L0*L2)));
            
    while(1) {                              //main() loop

        //Task judging start
        if(Flag_1 == 1) {                   //check pending
            //Task_1 start         
            //read in condition & estimate state
            q_spring_Est();
            q_act_Est();

            //calculate control effort
            Impedence_Ref();
            Torque_Reg();

            //PWM Drive out process
            for(int i=0; i<3; i++) {
                PWM[i] = PWM_base[i] + q_ref[i];    // X + Controll
            }
            //PWM[0] = 1550;
            //PWM[1] = 1850;
            //PWM[2] = 1670;

            PWM[0] =  constrain(PWM[0],800 ,2100);//safty constrain
            PWM[1] =  constrain(PWM[1],800 ,2100);//safty constrain
            PWM[2] =  constrain(PWM[2],800 ,2100);//safty constrain

            Drive1.pulsewidth_us(PWM[0]);           //drive command
            Drive2.pulsewidth_us(PWM[1]);
            Drive3.pulsewidth_us(PWM[2]);

            //for Serial-Oscilloscope
#if DEBUG
//            q_0[2] = -0.9;
            Debug.printf("%f\n", T_ref[2]);
//            Debug.printf("%f, %f, %f\r", q_0[0], q_0[1], q_0[2]);
//            Debug.printf("%lf\r", adc3.read());
//            Debug.printf("%f\n", 123.23);
//            Debug.printf("%.f\r", q_ref[0]);L_act_est
//            Debug.printf("%.2f\r", q_spring_0[0]);
//            Debug.printf("%.2f\r", J33);
#endif

            //Task_1 done
            Flag_1 = 0;                 //clear pending
            Task_1 = 0;                 //clear sign
        }
        
        if(Cmd_Flag == 1) {
            //{...}{Hip, Len, Phi}, {'#'}
            //Received some shit, start decoding
            led = 1;
            if(Buff[3] == 'Q') {
                //q_0 command
                q_0_E[0] = (Buff[0] - 128);
                q_0_E[1] = 0.1f + q_0_E_1_gain * Buff[1];
                q_0_E[2] = q_0_E_2_gain * (Buff[2]-128);
            } else if(Buff[3] == 'T') {
                //T_0 command
                T_0[0] = 1.0f * (Buff[0] - 128);
                T_0[1] = 1.0f * (Buff[1] - 128);
                T_0[2] = 1.0f * (Buff[2] - 128);
            } else if(Buff[3] == 'K') {
                K_c[0] = 0.02f * Buff[0];
                K_c[1] = 0.02f * Buff[1];
                K_c[2] = 0.02f * Buff[2];
            } else if (Buff[3] == 'B') {
                B_c[0] = 0.02f * (Buff[0] - 128);
                B_c[1] = 0.02f * (Buff[1] - 128);
                B_c[2] = 0.02f * (Buff[2] - 128);
            } else {
            //Show error in communication
                led2 = 1;
            }

            Buff[3] = 0x00;           //clear end signals
            Cmd_Flag = 0;
            led = 0;
            //Send out message threw Cmd
//            Cmd.printf("%c %c %c\r", Dummy[0], Dummy[1], Dummy[2]);
        }
    }
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of main funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Timebase funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void init_TIMER()                       //set TT_main{} rate
{
    TT.attach_us(&TT_main, Rms);
}
void TT_main()                          //interrupt function by TT
{
    Task_1_count = Task_1_count + 1;
    if(Task_1_count > Task_1_NN) {
        Task_1_count = 0;               //Task triggering
        Flag_1 = 1;
        Task_1 = 1;                     //show for indicator
    }
    Task_2_count = Task_2_count + 1;
    if(Task_2_count > Task_2_NN) {
        Task_2_count = 0;
        //Task triggering
        Flag_2 = 1;
        Task_2 = 1;                   //show for indicator
    }
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Timebase funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡init_IO funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void init_IO(void)                      //initialize
{
    CS1.mode(PullUp);       //A4
    CS2.mode(PullUp);       //A5
    CSG = 0;                //A6
    led = 0;
    Drive1.period_us(15000);
    Drive2.period_us(15000);
    Drive3.period_us(15000);
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of init_IO funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡controller funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//

//follow steps for discrete process in j oder
//1.update w0[n] threw input x[n] & wi[n] from last sequence
//    w0[n] = x[n] + Zigma( ai*wi[n] ), i = 1, 2, 3...j-1, j.
//    y[n] = Zigma( bi*wi[n] ), i = 0, 1, 2...j-1, j.
//2.update wi[n+1] for next sequence
//    wi[n] = wi-1[n], i = j, j-1...2, 1.

void    q_spring_Est(void)
{
    //read in from adc
    q_spring[0] = -(Kadc * adc1.read() - q_spring_0[0]);
    q_spring[1] = -(Kadc * adc2.read() - q_spring_0[1]);
    q_spring[2] = -(Kadc * adc3.read() - q_spring_0[2]);
}

void    q_act_Est(void)
{
    for(int i=0; i<3; i++) {
        q_ref[i] = -(PWM[i] - PWM_base[i]);
    }
    //q_act_est = q_ref*G1_rad_TUT(z) - q_spring
    //G1_rad_TUT(z) is done below
    for(int i=0; i<3; i++) {
        //update w0[n] threw input x[n] & wi[n] from last sequence
        G1_w_n[i][0] = q_ref[i] + G1_a[1]*G1_w_n[i][1] + G1_a[2]*G1_w_n[i][2];
        q_act_est[i] = G1_b[0]*G1_w_n[i][0] + G1_b[1]*G1_w_n[i][1] + G1_b[2]*G1_w_n[i][2];
        q_act_est[i] = q_act_est[i]*0.0001f - q_spring[i];

        //update wi[n+1]
        G1_w_n[i][2] = G1_w_n[i][1];
        G1_w_n[i][1] = G1_w_n[i][0];
    }
    //q_act_est[i] is now ready for following app
}

void    Impedence_Ref(void)
{  
    //Transform q_act_est from joint to end point
    q_act_est[1] -= Phi1_0;
    q_act_est[2] += Phi2_0;
    
    
    
    L_act_est = sqrt( LA + 2*LB*cos(q_act_est[2]) );
    P_act_est = q_act_est[1] + asin( L2*sin(q_act_est[2]) / L_act_est );
    
    
    //Calculate error in end point level
    //q_0[i] => {Hip, Tigh, Shin}
    q_err_E[0] = q_0_E[0] - q_act_est[0];       //Hip  motor#1
    q_err_E[1] = q_0_E[1] - L_act_est;       
    q_err_E[2] = q_0_E[2] - P_act_est;       
    
    
    
    for(int i=0; i<3; i++) {
        //Generate T_ref in Operational Space (OSC control perhaps?)
        //T_ref_E = PDn( q_err_E );

        //Compliance
        T_ref_WD_E[i] = K_c[i] * q_err_E[i];

        //Damping
        //update w0[n] threw input x[n] & wi[n] from last sequence
        Cref_w[i][0] = q_err_E[i] + Cref_a[1]*Cref_w[i][1];
        q_err_D_E[i] = Cref_b[0]*Cref_w[i][0] + Cref_b[1]*Cref_w[i][1];

        //update wi[n+1]
        Cref_w[i][1] = Cref_w[i][0];

        //Generate T_ref in endpoint level
        T_ref_E[i] = T_ref_WD_E[i] + B_c[i]*q_err_D_E[i];
    }

    //Transform torque to joint space, T_ref[i] = Σ( J[i][j] * T_ref_E[j] )
    J32 = -LB*sin(q_act_est[2]) / L_act_est;
    J33 = ( pow(J32,2)/LB + cos(q_act_est[2]) ) / (LC + cos(q_act_est[2]));

    //Torque of Hip
    T_ref[0] = T_ref_E[0];
    T_ref_WD[0] = T_ref_WD_E[0];

    //Torque of Tigh
    T_ref[1] = T_ref_E[2] + T_0[1];
    T_ref_WD[1] = T_ref_WD_E[2] + T_0[1];

    //Torque of Shin
    T_ref[2] = J32*T_ref_E[1] + J33*T_ref_E[2] + J32*T_0[1] + J33*T_0[2];
    T_ref_WD[2] = J32*T_ref_WD_E[1] + J33*T_ref_WD_E[2] + J32*T_0[1] + J33*T_0[2];
    
}

void    Torque_Reg(void)
{
    if(CS2 == 1) {
        
        //Transform q_act_est from end point to joint
            q_act_est[1] += Phi1_0;
            q_act_est[2] -= Phi2_0;
    
        //Normal command
        for(int i=0; i<3; i++) {
            //FeedFoward
            q_ref[i] = q_act_est[i]*Kqff + T_ref_WD[i]*KTff;    //full feedfoward
            //q_ref[i] = 0;                                       //w/o feedfoward

            //FeedBack
            T_err[i] = T_ref[i] - K_q*q_spring[i];
            T_reg_I[i] = T_reg_I[i] + T_err[i]*Kit[i];
            T_reg_I[i] =  constrain(T_reg_I[i], -200, 200);
            q_ref[i] = -(T_err[i]*Kp[i] + T_reg_I[i] + q_ref[i]);
        }
        q_ref[0] = -(q_0_E[0]/128.0f)*225;
        q_ref[0] = constrain(q_ref[0], -650, 650);
        q_ref[1] = constrain(q_ref[1], -650, 650);
        q_ref[2] = constrain(q_ref[2], -650, 650);
           
        
    } else {
        //Stretch command
        for(int i=0; i<3; i++) {
            T_reg_I[i] = 0;
            q_ref[i] = 0;
        }
    }
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of controller funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡init_q_spring_0 funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void init_q_spring_0(void)
{
    for(int i=0; i<1000; i++) {
        q_spring_0[0] = q_spring_0[0] + Kadc * adc1.read();
        q_spring_0[1] = q_spring_0[1] + Kadc * adc2.read();
        q_spring_0[2] = q_spring_0[2] + Kadc * adc3.read();
        wait_us(100);
    }
    q_spring_0[0] = q_spring_0[0] /1000.0f;
    q_spring_0[1] = q_spring_0[1] /1000.0f;
    q_spring_0[2] = q_spring_0[2] /1000.0f;
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of init_q_spring_0 funtion■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Rx_xxx funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void    Rx_irq(void)
{
    //Keep clear in Rx_irq and leave most work after Rx_end
    Buff[Recieve_index] = Cmd.getc();
    Recieve_index = Recieve_index + Rx_enable;
}

void    Rx_srt(void)
{
    //start receiving Serial message from Cmd
//    NVIC_EnableIRQ(USART1_IRQn);
    Rx_enable = 1;
}
void    Rx_end(void)
{
    //end of receiving Serial message from Cmd
//    NVIC_DisableIRQ(USART1_IRQn);
    Rx_enable = 0;
    Recieve_index = 0;
    Cmd_Flag = 1;
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Rx_irq funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//