Oscar Liao / Mbed 2 deprecated controller_test_endpoint_RF

test

Dependencies:   mbed

main.cpp@0:b5f9b0530f25, 2018-12-21 (annotated)

Committer:
OscarLiao
Date:
Fri Dec 21 05:53:27 2018 +0000
Revision:
0:b5f9b0530f25
test

Who changed what in which revision?

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