RH_leg_controller

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
OscarLiao
Date:
Thu Dec 13 14:21:31 2018 +0000
Commit message:
RH_leg_controller

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 6626810b2241 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 13 14:21:31 2018 +0000
@@ -0,0 +1,636 @@
+/*
+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     1650            //900~1550~2200 2050
+#define PWM_Thigh   1860            //900~1550~2200 1300
+#define PWM_Shin    1680            //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■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
\ No newline at end of file
diff -r 000000000000 -r 6626810b2241 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Dec 13 14:21:31 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/a7c7b631e539
\ No newline at end of file