Oscar Liao
controller_test_endpoint_LH
main.cpp@0:80f74d98d66f, 2018-11-30 (annotated)
- Committer:
- OscarLiao
- Date:
- Fri Nov 30 13:11:38 2018 +0000
- Revision:
- 0:80f74d98d66f
LH_leg_controller
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| OscarLiao | 0:80f74d98d66f | 1 | /* |
| OscarLiao | 0:80f74d98d66f | 2 | This is a program for localized controller of one leg |
| OscarLiao | 0:80f74d98d66f | 3 | Endpoint space Impedence Control and Force/Torque feedforward |
| OscarLiao | 0:80f74d98d66f | 4 | Two input was acceptable |
| OscarLiao | 0:80f74d98d66f | 5 | q_0_E[3] Neutral point for Impedence reference |
| OscarLiao | 0:80f74d98d66f | 6 | T_0[3] Feedforward of Force/Torque |
| OscarLiao | 0:80f74d98d66f | 7 | |
| OscarLiao | 0:80f74d98d66f | 8 | ---------o------↺ q_act_est[0] |
| OscarLiao | 0:80f74d98d66f | 9 | /| |
| OscarLiao | 0:80f74d98d66f | 10 | L1 / | ⤸ q_act_est[1] |
| OscarLiao | 0:80f74d98d66f | 11 | o | |
| OscarLiao | 0:80f74d98d66f | 12 | L2 ／/ |
| OscarLiao | 0:80f74d98d66f | 13 | ／ / ⤸ q_act_est[2] |
| OscarLiao | 0:80f74d98d66f | 14 | o / |
| OscarLiao | 0:80f74d98d66f | 15 | MCU target: F303K8 |
| OscarLiao | 0:80f74d98d66f | 16 | ADC config: 3.3 Vadc common source |
| OscarLiao | 0:80f74d98d66f | 17 | */ |
| OscarLiao | 0:80f74d98d66f | 18 | //**************** Wiring Map ********************// |
| OscarLiao | 0:80f74d98d66f | 19 | // |---------------| | |---------------| |
| OscarLiao | 0:80f74d98d66f | 20 | // Srl| PA_9 | D1 |Cmd Serail | | VIN | VIN | |
| OscarLiao | 0:80f74d98d66f | 21 | // Srl| PA_10 | D0 |Cmd Serial | | GND | GND | |
| OscarLiao | 0:80f74d98d66f | 22 | // | NRST | NRST | | | NRST | NRST | |
| OscarLiao | 0:80f74d98d66f | 23 | // | GND | GND | | | 5V | 5V | |
| OscarLiao | 0:80f74d98d66f | 24 | // | PA_12 | D2 | |Srl| A7 | PA_2 |PC Serial |
| OscarLiao | 0:80f74d98d66f | 25 | // PWM| PB_0 | D3 |Shing Drv |Dou| A6 | PA_7 |CSG |
| OscarLiao | 0:80f74d98d66f | 26 | // Dou| PB_7 | D4 |Task1 |Din| A5 | PA_6 |CS2 Stretch Cmd |
| OscarLiao | 0:80f74d98d66f | 27 | // Dou| PB_6 | D5 |Task2 |Din| A4 | PA_5 |CS1 Serial Sync |
| OscarLiao | 0:80f74d98d66f | 28 | // PWM| PB_1 | D6 |Tigh Drv |Ain| A3 | PA_4 |Shing sense |
| OscarLiao | 0:80f74d98d66f | 29 | // | PF_0 | D7 | |Ain| A2 | PA_3 |Tigh sense |
| OscarLiao | 0:80f74d98d66f | 30 | // | PF_1 | D8 | |Ain| A1 | PA_1 |Hip sense |
| OscarLiao | 0:80f74d98d66f | 31 | // PWM| PA_8 | D9 |Hip Drv | | A0 | PA_0 | |
| OscarLiao | 0:80f74d98d66f | 32 | // | PA_11 | D10 | | | AREF | AREF |VR source |
| OscarLiao | 0:80f74d98d66f | 33 | // | PB_5 | D11 | | | 3V3 | 3V3 | |
| OscarLiao | 0:80f74d98d66f | 34 | // | PB_4 | D12 | |Dou| D13 | PB_3 |led / Serial error |
| OscarLiao | 0:80f74d98d66f | 35 | // |---------------| | |---------------| |
| OscarLiao | 0:80f74d98d66f | 36 | #include "mbed.h" |
| OscarLiao | 0:80f74d98d66f | 37 | //#include "LSM9DS0_SH.h" |
| OscarLiao | 0:80f74d98d66f | 38 | //#define DEBUG 1 |
| OscarLiao | 0:80f74d98d66f | 39 | #define DEBUG 0 |
| OscarLiao | 0:80f74d98d66f | 40 | #define pi 3.141592f |
| OscarLiao | 0:80f74d98d66f | 41 | #define d2r 0.01745329f |
| OscarLiao | 0:80f74d98d66f | 42 | |
| OscarLiao | 0:80f74d98d66f | 43 | #define Rms 5000 //TT rate |
| OscarLiao | 0:80f74d98d66f | 44 | #define dt 0.015f |
| OscarLiao | 0:80f74d98d66f | 45 | #define Task_1_NN 2 |
| OscarLiao | 0:80f74d98d66f | 46 | #define Task_2_NN 199 |
| OscarLiao | 0:80f74d98d66f | 47 | |
| OscarLiao | 0:80f74d98d66f | 48 | //Torque-Controller gain |
| OscarLiao | 0:80f74d98d66f | 49 | #define G1_rad_dc 0.00157f |
| OscarLiao | 0:80f74d98d66f | 50 | #define Kqff 636.7f //ffoward gain of q_act_est = 1 / G1_rad_dc |
| OscarLiao | 0:80f74d98d66f | 51 | #define KTff 60.408f //ffoward gain of T_ref_WD = 1 / (K_q * G1_rad_dc) |
| OscarLiao | 0:80f74d98d66f | 52 | #define Kpt 22.9f |
| OscarLiao | 0:80f74d98d66f | 53 | #define Ki 151.9f |
| OscarLiao | 0:80f74d98d66f | 54 | |
| OscarLiao | 0:80f74d98d66f | 55 | //ADC reference constant |
| OscarLiao | 0:80f74d98d66f | 56 | #define Kadc 5.817f //adc value to q_spring |
| OscarLiao | 0:80f74d98d66f | 57 | #define K_spring 13180f //sping constant of single spring |
| OscarLiao | 0:80f74d98d66f | 58 | #define K_q 10.544f //2*K_spring*R^2 |
| OscarLiao | 0:80f74d98d66f | 59 | #define R 0.02f //wheely's radius |
| OscarLiao | 0:80f74d98d66f | 60 | |
| OscarLiao | 0:80f74d98d66f | 61 | //Endpoint Impedence Contorller coefficcient |
| OscarLiao | 0:80f74d98d66f | 62 | #define Kpi_Hip 0 |
| OscarLiao | 0:80f74d98d66f | 63 | #define Kpi_Thigh 2000 |
| OscarLiao | 0:80f74d98d66f | 64 | #define Kpi_Shin 8 |
| OscarLiao | 0:80f74d98d66f | 65 | #define Kd_Hip 0.0 |
| OscarLiao | 0:80f74d98d66f | 66 | #define Kd_Thigh 0.0 |
| OscarLiao | 0:80f74d98d66f | 67 | #define Kd_Shin 0.0 |
| OscarLiao | 0:80f74d98d66f | 68 | #define KN 20 |
| OscarLiao | 0:80f74d98d66f | 69 | #define L1 0.120f //Tigh length |
| OscarLiao | 0:80f74d98d66f | 70 | #define L2 0.1480f //Shing lenth |
| OscarLiao | 0:80f74d98d66f | 71 | #define LA (L1*L1+L2*L2) //L1^2 + L2^2 |
| OscarLiao | 0:80f74d98d66f | 72 | #define LB (L1*L2) //L1*L2 |
| OscarLiao | 0:80f74d98d66f | 73 | #define LC (L1/L2) //L1/L2 |
| OscarLiao | 0:80f74d98d66f | 74 | #define L0 0.2f //Length of the leg |
| OscarLiao | 0:80f74d98d66f | 75 | |
| OscarLiao | 0:80f74d98d66f | 76 | //Structure |
| OscarLiao | 0:80f74d98d66f | 77 | #define PWM_Hip 1410 //900~1550~2200 2050 |
| OscarLiao | 0:80f74d98d66f | 78 | #define PWM_Thigh 1490 //900~1550~2200 1300 |
| OscarLiao | 0:80f74d98d66f | 79 | #define PWM_Shin 1710 //900~1550~2200 1450 1740 |
| OscarLiao | 0:80f74d98d66f | 80 | |
| OscarLiao | 0:80f74d98d66f | 81 | #define Buff_size 16 //Serial Buffer |
| OscarLiao | 0:80f74d98d66f | 82 | |
| OscarLiao | 0:80f74d98d66f | 83 | #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) |
| OscarLiao | 0:80f74d98d66f | 84 | |
| OscarLiao | 0:80f74d98d66f | 85 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
| OscarLiao | 0:80f74d98d66f | 86 | //╔═════════════════╗ |
| OscarLiao | 0:80f74d98d66f | 87 | //║ Structure ║ |
| OscarLiao | 0:80f74d98d66f | 88 | //╚═════════════════╝ |
| OscarLiao | 0:80f74d98d66f | 89 | DigitalOut led(D13); //detection |
| OscarLiao | 0:80f74d98d66f | 90 | DigitalOut led2(D12); //detection |
| OscarLiao | 0:80f74d98d66f | 91 | DigitalOut Task_1(D4); //Task indicate |
| OscarLiao | 0:80f74d98d66f | 92 | DigitalOut Task_2(D5); |
| OscarLiao | 0:80f74d98d66f | 93 | DigitalOut CSG(A6); //Stretch interrupt pull down |
| OscarLiao | 0:80f74d98d66f | 94 | |
| OscarLiao | 0:80f74d98d66f | 95 | AnalogIn adc1(A1); //adc |
| OscarLiao | 0:80f74d98d66f | 96 | AnalogIn adc2(A2); //adc |
| OscarLiao | 0:80f74d98d66f | 97 | AnalogIn adc3(A3); //adc |
| OscarLiao | 0:80f74d98d66f | 98 | |
| OscarLiao | 0:80f74d98d66f | 99 | InterruptIn CS1(D2); //Serial synchronizor |
| OscarLiao | 0:80f74d98d66f | 100 | DigitalIn CS2(A5); //Stretch interrupt |
| OscarLiao | 0:80f74d98d66f | 101 | |
| OscarLiao | 0:80f74d98d66f | 102 | |
| OscarLiao | 0:80f74d98d66f | 103 | //╔═════════════════╗ |
| OscarLiao | 0:80f74d98d66f | 104 | //║ Servo out ║ |
| OscarLiao | 0:80f74d98d66f | 105 | //╚═════════════════╝ |
| OscarLiao | 0:80f74d98d66f | 106 | PwmOut Drive1(D9); //PWM 1/1 |
| OscarLiao | 0:80f74d98d66f | 107 | PwmOut Drive2(D6); //PWM 1/2N |
| OscarLiao | 0:80f74d98d66f | 108 | PwmOut Drive3(D3); //PWM 1/3N |
| OscarLiao | 0:80f74d98d66f | 109 | |
| OscarLiao | 0:80f74d98d66f | 110 | //╔═════════════════╗ |
| OscarLiao | 0:80f74d98d66f | 111 | //║ Serial ║ |
| OscarLiao | 0:80f74d98d66f | 112 | //╚═════════════════╝ |
| OscarLiao | 0:80f74d98d66f | 113 | Serial Debug(PA_2, PA_15); //Serial reg(TX RX) USB port |
| OscarLiao | 0:80f74d98d66f | 114 | Serial Cmd(D1, D0); //Serial reg(TX RX) |
| OscarLiao | 0:80f74d98d66f | 115 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of GPIO registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
| OscarLiao | 0:80f74d98d66f | 116 | |
| OscarLiao | 0:80f74d98d66f | 117 | |
| OscarLiao | 0:80f74d98d66f | 118 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Varible registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
| OscarLiao | 0:80f74d98d66f | 119 | //╔═════════════════╗ |
| OscarLiao | 0:80f74d98d66f | 120 | //║ Structure ║ |
| OscarLiao | 0:80f74d98d66f | 121 | //╚═════════════════╝ |
| OscarLiao | 0:80f74d98d66f | 122 | Ticker TT; //call a timer |
| OscarLiao | 0:80f74d98d66f | 123 | uint8_t Task_1_count = 0; //1st prior task count |
| OscarLiao | 0:80f74d98d66f | 124 | uint8_t Task_2_count = 0; //2nd prior task count |
| OscarLiao | 0:80f74d98d66f | 125 | uint8_t Flag_1 = 0; //1st prior task flag |
| OscarLiao | 0:80f74d98d66f | 126 | uint8_t Flag_2 = 0; //2nd prior task flag |
| OscarLiao | 0:80f74d98d66f | 127 | |
| OscarLiao | 0:80f74d98d66f | 128 | //╔═════════════════╗ |
| OscarLiao | 0:80f74d98d66f | 129 | //║ ADC ║ |
| OscarLiao | 0:80f74d98d66f | 130 | //╚═════════════════╝ |
| OscarLiao | 0:80f74d98d66f | 131 | |
| OscarLiao | 0:80f74d98d66f | 132 | //╔═════════════════╗ |
| OscarLiao | 0:80f74d98d66f | 133 | //║ PWM ║ |
| OscarLiao | 0:80f74d98d66f | 134 | //╚═════════════════╝ |
| OscarLiao | 0:80f74d98d66f | 135 | const int16_t PWM_base[3] = { //reference command at 0, -45, 45 deg |
| OscarLiao | 0:80f74d98d66f | 136 | PWM_Hip,PWM_Thigh,PWM_Shin |
| OscarLiao | 0:80f74d98d66f | 137 | }; |
| OscarLiao | 0:80f74d98d66f | 138 | int16_t PWM[3] = { //command out PWM_base + q_ref |
| OscarLiao | 0:80f74d98d66f | 139 | 0, 0, 0 //transfer: 10us to 1 deg |
| OscarLiao | 0:80f74d98d66f | 140 | }; |
| OscarLiao | 0:80f74d98d66f | 141 | |
| OscarLiao | 0:80f74d98d66f | 142 | //╔═════════════════╗ |
| OscarLiao | 0:80f74d98d66f | 143 | //║ q_actEstimator ║ |
| OscarLiao | 0:80f74d98d66f | 144 | //╚═════════════════╝ |
| OscarLiao | 0:80f74d98d66f | 145 | /* |
| OscarLiao | 0:80f74d98d66f | 146 | q_act_est = q_ref*G1_rad_TUT(z) - q_spring |
| OscarLiao | 0:80f74d98d66f | 147 | |
| OscarLiao | 0:80f74d98d66f | 148 | G1_rad_TUT(z) is shown below |
| OscarLiao | 0:80f74d98d66f | 149 | |
| OscarLiao | 0:80f74d98d66f | 150 | y(z) 1.316 + 2.633*z^-1 + 1.316*z^-2 b0 + b1*z^-1 + b2*z^-2 |
| OscarLiao | 0:80f74d98d66f | 151 | ----- = ----------------------------------- * 1e-04 = ------------------------ |
| OscarLiao | 0:80f74d98d66f | 152 | x(z) 1 - 0.8447*z^-1 + 0.1799*z^-2 1 - a1*z^-1 - a2*z^-2 |
| OscarLiao | 0:80f74d98d66f | 153 | |
| OscarLiao | 0:80f74d98d66f | 154 | q_ref═>[Σ]════════╦═>[b0]═>[Σ]═[1e-04]═>y |
| OscarLiao | 0:80f74d98d66f | 155 | ⇑ ⇓w0 ⇑ |
| OscarLiao | 0:80f74d98d66f | 156 | ║ [1/z] ║ |
| OscarLiao | 0:80f74d98d66f | 157 | [Σ]<═[a1]<═╬═>[b1]═>[Σ] |
| OscarLiao | 0:80f74d98d66f | 158 | ⇑ ⇓w1 ⇑ |
| OscarLiao | 0:80f74d98d66f | 159 | ║ [1/z] ║ |
| OscarLiao | 0:80f74d98d66f | 160 | ╚═══[a2]<═╩═>[b2]═══╝ |
| OscarLiao | 0:80f74d98d66f | 161 | w2 |
| OscarLiao | 0:80f74d98d66f | 162 | */ |
| OscarLiao | 0:80f74d98d66f | 163 | float q_act_est[3] = { //output of filter |
| OscarLiao | 0:80f74d98d66f | 164 | 0, 0, 0 |
| OscarLiao | 0:80f74d98d66f | 165 | }; |
| OscarLiao | 0:80f74d98d66f | 166 | |
| OscarLiao | 0:80f74d98d66f | 167 | float L_act_est = 0.0; |
| OscarLiao | 0:80f74d98d66f | 168 | float P_act_est = 0.0; |
| OscarLiao | 0:80f74d98d66f | 169 | |
| OscarLiao | 0:80f74d98d66f | 170 | float G1_w_n[3][3] = { //G1_rad_TUT's |
| OscarLiao | 0:80f74d98d66f | 171 | {0, 0, 0}, //{w0, w1, w2} |
| OscarLiao | 0:80f74d98d66f | 172 | {0, 0, 0}, //second sea |
| OscarLiao | 0:80f74d98d66f | 173 | {0, 0, 0}, |
| OscarLiao | 0:80f74d98d66f | 174 | }; |
| OscarLiao | 0:80f74d98d66f | 175 | const float G1_a[3] = { //{a0, a1, a2} |
| OscarLiao | 0:80f74d98d66f | 176 | 1.00, 0.8447, -0.1799 |
| OscarLiao | 0:80f74d98d66f | 177 | }; |
| OscarLiao | 0:80f74d98d66f | 178 | const float G1_b[3] = { //{b0, b1, b2} |
| OscarLiao | 0:80f74d98d66f | 179 | 1.316, 2.633, 1.316 |
| OscarLiao | 0:80f74d98d66f | 180 | }; |
| OscarLiao | 0:80f74d98d66f | 181 | |
| OscarLiao | 0:80f74d98d66f | 182 | //╔═════════════════╗//╔══════════════════╗ |
| OscarLiao | 0:80f74d98d66f | 183 | //║ Comp_ref ║//║ Controller Input ║ |
| OscarLiao | 0:80f74d98d66f | 184 | //╚═════════════════╝//╚══════════════════╝ |
| OscarLiao | 0:80f74d98d66f | 185 | /* |
| OscarLiao | 0:80f74d98d66f | 186 | |
| OscarLiao | 0:80f74d98d66f | 187 | ---------o------↺ q_act_est[0] |
| OscarLiao | 0:80f74d98d66f | 188 | /| |
| OscarLiao | 0:80f74d98d66f | 189 | L1 / | ↻ q_act_est[1] |
| OscarLiao | 0:80f74d98d66f | 190 | o | |
| OscarLiao | 0:80f74d98d66f | 191 | L2 ／/ |
| OscarLiao | 0:80f74d98d66f | 192 | ／ / ↻ q_act_est[2] |
| OscarLiao | 0:80f74d98d66f | 193 | o / |
| OscarLiao | 0:80f74d98d66f | 194 | |
| OscarLiao | 0:80f74d98d66f | 195 | 1 - z^-1 |
| OscarLiao | 0:80f74d98d66f | 196 | PDn(z) = P + D*N*--------------------- |
| OscarLiao | 0:80f74d98d66f | 197 | 1 - (1 - N*Ts)*z^-1 |
| OscarLiao | 0:80f74d98d66f | 198 | |
| OscarLiao | 0:80f74d98d66f | 199 | y(z) b0 + b1*z^-1 1 + (-1)*z^-1 |
| OscarLiao | 0:80f74d98d66f | 200 | ----- = -------------- = ------------------ |
| OscarLiao | 0:80f74d98d66f | 201 | x(z) 1 - a1*z^-1 1 - 0.7*z^-1 |
| OscarLiao | 0:80f74d98d66f | 202 | |
| OscarLiao | 0:80f74d98d66f | 203 | x═>[Σ]═══════╦═>[b0]═>[Σ]═>[D*N]═>y |
| OscarLiao | 0:80f74d98d66f | 204 | ⇑ ⇓w0 ⇑ |
| OscarLiao | 0:80f74d98d66f | 205 | ║ [1/z] ║ |
| OscarLiao | 0:80f74d98d66f | 206 | ╚══[a1]<═╩═>[b1]═══╝ |
| OscarLiao | 0:80f74d98d66f | 207 | w1 |
| OscarLiao | 0:80f74d98d66f | 208 | */ |
| OscarLiao | 0:80f74d98d66f | 209 | |
| OscarLiao | 0:80f74d98d66f | 210 | float K_c[3] = { //K_c reference impedence |
| OscarLiao | 0:80f74d98d66f | 211 | Kpi_Hip, Kpi_Thigh, Kpi_Shin //{Hip, Thigh, Shin} |
| OscarLiao | 0:80f74d98d66f | 212 | }; |
| OscarLiao | 0:80f74d98d66f | 213 | float B_c[3] = { //B_c = D*N (*20)reference damping (Dmax~0.4)(B_c_max~5) in joint!! |
| OscarLiao | 0:80f74d98d66f | 214 | Kd_Hip*KN, Kd_Thigh*KN, Kd_Shin*KN //{Hip, Thigh, Shin} |
| OscarLiao | 0:80f74d98d66f | 215 | }; |
| OscarLiao | 0:80f74d98d66f | 216 | float q_0_E[3] = { //"Controller Input" from upper controller in endpoint level (Operational Space) |
| OscarLiao | 0:80f74d98d66f | 217 | 0, 0, 0 //{Hip, leg length, leg angle} original point |
| OscarLiao | 0:80f74d98d66f | 218 | }; |
| OscarLiao | 0:80f74d98d66f | 219 | float T_0[3] = { //"Controller Input" from upper controller in joint level |
| OscarLiao | 0:80f74d98d66f | 220 | 0, 0, 0 //{T_0_Hip, T_0_Tigh, F_0_Stretch} force feed foward |
| OscarLiao | 0:80f74d98d66f | 221 | }; |
| OscarLiao | 0:80f74d98d66f | 222 | float q_err_E[3] = { //error in endpoint level |
| OscarLiao | 0:80f74d98d66f | 223 | 0, 0, 0 |
| OscarLiao | 0:80f74d98d66f | 224 | }; |
| OscarLiao | 0:80f74d98d66f | 225 | float q_err_D_E[3] = { //derivative error in endpoint level |
| OscarLiao | 0:80f74d98d66f | 226 | 0, 0, 0 |
| OscarLiao | 0:80f74d98d66f | 227 | }; |
| OscarLiao | 0:80f74d98d66f | 228 | float Cref_w[3][2] = { //filter storage |
| OscarLiao | 0:80f74d98d66f | 229 | {0, 0}, |
| OscarLiao | 0:80f74d98d66f | 230 | {0, 0}, |
| OscarLiao | 0:80f74d98d66f | 231 | {0, 0}, |
| OscarLiao | 0:80f74d98d66f | 232 | }; |
| OscarLiao | 0:80f74d98d66f | 233 | const float Cref_a[2] = { //{a0, a1} |
| OscarLiao | 0:80f74d98d66f | 234 | 1.00, 0.70 |
| OscarLiao | 0:80f74d98d66f | 235 | }; |
| OscarLiao | 0:80f74d98d66f | 236 | const float Cref_b[2] = { //{b0, b1} |
| OscarLiao | 0:80f74d98d66f | 237 | 1.00, -1.00 |
| OscarLiao | 0:80f74d98d66f | 238 | }; |
| OscarLiao | 0:80f74d98d66f | 239 | float J32 = 0.0; |
| OscarLiao | 0:80f74d98d66f | 240 | float J33 = 0.0; |
| OscarLiao | 0:80f74d98d66f | 241 | float Phi1_0 = 0.0; |
| OscarLiao | 0:80f74d98d66f | 242 | float Phi2_0 = 0.0; |
| OscarLiao | 0:80f74d98d66f | 243 | |
| OscarLiao | 0:80f74d98d66f | 244 | //╔═════════════════╗ |
| OscarLiao | 0:80f74d98d66f | 245 | //║ Torque_reg ║ |
| OscarLiao | 0:80f74d98d66f | 246 | //╚═════════════════╝ |
| OscarLiao | 0:80f74d98d66f | 247 | /* |
| OscarLiao | 0:80f74d98d66f | 248 | q_act_est T_ref_WD |
| OscarLiao | 0:80f74d98d66f | 249 | q_ref = ----------- + -------------------------- + PI( T_ref - 2*K_spring*R^2*q_spring ) |
| OscarLiao | 0:80f74d98d66f | 250 | G1_rad_dc 2*K_spring*R^2*G1_rad_dc |
| OscarLiao | 0:80f74d98d66f | 251 | |
| OscarLiao | 0:80f74d98d66f | 252 | PI(z) = P + I*dt / (z-1) |
| OscarLiao | 0:80f74d98d66f | 253 | */ |
| OscarLiao | 0:80f74d98d66f | 254 | const float Kp[3] = { //P gain for Torque REG |
| OscarLiao | 0:80f74d98d66f | 255 | Kpt, Kpt, Kpt //{Hip, Tigh, Shin} |
| OscarLiao | 0:80f74d98d66f | 256 | }; |
| OscarLiao | 0:80f74d98d66f | 257 | const float Kit[3] = { //I gain for Torque REG Kit = Ki*Ts |
| OscarLiao | 0:80f74d98d66f | 258 | Ki*dt, Ki*dt, Ki*dt //{Hip, Tigh, Shin} |
| OscarLiao | 0:80f74d98d66f | 259 | }; |
| OscarLiao | 0:80f74d98d66f | 260 | float T_ref[3] = { //command to T_reg in joint level |
| OscarLiao | 0:80f74d98d66f | 261 | 0, 0, 0 //{Hip, Tigh, Shin} |
| OscarLiao | 0:80f74d98d66f | 262 | }; |
| OscarLiao | 0:80f74d98d66f | 263 | float T_ref_E[3] = { //command to T_reg in end point level |
| OscarLiao | 0:80f74d98d66f | 264 | 0, 0, 0 //{Hip, Len, Phi} |
| OscarLiao | 0:80f74d98d66f | 265 | }; |
| OscarLiao | 0:80f74d98d66f | 266 | float T_ref_WD[3] = { //command without damping reference in joint level |
| OscarLiao | 0:80f74d98d66f | 267 | 0, 0, 0 |
| OscarLiao | 0:80f74d98d66f | 268 | }; |
| OscarLiao | 0:80f74d98d66f | 269 | float T_ref_WD_E[3] = { //command without damping reference in endpoint level |
| OscarLiao | 0:80f74d98d66f | 270 | 0, 0, 0 |
| OscarLiao | 0:80f74d98d66f | 271 | }; |
| OscarLiao | 0:80f74d98d66f | 272 | float T_err[3] = { //T_ref - T_act_est, T_act_est = q_spring * K_q |
| OscarLiao | 0:80f74d98d66f | 273 | 0, 0, 0 //{Hip, Tigh, Shin} |
| OscarLiao | 0:80f74d98d66f | 274 | }; |
| OscarLiao | 0:80f74d98d66f | 275 | float q_ref[3] = { //output of T_reg |
| OscarLiao | 0:80f74d98d66f | 276 | 0, 0, 0 //{Hip, Thigh, Shin} |
| OscarLiao | 0:80f74d98d66f | 277 | }; |
| OscarLiao | 0:80f74d98d66f | 278 | float q_spring[3] = { //read in spring travel |
| OscarLiao | 0:80f74d98d66f | 279 | 0, 0, 0 |
| OscarLiao | 0:80f74d98d66f | 280 | }; |
| OscarLiao | 0:80f74d98d66f | 281 | float q_spring_0[3] = { //neutral point of VR |
| OscarLiao | 0:80f74d98d66f | 282 | 0, 0, 0 |
| OscarLiao | 0:80f74d98d66f | 283 | }; |
| OscarLiao | 0:80f74d98d66f | 284 | float T_reg_I[3] = { //PI_w |
| OscarLiao | 0:80f74d98d66f | 285 | 0, 0, 0 |
| OscarLiao | 0:80f74d98d66f | 286 | }; |
| OscarLiao | 0:80f74d98d66f | 287 | |
| OscarLiao | 0:80f74d98d66f | 288 | //╔═════════════════╗ |
| OscarLiao | 0:80f74d98d66f | 289 | //║ sine signal ║ |
| OscarLiao | 0:80f74d98d66f | 290 | //╚═════════════════╝ |
| OscarLiao | 0:80f74d98d66f | 291 | int data = 0; |
| OscarLiao | 0:80f74d98d66f | 292 | float amp = 0.1; |
| OscarLiao | 0:80f74d98d66f | 293 | float freq = 55; //rad/s |
| OscarLiao | 0:80f74d98d66f | 294 | int totalticks = 0; |
| OscarLiao | 0:80f74d98d66f | 295 | int ticks = 0; |
| OscarLiao | 0:80f74d98d66f | 296 | |
| OscarLiao | 0:80f74d98d66f | 297 | //╔═════════════════╗ |
| OscarLiao | 0:80f74d98d66f | 298 | //║ I/O Serial ║ |
| OscarLiao | 0:80f74d98d66f | 299 | //╚═════════════════╝ |
| OscarLiao | 0:80f74d98d66f | 300 | uint8_t Buff[Buff_size]; |
| OscarLiao | 0:80f74d98d66f | 301 | uint8_t Recieve_index = 0; |
| OscarLiao | 0:80f74d98d66f | 302 | uint8_t Cmd_Flag = 0; |
| OscarLiao | 0:80f74d98d66f | 303 | uint8_t Rx_enable = 0; |
| OscarLiao | 0:80f74d98d66f | 304 | char buffer[128]; |
| OscarLiao | 0:80f74d98d66f | 305 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
| OscarLiao | 0:80f74d98d66f | 306 | |
| OscarLiao | 0:80f74d98d66f | 307 | |
| OscarLiao | 0:80f74d98d66f | 308 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Function registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
| OscarLiao | 0:80f74d98d66f | 309 | void init_TIMER(); //set TT_main() rate |
| OscarLiao | 0:80f74d98d66f | 310 | void TT_main(); //timebase function rated by TT |
| OscarLiao | 0:80f74d98d66f | 311 | void init_q_spring_0(); //calibrating |
| OscarLiao | 0:80f74d98d66f | 312 | void init_IO(); //initialize IO state |
| OscarLiao | 0:80f74d98d66f | 313 | |
| OscarLiao | 0:80f74d98d66f | 314 | void q_spring_Est(); |
| OscarLiao | 0:80f74d98d66f | 315 | void q_act_Est(); |
| OscarLiao | 0:80f74d98d66f | 316 | void Impedence_Ref(); |
| OscarLiao | 0:80f74d98d66f | 317 | void Torque_Reg(); |
| OscarLiao | 0:80f74d98d66f | 318 | |
| OscarLiao | 0:80f74d98d66f | 319 | void Rx_irq(); |
| OscarLiao | 0:80f74d98d66f | 320 | void Rx_srt(); |
| OscarLiao | 0:80f74d98d66f | 321 | void Rx_end(); |
| OscarLiao | 0:80f74d98d66f | 322 | //void NVIC_DisableIRQ(USART1_IRQn); |
| OscarLiao | 0:80f74d98d66f | 323 | //void NVIC_EnableIRQ(USART1_IRQn); |
| OscarLiao | 0:80f74d98d66f | 324 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
| OscarLiao | 0:80f74d98d66f | 325 | |
| OscarLiao | 0:80f74d98d66f | 326 | |
| OscarLiao | 0:80f74d98d66f | 327 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡main funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
| OscarLiao | 0:80f74d98d66f | 328 | int main() |
| OscarLiao | 0:80f74d98d66f | 329 | { |
| OscarLiao | 0:80f74d98d66f | 330 | Debug.baud(115200); //set baud rate |
| OscarLiao | 0:80f74d98d66f | 331 | Cmd.baud(115200); //set baud rate |
| OscarLiao | 0:80f74d98d66f | 332 | |
| OscarLiao | 0:80f74d98d66f | 333 | wait_ms(10); |
| OscarLiao | 0:80f74d98d66f | 334 | init_q_spring_0(); //calibrating q_spring reference point |
| OscarLiao | 0:80f74d98d66f | 335 | |
| OscarLiao | 0:80f74d98d66f | 336 | wait_ms(600); |
| OscarLiao | 0:80f74d98d66f | 337 | init_IO(); //initialized value |
| OscarLiao | 0:80f74d98d66f | 338 | init_TIMER(); //start TT_main |
| OscarLiao | 0:80f74d98d66f | 339 | |
| OscarLiao | 0:80f74d98d66f | 340 | //for sine signal |
| OscarLiao | 0:80f74d98d66f | 341 | totalticks = ((2*pi)/freq)/((Rms/1E6)*(Task_2_NN+1)); |
| OscarLiao | 0:80f74d98d66f | 342 | Cmd.attach(&Rx_irq,Serial::RxIrq); //start recieving message |
| OscarLiao | 0:80f74d98d66f | 343 | // NVIC_DisableIRQ(USART1_IRQn); //stay down before CS1 signal |
| OscarLiao | 0:80f74d98d66f | 344 | |
| OscarLiao | 0:80f74d98d66f | 345 | CS1.fall(&Rx_srt); //attach the address of Rx_srt to falling edge as start signal of communication |
| OscarLiao | 0:80f74d98d66f | 346 | CS1.rise(&Rx_end); //attach the address of Rx_end to rise edge |
| OscarLiao | 0:80f74d98d66f | 347 | |
| OscarLiao | 0:80f74d98d66f | 348 | q_0_E[0] = 0; |
| OscarLiao | 0:80f74d98d66f | 349 | q_0_E[1] = 0.2; |
| OscarLiao | 0:80f74d98d66f | 350 | q_0_E[2] = 0.0; |
| OscarLiao | 0:80f74d98d66f | 351 | |
| OscarLiao | 0:80f74d98d66f | 352 | Phi1_0 = (acos((L0*L0+L1*L1-L2*L2)/(2*L0*L1))); |
| OscarLiao | 0:80f74d98d66f | 353 | Phi2_0 = (Phi1_0 + acos((L0*L0+L2*L2-L1*L1)/(2*L0*L2))); |
| OscarLiao | 0:80f74d98d66f | 354 | |
| OscarLiao | 0:80f74d98d66f | 355 | while(1) { //main() loop |
| OscarLiao | 0:80f74d98d66f | 356 | |
| OscarLiao | 0:80f74d98d66f | 357 | //Task judging start |
| OscarLiao | 0:80f74d98d66f | 358 | if(Flag_1 == 1) { //check pending |
| OscarLiao | 0:80f74d98d66f | 359 | //Task_1 start |
| OscarLiao | 0:80f74d98d66f | 360 | //read in condition & estimate state |
| OscarLiao | 0:80f74d98d66f | 361 | q_spring_Est(); |
| OscarLiao | 0:80f74d98d66f | 362 | q_act_Est(); |
| OscarLiao | 0:80f74d98d66f | 363 | |
| OscarLiao | 0:80f74d98d66f | 364 | //calculate control effort |
| OscarLiao | 0:80f74d98d66f | 365 | Impedence_Ref(); |
| OscarLiao | 0:80f74d98d66f | 366 | Torque_Reg(); |
| OscarLiao | 0:80f74d98d66f | 367 | |
| OscarLiao | 0:80f74d98d66f | 368 | //PWM Drive out process |
| OscarLiao | 0:80f74d98d66f | 369 | for(int i=0; i<3; i++) { |
| OscarLiao | 0:80f74d98d66f | 370 | PWM[i] = PWM_base[i] + q_ref[i]; // X + Controll |
| OscarLiao | 0:80f74d98d66f | 371 | } |
| OscarLiao | 0:80f74d98d66f | 372 | //PWM[0] = 1460; |
| OscarLiao | 0:80f74d98d66f | 373 | //PWM[1] = 1330; |
| OscarLiao | 0:80f74d98d66f | 374 | //PWM[2] = 1830; |
| OscarLiao | 0:80f74d98d66f | 375 | |
| OscarLiao | 0:80f74d98d66f | 376 | PWM[0] = constrain(PWM[0],800 ,2100);//safty constrain |
| OscarLiao | 0:80f74d98d66f | 377 | PWM[1] = constrain(PWM[1],800 ,2100);//safty constrain |
| OscarLiao | 0:80f74d98d66f | 378 | PWM[2] = constrain(PWM[2],800 ,2100);//safty constrain |
| OscarLiao | 0:80f74d98d66f | 379 | |
| OscarLiao | 0:80f74d98d66f | 380 | Drive1.pulsewidth_us(PWM[0]); //drive command |
| OscarLiao | 0:80f74d98d66f | 381 | Drive2.pulsewidth_us(PWM[1]); |
| OscarLiao | 0:80f74d98d66f | 382 | Drive3.pulsewidth_us(PWM[2]); |
| OscarLiao | 0:80f74d98d66f | 383 | |
| OscarLiao | 0:80f74d98d66f | 384 | //for Serial-Oscilloscope |
| OscarLiao | 0:80f74d98d66f | 385 | #if DEBUG |
| OscarLiao | 0:80f74d98d66f | 386 | // q_0[2] = -0.9; |
| OscarLiao | 0:80f74d98d66f | 387 | // Debug.printf("%f\n", q_act_est[1]); |
| OscarLiao | 0:80f74d98d66f | 388 | // Debug.printf("%f, %f, %f\r", q_0[0], q_0[1], q_0[2]); |
| OscarLiao | 0:80f74d98d66f | 389 | Debug.printf("%lf\r", adc3.read()); |
| OscarLiao | 0:80f74d98d66f | 390 | // Debug.printf("%f\n", 123.23); |
| OscarLiao | 0:80f74d98d66f | 391 | // Debug.printf("%.f\r", q_ref[0]);L_act_est |
| OscarLiao | 0:80f74d98d66f | 392 | // Debug.printf("%.2f\r", q_spring_0[0]); |
| OscarLiao | 0:80f74d98d66f | 393 | // Debug.printf("%.2f\r", J33); |
| OscarLiao | 0:80f74d98d66f | 394 | #endif |
| OscarLiao | 0:80f74d98d66f | 395 | |
| OscarLiao | 0:80f74d98d66f | 396 | //Task_1 done |
| OscarLiao | 0:80f74d98d66f | 397 | Flag_1 = 0; //clear pending |
| OscarLiao | 0:80f74d98d66f | 398 | Task_1 = 0; //clear sign |
| OscarLiao | 0:80f74d98d66f | 399 | } |
| OscarLiao | 0:80f74d98d66f | 400 | #if DEBUG |
| OscarLiao | 0:80f74d98d66f | 401 | if(Flag_2 == 1) { //check pending |
| OscarLiao | 0:80f74d98d66f | 402 | //Task_2 start |
| OscarLiao | 0:80f74d98d66f | 403 | |
| OscarLiao | 0:80f74d98d66f | 404 | //%Debug.printf("%d, %d, %d\r", PWM[0], PWM[1], PWM[2]); |
| OscarLiao | 0:80f74d98d66f | 405 | // Debug.printf("%f\n", q_act_est[2]); |
| OscarLiao | 0:80f74d98d66f | 406 | // |
| OscarLiao | 0:80f74d98d66f | 407 | // if(data) |
| OscarLiao | 0:80f74d98d66f | 408 | // { |
| OscarLiao | 0:80f74d98d66f | 409 | // //generate sine signal |
| OscarLiao | 0:80f74d98d66f | 410 | // //q_0[2] = amp*sin((ticks/(float)totalticks)*(2*pi)); |
| OscarLiao | 0:80f74d98d66f | 411 | //// ticks++; |
| OscarLiao | 0:80f74d98d66f | 412 | //// ticks = ticks%(totalticks+1); |
| OscarLiao | 0:80f74d98d66f | 413 | // //Debug.printf("%f\n", q_act_est[2]); |
| OscarLiao | 0:80f74d98d66f | 414 | // q_0[2] = 0.5; |
| OscarLiao | 0:80f74d98d66f | 415 | // } |
| OscarLiao | 0:80f74d98d66f | 416 | // else |
| OscarLiao | 0:80f74d98d66f | 417 | // { |
| OscarLiao | 0:80f74d98d66f | 418 | // q_0[2] = 0; |
| OscarLiao | 0:80f74d98d66f | 419 | // } |
| OscarLiao | 0:80f74d98d66f | 420 | |
| OscarLiao | 0:80f74d98d66f | 421 | //Task_2 done |
| OscarLiao | 0:80f74d98d66f | 422 | |
| OscarLiao | 0:80f74d98d66f | 423 | Flag_2 = 0; //clear pending |
| OscarLiao | 0:80f74d98d66f | 424 | Task_2 = 0; |
| OscarLiao | 0:80f74d98d66f | 425 | } |
| OscarLiao | 0:80f74d98d66f | 426 | #endif |
| OscarLiao | 0:80f74d98d66f | 427 | //data test |
| OscarLiao | 0:80f74d98d66f | 428 | if(Debug.readable()) { |
| OscarLiao | 0:80f74d98d66f | 429 | // Debug.gets(buffer, 4); |
| OscarLiao | 0:80f74d98d66f | 430 | // q_0[0] = ((pi/2)/256) * (buffer[0] - 238); |
| OscarLiao | 0:80f74d98d66f | 431 | // q_0[1] = ((pi/2)/256) * (buffer[1] - 128); |
| OscarLiao | 0:80f74d98d66f | 432 | // q_0[2] = ((pi/2)/256) * (buffer[2] - 128); |
| OscarLiao | 0:80f74d98d66f | 433 | q_0_E[1] -= 0.05; |
| OscarLiao | 0:80f74d98d66f | 434 | Debug.getc(); |
| OscarLiao | 0:80f74d98d66f | 435 | data = !data; |
| OscarLiao | 0:80f74d98d66f | 436 | } |
| OscarLiao | 0:80f74d98d66f | 437 | |
| OscarLiao | 0:80f74d98d66f | 438 | if(Cmd_Flag == 1) { |
| OscarLiao | 0:80f74d98d66f | 439 | //{...}{Hip, Len, Phi}, {'#'} |
| OscarLiao | 0:80f74d98d66f | 440 | //Received some shit, start decoding |
| OscarLiao | 0:80f74d98d66f | 441 | led = 1; |
| OscarLiao | 0:80f74d98d66f | 442 | if(Buff[3] == 'Q') { |
| OscarLiao | 0:80f74d98d66f | 443 | //q_0 command |
| OscarLiao | 0:80f74d98d66f | 444 | q_0_E[0] = (Buff[0]); |
| OscarLiao | 0:80f74d98d66f | 445 | q_0_E[1] = 0.1f + 0.2f * (Buff[1]/255.0f); |
| OscarLiao | 0:80f74d98d66f | 446 | q_0_E[2] = (Buff[2]); |
| OscarLiao | 0:80f74d98d66f | 447 | //Debug.printf("%d %f\n", Buff[1], q_0_E[1]); |
| OscarLiao | 0:80f74d98d66f | 448 | } else if(Buff[3] == 'T') { |
| OscarLiao | 0:80f74d98d66f | 449 | //T_0 command |
| OscarLiao | 0:80f74d98d66f | 450 | T_0[0] = 1.0f * (Buff[0] - 128); |
| OscarLiao | 0:80f74d98d66f | 451 | T_0[1] = 1.0f * (Buff[1] - 128); |
| OscarLiao | 0:80f74d98d66f | 452 | T_0[2] = 1.0f * (Buff[2] - 128); |
| OscarLiao | 0:80f74d98d66f | 453 | } else if(Buff[3] == 'K') { |
| OscarLiao | 0:80f74d98d66f | 454 | K_c[0] = 0.02f * Buff[0]; |
| OscarLiao | 0:80f74d98d66f | 455 | K_c[1] = 0.02f * Buff[1]; |
| OscarLiao | 0:80f74d98d66f | 456 | K_c[2] = 0.02f * Buff[2]; |
| OscarLiao | 0:80f74d98d66f | 457 | } else if (Buff[3] == 'B') { |
| OscarLiao | 0:80f74d98d66f | 458 | B_c[0] = 0.02f * (Buff[0] - 128); |
| OscarLiao | 0:80f74d98d66f | 459 | B_c[1] = 0.02f * (Buff[1] - 128); |
| OscarLiao | 0:80f74d98d66f | 460 | B_c[2] = 0.02f * (Buff[2] - 128); |
| OscarLiao | 0:80f74d98d66f | 461 | } else { |
| OscarLiao | 0:80f74d98d66f | 462 | //Show error in communication |
| OscarLiao | 0:80f74d98d66f | 463 | // led = 1; |
| OscarLiao | 0:80f74d98d66f | 464 | } |
| OscarLiao | 0:80f74d98d66f | 465 | |
| OscarLiao | 0:80f74d98d66f | 466 | Buff[3] = 0x00; //clear end signals |
| OscarLiao | 0:80f74d98d66f | 467 | Cmd_Flag = 0; |
| OscarLiao | 0:80f74d98d66f | 468 | led = 0; |
| OscarLiao | 0:80f74d98d66f | 469 | //Send out message threw Cmd |
| OscarLiao | 0:80f74d98d66f | 470 | // Cmd.printf("%c %c %c\r", Dummy[0], Dummy[1], Dummy[2]); |
| OscarLiao | 0:80f74d98d66f | 471 | } |
| OscarLiao | 0:80f74d98d66f | 472 | } |
| OscarLiao | 0:80f74d98d66f | 473 | } |
| OscarLiao | 0:80f74d98d66f | 474 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of main funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
| OscarLiao | 0:80f74d98d66f | 475 | |
| OscarLiao | 0:80f74d98d66f | 476 | |
| OscarLiao | 0:80f74d98d66f | 477 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Timebase funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
| OscarLiao | 0:80f74d98d66f | 478 | void init_TIMER() //set TT_main{} rate |
| OscarLiao | 0:80f74d98d66f | 479 | { |
| OscarLiao | 0:80f74d98d66f | 480 | TT.attach_us(&TT_main, Rms); |
| OscarLiao | 0:80f74d98d66f | 481 | } |
| OscarLiao | 0:80f74d98d66f | 482 | void TT_main() //interrupt function by TT |
| OscarLiao | 0:80f74d98d66f | 483 | { |
| OscarLiao | 0:80f74d98d66f | 484 | Task_1_count = Task_1_count + 1; |
| OscarLiao | 0:80f74d98d66f | 485 | if(Task_1_count > Task_1_NN) { |
| OscarLiao | 0:80f74d98d66f | 486 | Task_1_count = 0; //Task triggering |
| OscarLiao | 0:80f74d98d66f | 487 | Flag_1 = 1; |
| OscarLiao | 0:80f74d98d66f | 488 | Task_1 = 1; //show for indicator |
| OscarLiao | 0:80f74d98d66f | 489 | } |
| OscarLiao | 0:80f74d98d66f | 490 | Task_2_count = Task_2_count + 1; |
| OscarLiao | 0:80f74d98d66f | 491 | if(Task_2_count > Task_2_NN) { |
| OscarLiao | 0:80f74d98d66f | 492 | Task_2_count = 0; |
| OscarLiao | 0:80f74d98d66f | 493 | //Task triggering |
| OscarLiao | 0:80f74d98d66f | 494 | Flag_2 = 1; |
| OscarLiao | 0:80f74d98d66f | 495 | Task_2 = 1; //show for indicator |
| OscarLiao | 0:80f74d98d66f | 496 | } |
| OscarLiao | 0:80f74d98d66f | 497 | } |
| OscarLiao | 0:80f74d98d66f | 498 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Timebase funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
| OscarLiao | 0:80f74d98d66f | 499 | |
| OscarLiao | 0:80f74d98d66f | 500 | |
| OscarLiao | 0:80f74d98d66f | 501 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡init_IO funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
| OscarLiao | 0:80f74d98d66f | 502 | void init_IO(void) //initialize |
| OscarLiao | 0:80f74d98d66f | 503 | { |
| OscarLiao | 0:80f74d98d66f | 504 | CS1.mode(PullUp); //A4 |
| OscarLiao | 0:80f74d98d66f | 505 | CS2.mode(PullUp); //A5 |
| OscarLiao | 0:80f74d98d66f | 506 | CSG = 0; //A6 |
| OscarLiao | 0:80f74d98d66f | 507 | led = 0; |
| OscarLiao | 0:80f74d98d66f | 508 | Drive1.period_us(15000); |
| OscarLiao | 0:80f74d98d66f | 509 | Drive2.period_us(15000); |
| OscarLiao | 0:80f74d98d66f | 510 | Drive3.period_us(15000); |
| OscarLiao | 0:80f74d98d66f | 511 | } |
| OscarLiao | 0:80f74d98d66f | 512 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of init_IO funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
| OscarLiao | 0:80f74d98d66f | 513 | |
| OscarLiao | 0:80f74d98d66f | 514 | |
| OscarLiao | 0:80f74d98d66f | 515 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡controller funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
| OscarLiao | 0:80f74d98d66f | 516 | |
| OscarLiao | 0:80f74d98d66f | 517 | //follow steps for discrete process in j oder |
| OscarLiao | 0:80f74d98d66f | 518 | //1.update w0[n] threw input x[n] & wi[n] from last sequence |
| OscarLiao | 0:80f74d98d66f | 519 | // w0[n] = x[n] + Zigma( ai*wi[n] ), i = 1, 2, 3...j-1, j. |
| OscarLiao | 0:80f74d98d66f | 520 | // y[n] = Zigma( bi*wi[n] ), i = 0, 1, 2...j-1, j. |
| OscarLiao | 0:80f74d98d66f | 521 | //2.update wi[n+1] for next sequence |
| OscarLiao | 0:80f74d98d66f | 522 | // wi[n] = wi-1[n], i = j, j-1...2, 1. |
| OscarLiao | 0:80f74d98d66f | 523 | |
| OscarLiao | 0:80f74d98d66f | 524 | void q_spring_Est(void) |
| OscarLiao | 0:80f74d98d66f | 525 | { |
| OscarLiao | 0:80f74d98d66f | 526 | //read in from adc |
| OscarLiao | 0:80f74d98d66f | 527 | q_spring[0] = Kadc * adc1.read() - q_spring_0[0]; |
| OscarLiao | 0:80f74d98d66f | 528 | q_spring[1] = Kadc * adc2.read() - q_spring_0[1]; |
| OscarLiao | 0:80f74d98d66f | 529 | q_spring[2] = Kadc * adc3.read() - q_spring_0[2]; |
| OscarLiao | 0:80f74d98d66f | 530 | } |
| OscarLiao | 0:80f74d98d66f | 531 | |
| OscarLiao | 0:80f74d98d66f | 532 | void q_act_Est(void) |
| OscarLiao | 0:80f74d98d66f | 533 | { |
| OscarLiao | 0:80f74d98d66f | 534 | for(int i=0; i<3; i++) { |
| OscarLiao | 0:80f74d98d66f | 535 | q_ref[i] = PWM[i] - PWM_base[i]; |
| OscarLiao | 0:80f74d98d66f | 536 | } |
| OscarLiao | 0:80f74d98d66f | 537 | //q_act_est = q_ref*G1_rad_TUT(z) - q_spring |
| OscarLiao | 0:80f74d98d66f | 538 | //G1_rad_TUT(z) is done below |
| OscarLiao | 0:80f74d98d66f | 539 | for(int i=0; i<3; i++) { |
| OscarLiao | 0:80f74d98d66f | 540 | //update w0[n] threw input x[n] & wi[n] from last sequence |
| OscarLiao | 0:80f74d98d66f | 541 | G1_w_n[i][0] = q_ref[i] + G1_a[1]*G1_w_n[i][1] + G1_a[2]*G1_w_n[i][2]; |
| OscarLiao | 0:80f74d98d66f | 542 | 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]; |
| OscarLiao | 0:80f74d98d66f | 543 | q_act_est[i] = q_act_est[i]*0.0001f - q_spring[i]; |
| OscarLiao | 0:80f74d98d66f | 544 | |
| OscarLiao | 0:80f74d98d66f | 545 | //update wi[n+1] |
| OscarLiao | 0:80f74d98d66f | 546 | G1_w_n[i][2] = G1_w_n[i][1]; |
| OscarLiao | 0:80f74d98d66f | 547 | G1_w_n[i][1] = G1_w_n[i][0]; |
| OscarLiao | 0:80f74d98d66f | 548 | } |
| OscarLiao | 0:80f74d98d66f | 549 | //q_act_est[i] is now ready for following app |
| OscarLiao | 0:80f74d98d66f | 550 | } |
| OscarLiao | 0:80f74d98d66f | 551 | |
| OscarLiao | 0:80f74d98d66f | 552 | void Impedence_Ref(void) |
| OscarLiao | 0:80f74d98d66f | 553 | { |
| OscarLiao | 0:80f74d98d66f | 554 | //Transform q_act_est from joint to end point |
| OscarLiao | 0:80f74d98d66f | 555 | q_act_est[1] -= Phi1_0; |
| OscarLiao | 0:80f74d98d66f | 556 | q_act_est[2] += Phi2_0; |
| OscarLiao | 0:80f74d98d66f | 557 | |
| OscarLiao | 0:80f74d98d66f | 558 | |
| OscarLiao | 0:80f74d98d66f | 559 | |
| OscarLiao | 0:80f74d98d66f | 560 | L_act_est = sqrt( LA + 2*LB*cos(q_act_est[2]) ); |
| OscarLiao | 0:80f74d98d66f | 561 | P_act_est = q_act_est[1] + asin( L2*sin(q_act_est[2]) / L_act_est ); |
| OscarLiao | 0:80f74d98d66f | 562 | |
| OscarLiao | 0:80f74d98d66f | 563 | |
| OscarLiao | 0:80f74d98d66f | 564 | //Calculate error in end point level |
| OscarLiao | 0:80f74d98d66f | 565 | //q_0[i] => {Hip, Tigh, Shin} |
| OscarLiao | 0:80f74d98d66f | 566 | q_err_E[0] = q_0_E[0] - q_act_est[0]; //Hip motor#1 |
| OscarLiao | 0:80f74d98d66f | 567 | q_err_E[1] = q_0_E[1] - L_act_est; |
| OscarLiao | 0:80f74d98d66f | 568 | q_err_E[2] = q_0_E[2] - P_act_est; |
| OscarLiao | 0:80f74d98d66f | 569 | |
| OscarLiao | 0:80f74d98d66f | 570 | |
| OscarLiao | 0:80f74d98d66f | 571 | |
| OscarLiao | 0:80f74d98d66f | 572 | for(int i=0; i<3; i++) { |
| OscarLiao | 0:80f74d98d66f | 573 | //Generate T_ref in Operational Space (OSC control perhaps?) |
| OscarLiao | 0:80f74d98d66f | 574 | //T_ref_E = PDn( q_err_E ); |
| OscarLiao | 0:80f74d98d66f | 575 | |
| OscarLiao | 0:80f74d98d66f | 576 | //Compliance |
| OscarLiao | 0:80f74d98d66f | 577 | T_ref_WD_E[i] = K_c[i] * q_err_E[i]; |
| OscarLiao | 0:80f74d98d66f | 578 | |
| OscarLiao | 0:80f74d98d66f | 579 | //Damping |
| OscarLiao | 0:80f74d98d66f | 580 | //update w0[n] threw input x[n] & wi[n] from last sequence |
| OscarLiao | 0:80f74d98d66f | 581 | Cref_w[i][0] = q_err_E[i] + Cref_a[1]*Cref_w[i][1]; |
| OscarLiao | 0:80f74d98d66f | 582 | q_err_D_E[i] = Cref_b[0]*Cref_w[i][0] + Cref_b[1]*Cref_w[i][1]; |
| OscarLiao | 0:80f74d98d66f | 583 | |
| OscarLiao | 0:80f74d98d66f | 584 | //update wi[n+1] |
| OscarLiao | 0:80f74d98d66f | 585 | Cref_w[i][1] = Cref_w[i][0]; |
| OscarLiao | 0:80f74d98d66f | 586 | |
| OscarLiao | 0:80f74d98d66f | 587 | //Generate T_ref in endpoint level |
| OscarLiao | 0:80f74d98d66f | 588 | T_ref_E[i] = T_ref_WD_E[i] + B_c[i]*q_err_D_E[i]; |
| OscarLiao | 0:80f74d98d66f | 589 | } |
| OscarLiao | 0:80f74d98d66f | 590 | |
| OscarLiao | 0:80f74d98d66f | 591 | //Transform torque to joint space, T_ref[i] = Σ( J[i][j] * T_ref_E[j] ) |
| OscarLiao | 0:80f74d98d66f | 592 | J32 = -LB*sin(q_act_est[2]) / L_act_est; |
| OscarLiao | 0:80f74d98d66f | 593 | J33 = ( pow(J32,2)/LB + cos(q_act_est[2]) ) / (LC + cos(q_act_est[2])); |
| OscarLiao | 0:80f74d98d66f | 594 | |
| OscarLiao | 0:80f74d98d66f | 595 | //Torque of Hip |
| OscarLiao | 0:80f74d98d66f | 596 | T_ref[0] = T_ref_E[0]; |
| OscarLiao | 0:80f74d98d66f | 597 | T_ref_WD[0] = T_ref_WD_E[0]; |
| OscarLiao | 0:80f74d98d66f | 598 | |
| OscarLiao | 0:80f74d98d66f | 599 | //Torque of Tigh |
| OscarLiao | 0:80f74d98d66f | 600 | T_ref[1] = T_ref_E[2] + T_0[1]; |
| OscarLiao | 0:80f74d98d66f | 601 | T_ref_WD[1] = T_ref_WD_E[2] + T_0[1]; |
| OscarLiao | 0:80f74d98d66f | 602 | |
| OscarLiao | 0:80f74d98d66f | 603 | //Torque of Shin |
| OscarLiao | 0:80f74d98d66f | 604 | T_ref[2] = J32*T_ref_E[1] + J33*T_ref_E[2] + J32*T_0[1] + J33*T_0[2]; |
| OscarLiao | 0:80f74d98d66f | 605 | T_ref_WD[2] = J32*T_ref_WD_E[1] + J33*T_ref_WD_E[2] + J32*T_0[1] + J33*T_0[2]; |
| OscarLiao | 0:80f74d98d66f | 606 | |
| OscarLiao | 0:80f74d98d66f | 607 | } |
| OscarLiao | 0:80f74d98d66f | 608 | |
| OscarLiao | 0:80f74d98d66f | 609 | void Torque_Reg(void) |
| OscarLiao | 0:80f74d98d66f | 610 | { |
| OscarLiao | 0:80f74d98d66f | 611 | if(CS2 == 1) { |
| OscarLiao | 0:80f74d98d66f | 612 | |
| OscarLiao | 0:80f74d98d66f | 613 | //Transform q_act_est from end point to joint |
| OscarLiao | 0:80f74d98d66f | 614 | q_act_est[1] += Phi1_0; |
| OscarLiao | 0:80f74d98d66f | 615 | q_act_est[2] -= Phi2_0; |
| OscarLiao | 0:80f74d98d66f | 616 | |
| OscarLiao | 0:80f74d98d66f | 617 | //Normal command |
| OscarLiao | 0:80f74d98d66f | 618 | for(int i=0; i<3; i++) { |
| OscarLiao | 0:80f74d98d66f | 619 | //FeedFoward |
| OscarLiao | 0:80f74d98d66f | 620 | q_ref[i] = q_act_est[i]*Kqff + T_ref_WD[i]*KTff; //full feedfoward |
| OscarLiao | 0:80f74d98d66f | 621 | //q_ref[i] = 0; //w/o feedfoward |
| OscarLiao | 0:80f74d98d66f | 622 | |
| OscarLiao | 0:80f74d98d66f | 623 | //FeedBack |
| OscarLiao | 0:80f74d98d66f | 624 | T_err[i] = T_ref[i] - K_q*q_spring[i]; |
| OscarLiao | 0:80f74d98d66f | 625 | T_reg_I[i] = T_reg_I[i] + T_err[i]*Kit[i]; |
| OscarLiao | 0:80f74d98d66f | 626 | T_reg_I[i] = constrain(T_reg_I[i], -200, 200); |
| OscarLiao | 0:80f74d98d66f | 627 | q_ref[i] = T_err[i]*Kp[i] + T_reg_I[i] + q_ref[i]; |
| OscarLiao | 0:80f74d98d66f | 628 | } |
| OscarLiao | 0:80f74d98d66f | 629 | q_ref[0] = q_0_E[0]*(550/(float)(0.5*pi)); |
| OscarLiao | 0:80f74d98d66f | 630 | q_ref[0] = constrain(q_ref[0], -650, 650); |
| OscarLiao | 0:80f74d98d66f | 631 | q_ref[1] = constrain(q_ref[1], -650, 650); |
| OscarLiao | 0:80f74d98d66f | 632 | q_ref[2] = constrain(q_ref[2], -650, 650); |
| OscarLiao | 0:80f74d98d66f | 633 | |
| OscarLiao | 0:80f74d98d66f | 634 | |
| OscarLiao | 0:80f74d98d66f | 635 | } else { |
| OscarLiao | 0:80f74d98d66f | 636 | //Stretch command |
| OscarLiao | 0:80f74d98d66f | 637 | for(int i=0; i<3; i++) { |
| OscarLiao | 0:80f74d98d66f | 638 | T_reg_I[i] = 0; |
| OscarLiao | 0:80f74d98d66f | 639 | q_ref[i] = 0; |
| OscarLiao | 0:80f74d98d66f | 640 | } |
| OscarLiao | 0:80f74d98d66f | 641 | } |
| OscarLiao | 0:80f74d98d66f | 642 | } |
| OscarLiao | 0:80f74d98d66f | 643 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of controller funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
| OscarLiao | 0:80f74d98d66f | 644 | |
| OscarLiao | 0:80f74d98d66f | 645 | |
| OscarLiao | 0:80f74d98d66f | 646 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡init_q_spring_0 funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
| OscarLiao | 0:80f74d98d66f | 647 | void init_q_spring_0(void) |
| OscarLiao | 0:80f74d98d66f | 648 | { |
| OscarLiao | 0:80f74d98d66f | 649 | for(int i=0; i<1000; i++) { |
| OscarLiao | 0:80f74d98d66f | 650 | q_spring_0[0] = q_spring_0[0] + Kadc * adc1.read(); |
| OscarLiao | 0:80f74d98d66f | 651 | q_spring_0[1] = q_spring_0[1] + Kadc * adc2.read(); |
| OscarLiao | 0:80f74d98d66f | 652 | q_spring_0[2] = q_spring_0[2] + Kadc * adc3.read(); |
| OscarLiao | 0:80f74d98d66f | 653 | wait_us(100); |
| OscarLiao | 0:80f74d98d66f | 654 | } |
| OscarLiao | 0:80f74d98d66f | 655 | q_spring_0[0] = q_spring_0[0] /1000.0f; |
| OscarLiao | 0:80f74d98d66f | 656 | q_spring_0[1] = q_spring_0[1] /1000.0f; |
| OscarLiao | 0:80f74d98d66f | 657 | q_spring_0[2] = q_spring_0[2] /1000.0f; |
| OscarLiao | 0:80f74d98d66f | 658 | } |
| OscarLiao | 0:80f74d98d66f | 659 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of init_q_spring_0 funtion■■■■■■■■■■■■■■■■■■■■■■■■// |
| OscarLiao | 0:80f74d98d66f | 660 | |
| OscarLiao | 0:80f74d98d66f | 661 | |
| OscarLiao | 0:80f74d98d66f | 662 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Rx_xxx funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
| OscarLiao | 0:80f74d98d66f | 663 | void Rx_irq(void) |
| OscarLiao | 0:80f74d98d66f | 664 | { |
| OscarLiao | 0:80f74d98d66f | 665 | //Keep clear in Rx_irq and leave most work after Rx_end |
| OscarLiao | 0:80f74d98d66f | 666 | Buff[Recieve_index] = Cmd.getc(); |
| OscarLiao | 0:80f74d98d66f | 667 | Recieve_index = Recieve_index + Rx_enable; |
| OscarLiao | 0:80f74d98d66f | 668 | } |
| OscarLiao | 0:80f74d98d66f | 669 | |
| OscarLiao | 0:80f74d98d66f | 670 | void Rx_srt(void) |
| OscarLiao | 0:80f74d98d66f | 671 | { |
| OscarLiao | 0:80f74d98d66f | 672 | //start receiving Serial message from Cmd |
| OscarLiao | 0:80f74d98d66f | 673 | // NVIC_EnableIRQ(USART1_IRQn); |
| OscarLiao | 0:80f74d98d66f | 674 | Rx_enable = 1; |
| OscarLiao | 0:80f74d98d66f | 675 | } |
| OscarLiao | 0:80f74d98d66f | 676 | void Rx_end(void) |
| OscarLiao | 0:80f74d98d66f | 677 | { |
| OscarLiao | 0:80f74d98d66f | 678 | //end of receiving Serial message from Cmd |
| OscarLiao | 0:80f74d98d66f | 679 | // NVIC_DisableIRQ(USART1_IRQn); |
| OscarLiao | 0:80f74d98d66f | 680 | Rx_enable = 0; |
| OscarLiao | 0:80f74d98d66f | 681 | Recieve_index = 0; |
| OscarLiao | 0:80f74d98d66f | 682 | Cmd_Flag = 1; |
| OscarLiao | 0:80f74d98d66f | 683 | } |
| OscarLiao | 0:80f74d98d66f | 684 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Rx_irq funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |