Oscar Liao
/
CHICAGO_balance_test_PID_copy
balance
main.cpp@0:94cf69f1f327, 2018-12-13 (annotated)
- Committer:
- OscarLiao
- Date:
- Thu Dec 13 14:27:35 2018 +0000
- Revision:
- 0:94cf69f1f327
- Child:
- 1:15dd461fbc2a
balance
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
OscarLiao | 0:94cf69f1f327 | 1 | /* |
OscarLiao | 0:94cf69f1f327 | 2 | This is a program for globlelized controller of whole DOGO |
OscarLiao | 0:94cf69f1f327 | 3 | F746ZG |
OscarLiao | 0:94cf69f1f327 | 4 | */ |
OscarLiao | 0:94cf69f1f327 | 5 | |
OscarLiao | 0:94cf69f1f327 | 6 | #include "mbed.h" |
OscarLiao | 0:94cf69f1f327 | 7 | #include "LSM9DS0_SH.h" |
OscarLiao | 0:94cf69f1f327 | 8 | #define DEBUG 0 |
OscarLiao | 0:94cf69f1f327 | 9 | |
OscarLiao | 0:94cf69f1f327 | 10 | #define pi 3.141592f |
OscarLiao | 0:94cf69f1f327 | 11 | #define d2r 0.01745329f |
OscarLiao | 0:94cf69f1f327 | 12 | |
OscarLiao | 0:94cf69f1f327 | 13 | #define Rms 5000 //TT rate |
OscarLiao | 0:94cf69f1f327 | 14 | #define dt 0.005f |
OscarLiao | 0:94cf69f1f327 | 15 | #define Task_1_NN 9 |
OscarLiao | 0:94cf69f1f327 | 16 | #define Task_2_NN 5 |
OscarLiao | 0:94cf69f1f327 | 17 | #define Task_4_NN 9 |
OscarLiao | 0:94cf69f1f327 | 18 | |
OscarLiao | 0:94cf69f1f327 | 19 | //Structure |
OscarLiao | 0:94cf69f1f327 | 20 | #define Buff_size 16 //Serial Buffer |
OscarLiao | 0:94cf69f1f327 | 21 | |
OscarLiao | 0:94cf69f1f327 | 22 | //Initial Position |
OscarLiao | 0:94cf69f1f327 | 23 | #define init_leg_Angle1 0 |
OscarLiao | 0:94cf69f1f327 | 24 | #define init_leg_length 0.22f |
OscarLiao | 0:94cf69f1f327 | 25 | #define init_leg_Angle2 0 |
OscarLiao | 0:94cf69f1f327 | 26 | |
OscarLiao | 0:94cf69f1f327 | 27 | #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) |
OscarLiao | 0:94cf69f1f327 | 28 | |
OscarLiao | 0:94cf69f1f327 | 29 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 30 | //╔═════════════════╗ |
OscarLiao | 0:94cf69f1f327 | 31 | //║ Structure ║ |
OscarLiao | 0:94cf69f1f327 | 32 | //╚═════════════════╝ |
OscarLiao | 0:94cf69f1f327 | 33 | DigitalOut LD1(PB_0); //detection |
OscarLiao | 0:94cf69f1f327 | 34 | DigitalOut LD2(PB_7); //detection |
OscarLiao | 0:94cf69f1f327 | 35 | DigitalOut LD3(PB_14); //detection |
OscarLiao | 0:94cf69f1f327 | 36 | InterruptIn button(USER_BUTTON); //Button press; |
OscarLiao | 0:94cf69f1f327 | 37 | |
OscarLiao | 0:94cf69f1f327 | 38 | //╔═════════════════╗ |
OscarLiao | 0:94cf69f1f327 | 39 | //║ Serial ║ |
OscarLiao | 0:94cf69f1f327 | 40 | //╚═════════════════╝ |
OscarLiao | 0:94cf69f1f327 | 41 | Serial Debug(PD_8, PD_9); //Serial_3 reg(TX RX) USB port |
OscarLiao | 0:94cf69f1f327 | 42 | |
OscarLiao | 0:94cf69f1f327 | 43 | Serial LF_Cmd(PD_5, PD_6); //Serial_2 |
OscarLiao | 0:94cf69f1f327 | 44 | DigitalOut LF_CS(PD_7); |
OscarLiao | 0:94cf69f1f327 | 45 | |
OscarLiao | 0:94cf69f1f327 | 46 | Serial LH_Cmd(PC_12, PD_2); //Serial_5 |
OscarLiao | 0:94cf69f1f327 | 47 | DigitalOut LH_CS(PG_2); |
OscarLiao | 0:94cf69f1f327 | 48 | |
OscarLiao | 0:94cf69f1f327 | 49 | Serial RF_Cmd(PG_14, PC_7); //Serial_6 |
OscarLiao | 0:94cf69f1f327 | 50 | DigitalOut RF_CS(PG_9); |
OscarLiao | 0:94cf69f1f327 | 51 | |
OscarLiao | 0:94cf69f1f327 | 52 | Serial RH_Cmd(PE_8, PE_7); //Serial_7 |
OscarLiao | 0:94cf69f1f327 | 53 | DigitalOut RH_CS(PE_10); |
OscarLiao | 0:94cf69f1f327 | 54 | |
OscarLiao | 0:94cf69f1f327 | 55 | //╔═════════════════╗ |
OscarLiao | 0:94cf69f1f327 | 56 | //║ IMU_SPI ║ |
OscarLiao | 0:94cf69f1f327 | 57 | //╚═════════════════╝ |
OscarLiao | 0:94cf69f1f327 | 58 | DigitalOut SPI_CSG(PD_14,1); //low for GYRO enable |
OscarLiao | 0:94cf69f1f327 | 59 | DigitalOut SPI_CSXM(PD_15,1); //low for ACC/MAG enable |
OscarLiao | 0:94cf69f1f327 | 60 | SPI spi(PA_7, PA_6, PA_5); //MOSI MISO SCLK |
OscarLiao | 0:94cf69f1f327 | 61 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of GPIO registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 62 | |
OscarLiao | 0:94cf69f1f327 | 63 | |
OscarLiao | 0:94cf69f1f327 | 64 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Varible registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 65 | //╔═════════════════╗ |
OscarLiao | 0:94cf69f1f327 | 66 | //║ Structure ║ |
OscarLiao | 0:94cf69f1f327 | 67 | //╚═════════════════╝ |
OscarLiao | 0:94cf69f1f327 | 68 | Ticker TT; //call a timer |
OscarLiao | 0:94cf69f1f327 | 69 | uint8_t Task_1_count = 0; //1st prior task count |
OscarLiao | 0:94cf69f1f327 | 70 | uint8_t Task_2_count = 0; //2nd prior task count |
OscarLiao | 0:94cf69f1f327 | 71 | uint8_t Task_3_count = 0; //3nd prior task count |
OscarLiao | 0:94cf69f1f327 | 72 | uint8_t Task_4_count = 0; //24nd prior task count |
OscarLiao | 0:94cf69f1f327 | 73 | uint8_t Flag_1 = 0; //1st prior task flag |
OscarLiao | 0:94cf69f1f327 | 74 | uint8_t Flag_2 = 0; //2nd prior task flag |
OscarLiao | 0:94cf69f1f327 | 75 | uint8_t Flag_3 = 0; //3nd prior task flag |
OscarLiao | 0:94cf69f1f327 | 76 | uint8_t Flag_4 = 0; //4nd prior task flag |
OscarLiao | 0:94cf69f1f327 | 77 | |
OscarLiao | 0:94cf69f1f327 | 78 | //╔═════════════════╗ |
OscarLiao | 0:94cf69f1f327 | 79 | //║ I/O Serial ║ |
OscarLiao | 0:94cf69f1f327 | 80 | //╚═════════════════╝ |
OscarLiao | 0:94cf69f1f327 | 81 | uint8_t SerBuff[Buff_size]; |
OscarLiao | 0:94cf69f1f327 | 82 | uint8_t Recieve_index = 0; |
OscarLiao | 0:94cf69f1f327 | 83 | |
OscarLiao | 0:94cf69f1f327 | 84 | //╔═════════════════╗ |
OscarLiao | 0:94cf69f1f327 | 85 | //║ Leg Command ║ |
OscarLiao | 0:94cf69f1f327 | 86 | //╚═════════════════╝ |
OscarLiao | 0:94cf69f1f327 | 87 | |
OscarLiao | 0:94cf69f1f327 | 88 | float LF_q_0_E[3] = { |
OscarLiao | 0:94cf69f1f327 | 89 | init_leg_Angle1, init_leg_length, init_leg_Angle2 |
OscarLiao | 0:94cf69f1f327 | 90 | }; |
OscarLiao | 0:94cf69f1f327 | 91 | float LH_q_0_E[3] = { |
OscarLiao | 0:94cf69f1f327 | 92 | init_leg_Angle1, init_leg_length, init_leg_Angle2 |
OscarLiao | 0:94cf69f1f327 | 93 | }; |
OscarLiao | 0:94cf69f1f327 | 94 | float RF_q_0_E[3] = { |
OscarLiao | 0:94cf69f1f327 | 95 | init_leg_Angle1, init_leg_length, init_leg_Angle2 |
OscarLiao | 0:94cf69f1f327 | 96 | }; |
OscarLiao | 0:94cf69f1f327 | 97 | float RH_q_0_E[3] = { |
OscarLiao | 0:94cf69f1f327 | 98 | init_leg_Angle1, init_leg_length, init_leg_Angle2 |
OscarLiao | 0:94cf69f1f327 | 99 | }; |
OscarLiao | 0:94cf69f1f327 | 100 | |
OscarLiao | 0:94cf69f1f327 | 101 | uint8_t LF_Cmd_Hex[3] = {0x00, 0x00, 0x00}; |
OscarLiao | 0:94cf69f1f327 | 102 | uint8_t LH_Cmd_Hex[3] = {0x00, 0x00, 0x00}; |
OscarLiao | 0:94cf69f1f327 | 103 | uint8_t RF_Cmd_Hex[3] = {0x00, 0x00, 0x00}; |
OscarLiao | 0:94cf69f1f327 | 104 | uint8_t RH_Cmd_Hex[3] = {0x00, 0x00, 0x00}; |
OscarLiao | 0:94cf69f1f327 | 105 | |
OscarLiao | 0:94cf69f1f327 | 106 | //╔═════════════════╗ |
OscarLiao | 0:94cf69f1f327 | 107 | //║ IMU_SPI ║ |
OscarLiao | 0:94cf69f1f327 | 108 | //╚═════════════════╝ |
OscarLiao | 0:94cf69f1f327 | 109 | short low_byte = 0x00; //buffer |
OscarLiao | 0:94cf69f1f327 | 110 | short high_byte = 0x00; |
OscarLiao | 0:94cf69f1f327 | 111 | short Buff = 0x00; |
OscarLiao | 0:94cf69f1f327 | 112 | float Wx = 0.0; |
OscarLiao | 0:94cf69f1f327 | 113 | float Wy = 0.0; |
OscarLiao | 0:94cf69f1f327 | 114 | float Wz = 0.0; |
OscarLiao | 0:94cf69f1f327 | 115 | float Ax = 0.0; |
OscarLiao | 0:94cf69f1f327 | 116 | float Ay = 0.0; |
OscarLiao | 0:94cf69f1f327 | 117 | float Az = 0.0; |
OscarLiao | 0:94cf69f1f327 | 118 | float gDIR[1][3] = { //g vector's direction , required unitconstrain |
OscarLiao | 0:94cf69f1f327 | 119 | {0.0,0.0,0.0}, //X Y Z |
OscarLiao | 0:94cf69f1f327 | 120 | }; |
OscarLiao | 0:94cf69f1f327 | 121 | float Bet = 0.002; //confidence of Acc data |
OscarLiao | 0:94cf69f1f327 | 122 | float gUnity = 0.0; |
OscarLiao | 0:94cf69f1f327 | 123 | float Gdx = 0.0; |
OscarLiao | 0:94cf69f1f327 | 124 | float Gdy = 0.0; |
OscarLiao | 0:94cf69f1f327 | 125 | float Gdz = 0.0; |
OscarLiao | 0:94cf69f1f327 | 126 | float Adx = 0.0; |
OscarLiao | 0:94cf69f1f327 | 127 | float Ady = 0.0; |
OscarLiao | 0:94cf69f1f327 | 128 | float Adz = 0.0; |
OscarLiao | 0:94cf69f1f327 | 129 | float Ele_phy = 0.0; //estimation of top plate attitide |
OscarLiao | 0:94cf69f1f327 | 130 | float Til_phy = 0.0; |
OscarLiao | 0:94cf69f1f327 | 131 | float Ele_phy_int = 0.0; |
OscarLiao | 0:94cf69f1f327 | 132 | float Til_phy_int = 0.0; |
OscarLiao | 0:94cf69f1f327 | 133 | float Ele_control = 0.0; |
OscarLiao | 0:94cf69f1f327 | 134 | float Til_control = 0.0; |
OscarLiao | 0:94cf69f1f327 | 135 | |
OscarLiao | 0:94cf69f1f327 | 136 | //╔═════════════════╗ |
OscarLiao | 0:94cf69f1f327 | 137 | //║ Balance ║ |
OscarLiao | 0:94cf69f1f327 | 138 | //╚═════════════════╝ |
OscarLiao | 0:94cf69f1f327 | 139 | float Kp = 0.04; |
OscarLiao | 0:94cf69f1f327 | 140 | float Ki = 0.0; |
OscarLiao | 0:94cf69f1f327 | 141 | float Kd = 0.0; |
OscarLiao | 0:94cf69f1f327 | 142 | float Pu = 0.0; |
OscarLiao | 0:94cf69f1f327 | 143 | float Iu = 0.0; |
OscarLiao | 0:94cf69f1f327 | 144 | float Du = 0.0; |
OscarLiao | 0:94cf69f1f327 | 145 | float Ele_phy_old = 0.0; |
OscarLiao | 0:94cf69f1f327 | 146 | |
OscarLiao | 0:94cf69f1f327 | 147 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 148 | |
OscarLiao | 0:94cf69f1f327 | 149 | |
OscarLiao | 0:94cf69f1f327 | 150 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Function registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 151 | void init_TIMER(); //set TT_main() rate |
OscarLiao | 0:94cf69f1f327 | 152 | void TT_main(); //timebase function rated by TT |
OscarLiao | 0:94cf69f1f327 | 153 | |
OscarLiao | 0:94cf69f1f327 | 154 | void init_IMU(); //initialize IMU |
OscarLiao | 0:94cf69f1f327 | 155 | void init_GXdrift(); //read GXdrift at start up |
OscarLiao | 0:94cf69f1f327 | 156 | void read_IMU(); //read IMU data give raw data |
OscarLiao | 0:94cf69f1f327 | 157 | void state_update(); //estimation of new attitude |
OscarLiao | 0:94cf69f1f327 | 158 | |
OscarLiao | 0:94cf69f1f327 | 159 | float lpf(float input, float output_old, float frequency); //lpf discrete |
OscarLiao | 0:94cf69f1f327 | 160 | |
OscarLiao | 0:94cf69f1f327 | 161 | void pressed(); |
OscarLiao | 0:94cf69f1f327 | 162 | void Balance(); |
OscarLiao | 0:94cf69f1f327 | 163 | |
OscarLiao | 0:94cf69f1f327 | 164 | void Rx_irq(); |
OscarLiao | 0:94cf69f1f327 | 165 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 166 | |
OscarLiao | 0:94cf69f1f327 | 167 | |
OscarLiao | 0:94cf69f1f327 | 168 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡main funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 169 | int main() |
OscarLiao | 0:94cf69f1f327 | 170 | { |
OscarLiao | 0:94cf69f1f327 | 171 | init_IMU(); //initialize IMU |
OscarLiao | 0:94cf69f1f327 | 172 | init_GXdrift(); //read GXdrift at start up |
OscarLiao | 0:94cf69f1f327 | 173 | |
OscarLiao | 0:94cf69f1f327 | 174 | Debug.baud(115200); //set baud rate |
OscarLiao | 0:94cf69f1f327 | 175 | LF_Cmd.baud(115200); |
OscarLiao | 0:94cf69f1f327 | 176 | LH_Cmd.baud(115200); |
OscarLiao | 0:94cf69f1f327 | 177 | RF_Cmd.baud(115200); |
OscarLiao | 0:94cf69f1f327 | 178 | RH_Cmd.baud(115200); |
OscarLiao | 0:94cf69f1f327 | 179 | |
OscarLiao | 0:94cf69f1f327 | 180 | wait_ms(10); |
OscarLiao | 0:94cf69f1f327 | 181 | init_TIMER(); //start TT_main |
OscarLiao | 0:94cf69f1f327 | 182 | |
OscarLiao | 0:94cf69f1f327 | 183 | Debug.attach(&Rx_irq,Serial::RxIrq); //Start recieving message |
OscarLiao | 0:94cf69f1f327 | 184 | LF_CS = 1; |
OscarLiao | 0:94cf69f1f327 | 185 | |
OscarLiao | 0:94cf69f1f327 | 186 | wait_ms(200); |
OscarLiao | 0:94cf69f1f327 | 187 | |
OscarLiao | 0:94cf69f1f327 | 188 | button.fall(&pressed); //button pressed |
OscarLiao | 0:94cf69f1f327 | 189 | |
OscarLiao | 0:94cf69f1f327 | 190 | while(1) { //main() loop |
OscarLiao | 0:94cf69f1f327 | 191 | |
OscarLiao | 0:94cf69f1f327 | 192 | // if(Debug.readable()) { |
OscarLiao | 0:94cf69f1f327 | 193 | // SerBuff[Recieve_index] = Debug.getc(); |
OscarLiao | 0:94cf69f1f327 | 194 | // Debug.putc(SerBuff[Recieve_index]); |
OscarLiao | 0:94cf69f1f327 | 195 | // Recieve_index = Recieve_index + 1; |
OscarLiao | 0:94cf69f1f327 | 196 | // } |
OscarLiao | 0:94cf69f1f327 | 197 | |
OscarLiao | 0:94cf69f1f327 | 198 | // if(device.readable()) { |
OscarLiao | 0:94cf69f1f327 | 199 | // pc.putc(device.getc()); |
OscarLiao | 0:94cf69f1f327 | 200 | // } |
OscarLiao | 0:94cf69f1f327 | 201 | //Balance |
OscarLiao | 0:94cf69f1f327 | 202 | if(Flag_1 == 1) { |
OscarLiao | 0:94cf69f1f327 | 203 | |
OscarLiao | 0:94cf69f1f327 | 204 | //Initial Position |
OscarLiao | 0:94cf69f1f327 | 205 | LF_q_0_E[1] = init_leg_length; |
OscarLiao | 0:94cf69f1f327 | 206 | LH_q_0_E[1] = init_leg_length; |
OscarLiao | 0:94cf69f1f327 | 207 | RF_q_0_E[1] = init_leg_length; |
OscarLiao | 0:94cf69f1f327 | 208 | RH_q_0_E[1] = init_leg_length; |
OscarLiao | 0:94cf69f1f327 | 209 | |
OscarLiao | 0:94cf69f1f327 | 210 | Balance(); |
OscarLiao | 0:94cf69f1f327 | 211 | Flag_1 = 0; |
OscarLiao | 0:94cf69f1f327 | 212 | } |
OscarLiao | 0:94cf69f1f327 | 213 | |
OscarLiao | 0:94cf69f1f327 | 214 | //Send Command |
OscarLiao | 0:94cf69f1f327 | 215 | if(Flag_4 == 1) { |
OscarLiao | 0:94cf69f1f327 | 216 | |
OscarLiao | 0:94cf69f1f327 | 217 | LF_Cmd_Hex[0] = (LF_q_0_E[0]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 218 | LF_Cmd_Hex[1] = ((LF_q_0_E[1]-0.1)/0.2)*255; |
OscarLiao | 0:94cf69f1f327 | 219 | LF_Cmd_Hex[2] = (LF_q_0_E[2]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 220 | |
OscarLiao | 0:94cf69f1f327 | 221 | LH_Cmd_Hex[0] = (LH_q_0_E[0]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 222 | LH_Cmd_Hex[1] = ((LH_q_0_E[1]-0.1)/0.2)*255; |
OscarLiao | 0:94cf69f1f327 | 223 | LH_Cmd_Hex[2] = (LH_q_0_E[2]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 224 | |
OscarLiao | 0:94cf69f1f327 | 225 | RF_Cmd_Hex[0] = (RF_q_0_E[0]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 226 | RF_Cmd_Hex[1] = ((RF_q_0_E[1]-0.1)/0.2)*255; |
OscarLiao | 0:94cf69f1f327 | 227 | RF_Cmd_Hex[2] = (RF_q_0_E[2]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 228 | |
OscarLiao | 0:94cf69f1f327 | 229 | RH_Cmd_Hex[0] = (RH_q_0_E[0]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 230 | RH_Cmd_Hex[1] = ((RH_q_0_E[1]-0.1)/0.2)*255; |
OscarLiao | 0:94cf69f1f327 | 231 | RH_Cmd_Hex[2] = (RH_q_0_E[2]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 232 | |
OscarLiao | 0:94cf69f1f327 | 233 | //Debug.printf("%d, %d, %d\r", LH_Cmd_Hex[0], LH_Cmd_Hex[1], LH_Cmd_Hex[2]); |
OscarLiao | 0:94cf69f1f327 | 234 | //LED |
OscarLiao | 0:94cf69f1f327 | 235 | LD1 = 1; |
OscarLiao | 0:94cf69f1f327 | 236 | |
OscarLiao | 0:94cf69f1f327 | 237 | //Left Front Leg |
OscarLiao | 0:94cf69f1f327 | 238 | LF_CS = 0; |
OscarLiao | 0:94cf69f1f327 | 239 | wait_us(20); |
OscarLiao | 0:94cf69f1f327 | 240 | LF_Cmd.putc(LF_Cmd_Hex[0]); |
OscarLiao | 0:94cf69f1f327 | 241 | LF_Cmd.putc(LF_Cmd_Hex[1]); |
OscarLiao | 0:94cf69f1f327 | 242 | LF_Cmd.putc(LF_Cmd_Hex[2]); |
OscarLiao | 0:94cf69f1f327 | 243 | LF_Cmd.putc('Q'); |
OscarLiao | 0:94cf69f1f327 | 244 | wait_us(180); |
OscarLiao | 0:94cf69f1f327 | 245 | LF_CS = 1; |
OscarLiao | 0:94cf69f1f327 | 246 | |
OscarLiao | 0:94cf69f1f327 | 247 | //Left Hind Leg |
OscarLiao | 0:94cf69f1f327 | 248 | LH_CS = 0; |
OscarLiao | 0:94cf69f1f327 | 249 | wait_us(20); |
OscarLiao | 0:94cf69f1f327 | 250 | LH_Cmd.putc(LH_Cmd_Hex[0]); |
OscarLiao | 0:94cf69f1f327 | 251 | LH_Cmd.putc(LH_Cmd_Hex[1]); |
OscarLiao | 0:94cf69f1f327 | 252 | LH_Cmd.putc(LH_Cmd_Hex[2]); |
OscarLiao | 0:94cf69f1f327 | 253 | LH_Cmd.putc('Q'); |
OscarLiao | 0:94cf69f1f327 | 254 | wait_us(180); |
OscarLiao | 0:94cf69f1f327 | 255 | LH_CS = 1; |
OscarLiao | 0:94cf69f1f327 | 256 | |
OscarLiao | 0:94cf69f1f327 | 257 | //Right Front Leg |
OscarLiao | 0:94cf69f1f327 | 258 | RF_CS = 0; |
OscarLiao | 0:94cf69f1f327 | 259 | wait_us(20); |
OscarLiao | 0:94cf69f1f327 | 260 | RF_Cmd.putc(RF_Cmd_Hex[0]); |
OscarLiao | 0:94cf69f1f327 | 261 | RF_Cmd.putc(RF_Cmd_Hex[1]); |
OscarLiao | 0:94cf69f1f327 | 262 | RF_Cmd.putc(RF_Cmd_Hex[2]); |
OscarLiao | 0:94cf69f1f327 | 263 | RF_Cmd.putc('Q'); |
OscarLiao | 0:94cf69f1f327 | 264 | wait_us(180); |
OscarLiao | 0:94cf69f1f327 | 265 | RF_CS = 1; |
OscarLiao | 0:94cf69f1f327 | 266 | |
OscarLiao | 0:94cf69f1f327 | 267 | //Right Hind Leg |
OscarLiao | 0:94cf69f1f327 | 268 | RH_CS = 0; |
OscarLiao | 0:94cf69f1f327 | 269 | wait_us(20); |
OscarLiao | 0:94cf69f1f327 | 270 | RH_Cmd.putc(RH_Cmd_Hex[0]); |
OscarLiao | 0:94cf69f1f327 | 271 | RH_Cmd.putc(RH_Cmd_Hex[1]); |
OscarLiao | 0:94cf69f1f327 | 272 | RH_Cmd.putc(RH_Cmd_Hex[2]); |
OscarLiao | 0:94cf69f1f327 | 273 | RH_Cmd.putc('Q'); |
OscarLiao | 0:94cf69f1f327 | 274 | wait_us(180); |
OscarLiao | 0:94cf69f1f327 | 275 | RH_CS = 1; |
OscarLiao | 0:94cf69f1f327 | 276 | |
OscarLiao | 0:94cf69f1f327 | 277 | LD1 = 0; |
OscarLiao | 0:94cf69f1f327 | 278 | |
OscarLiao | 0:94cf69f1f327 | 279 | Flag_4 = 0; |
OscarLiao | 0:94cf69f1f327 | 280 | } |
OscarLiao | 0:94cf69f1f327 | 281 | } |
OscarLiao | 0:94cf69f1f327 | 282 | |
OscarLiao | 0:94cf69f1f327 | 283 | |
OscarLiao | 0:94cf69f1f327 | 284 | } |
OscarLiao | 0:94cf69f1f327 | 285 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of main funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 286 | |
OscarLiao | 0:94cf69f1f327 | 287 | |
OscarLiao | 0:94cf69f1f327 | 288 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Timebase funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 289 | void init_TIMER() //set TT_main{} rate |
OscarLiao | 0:94cf69f1f327 | 290 | { |
OscarLiao | 0:94cf69f1f327 | 291 | TT.attach_us(&TT_main, Rms); |
OscarLiao | 0:94cf69f1f327 | 292 | } |
OscarLiao | 0:94cf69f1f327 | 293 | void TT_main() //interrupt function by TT |
OscarLiao | 0:94cf69f1f327 | 294 | { |
OscarLiao | 0:94cf69f1f327 | 295 | read_IMU(); //read IMU data give raw data |
OscarLiao | 0:94cf69f1f327 | 296 | state_update(); //estimation of new attitude |
OscarLiao | 0:94cf69f1f327 | 297 | |
OscarLiao | 0:94cf69f1f327 | 298 | Task_1_count = Task_1_count + 1; |
OscarLiao | 0:94cf69f1f327 | 299 | if(Task_1_count > Task_1_NN) { |
OscarLiao | 0:94cf69f1f327 | 300 | Task_1_count = 0; //Task triggering |
OscarLiao | 0:94cf69f1f327 | 301 | Flag_1 = 1; |
OscarLiao | 0:94cf69f1f327 | 302 | } |
OscarLiao | 0:94cf69f1f327 | 303 | |
OscarLiao | 0:94cf69f1f327 | 304 | Task_4_count = Task_4_count + 1; |
OscarLiao | 0:94cf69f1f327 | 305 | if(Task_4_count > Task_4_NN) { |
OscarLiao | 0:94cf69f1f327 | 306 | Task_4_count = 0; //Task triggering |
OscarLiao | 0:94cf69f1f327 | 307 | Flag_4 = 1; |
OscarLiao | 0:94cf69f1f327 | 308 | } |
OscarLiao | 0:94cf69f1f327 | 309 | } |
OscarLiao | 0:94cf69f1f327 | 310 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Timebase funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 311 | |
OscarLiao | 0:94cf69f1f327 | 312 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Button funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 313 | void pressed(void) |
OscarLiao | 0:94cf69f1f327 | 314 | { |
OscarLiao | 0:94cf69f1f327 | 315 | Ki = 0.005; |
OscarLiao | 0:94cf69f1f327 | 316 | Kd = 0.001; |
OscarLiao | 0:94cf69f1f327 | 317 | } |
OscarLiao | 0:94cf69f1f327 | 318 | |
OscarLiao | 0:94cf69f1f327 | 319 | void released(void) |
OscarLiao | 0:94cf69f1f327 | 320 | { |
OscarLiao | 0:94cf69f1f327 | 321 | |
OscarLiao | 0:94cf69f1f327 | 322 | } |
OscarLiao | 0:94cf69f1f327 | 323 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of button funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 324 | |
OscarLiao | 0:94cf69f1f327 | 325 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Balance funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 326 | |
OscarLiao | 0:94cf69f1f327 | 327 | void Balance(void) |
OscarLiao | 0:94cf69f1f327 | 328 | { |
OscarLiao | 0:94cf69f1f327 | 329 | Pu = Kp*Ele_phy; |
OscarLiao | 0:94cf69f1f327 | 330 | Iu += Ki*Ele_phy; |
OscarLiao | 0:94cf69f1f327 | 331 | Du = Kd*(Ele_phy-Ele_phy_old)/(dt*(Task_1_count+1)); |
OscarLiao | 0:94cf69f1f327 | 332 | |
OscarLiao | 0:94cf69f1f327 | 333 | LF_q_0_E[1] = LF_q_0_E[1] - (Pu+Iu+Du); |
OscarLiao | 0:94cf69f1f327 | 334 | RF_q_0_E[1] = RF_q_0_E[1] - (Pu+Iu+Du); |
OscarLiao | 0:94cf69f1f327 | 335 | LH_q_0_E[1] = LH_q_0_E[1] + (Pu+Iu+Du); |
OscarLiao | 0:94cf69f1f327 | 336 | RH_q_0_E[1] = RH_q_0_E[1] + (Pu+Iu+Du); |
OscarLiao | 0:94cf69f1f327 | 337 | |
OscarLiao | 0:94cf69f1f327 | 338 | LF_q_0_E[2] = - Ele_phy; |
OscarLiao | 0:94cf69f1f327 | 339 | RF_q_0_E[2] = - Ele_phy; |
OscarLiao | 0:94cf69f1f327 | 340 | LH_q_0_E[2] = - Ele_phy; |
OscarLiao | 0:94cf69f1f327 | 341 | RH_q_0_E[2] = - Ele_phy; |
OscarLiao | 0:94cf69f1f327 | 342 | |
OscarLiao | 0:94cf69f1f327 | 343 | //Update |
OscarLiao | 0:94cf69f1f327 | 344 | Ele_phy_old = Ele_phy; |
OscarLiao | 0:94cf69f1f327 | 345 | |
OscarLiao | 0:94cf69f1f327 | 346 | Debug.printf("%.3f, %.3f, %.3f\r", Pu, Iu, Du); |
OscarLiao | 0:94cf69f1f327 | 347 | } |
OscarLiao | 0:94cf69f1f327 | 348 | |
OscarLiao | 0:94cf69f1f327 | 349 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Balance funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 350 | |
OscarLiao | 0:94cf69f1f327 | 351 | |
OscarLiao | 0:94cf69f1f327 | 352 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
OscarLiao | 0:94cf69f1f327 | 353 | void init_IMU(void) //initialize |
OscarLiao | 0:94cf69f1f327 | 354 | { |
OscarLiao | 0:94cf69f1f327 | 355 | //gloable config |
OscarLiao | 0:94cf69f1f327 | 356 | SPI_CSXM = 1; //high as init for disable SPI |
OscarLiao | 0:94cf69f1f327 | 357 | SPI_CSG = 1; |
OscarLiao | 0:94cf69f1f327 | 358 | spi.format(8, 3); //byte width, spi mode |
OscarLiao | 0:94cf69f1f327 | 359 | spi.frequency(4000000); //8MHz |
OscarLiao | 0:94cf69f1f327 | 360 | |
OscarLiao | 0:94cf69f1f327 | 361 | //for GYRO config |
OscarLiao | 0:94cf69f1f327 | 362 | SPI_CSG = 0; //start spi talking |
OscarLiao | 0:94cf69f1f327 | 363 | spi.write(CTRL_REG1_G); |
OscarLiao | 0:94cf69f1f327 | 364 | spi.write(0x9F); //data rate 380 Hz/ cut off 25 Hz |
OscarLiao | 0:94cf69f1f327 | 365 | SPI_CSG = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 366 | |
OscarLiao | 0:94cf69f1f327 | 367 | SPI_CSG = 0; //start spi talking |
OscarLiao | 0:94cf69f1f327 | 368 | spi.write(CTRL_REG4_G); |
OscarLiao | 0:94cf69f1f327 | 369 | spi.write(0x10); //Scle 500dps |
OscarLiao | 0:94cf69f1f327 | 370 | SPI_CSG = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 371 | |
OscarLiao | 0:94cf69f1f327 | 372 | //for ACC config |
OscarLiao | 0:94cf69f1f327 | 373 | SPI_CSXM = 0; //start spi talking |
OscarLiao | 0:94cf69f1f327 | 374 | spi.write(CTRL_REG1_XM); |
OscarLiao | 0:94cf69f1f327 | 375 | spi.write(0x87); //data rate 400 Hz/ Enable |
OscarLiao | 0:94cf69f1f327 | 376 | SPI_CSXM = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 377 | |
OscarLiao | 0:94cf69f1f327 | 378 | SPI_CSXM = 0; //start spi talking |
OscarLiao | 0:94cf69f1f327 | 379 | spi.write(CTRL_REG2_XM); |
OscarLiao | 0:94cf69f1f327 | 380 | spi.write(0xC8); //cut off 50 Hz/ Scale +-4g |
OscarLiao | 0:94cf69f1f327 | 381 | SPI_CSXM = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 382 | } |
OscarLiao | 0:94cf69f1f327 | 383 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
OscarLiao | 0:94cf69f1f327 | 384 | |
OscarLiao | 0:94cf69f1f327 | 385 | |
OscarLiao | 0:94cf69f1f327 | 386 | |
OscarLiao | 0:94cf69f1f327 | 387 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_G/Xdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
OscarLiao | 0:94cf69f1f327 | 388 | void init_GXdrift(void) //initialize |
OscarLiao | 0:94cf69f1f327 | 389 | { |
OscarLiao | 0:94cf69f1f327 | 390 | for(int i=0; i<1000; i++) { |
OscarLiao | 0:94cf69f1f327 | 391 | read_IMU(); //note GXdrift = 0 at this moment |
OscarLiao | 0:94cf69f1f327 | 392 | gDIR[0][0] = gDIR[0][0] - Wx; |
OscarLiao | 0:94cf69f1f327 | 393 | gDIR[0][1] = gDIR[0][1] - Wy; |
OscarLiao | 0:94cf69f1f327 | 394 | gDIR[0][2] = gDIR[0][2] - Wz; |
OscarLiao | 0:94cf69f1f327 | 395 | wait_ms(2); |
OscarLiao | 0:94cf69f1f327 | 396 | } |
OscarLiao | 0:94cf69f1f327 | 397 | |
OscarLiao | 0:94cf69f1f327 | 398 | Gdx = gDIR[0][0] /1000.0f; |
OscarLiao | 0:94cf69f1f327 | 399 | Gdy = gDIR[0][1] /1000.0f; |
OscarLiao | 0:94cf69f1f327 | 400 | Gdz = gDIR[0][2] /1000.0f; |
OscarLiao | 0:94cf69f1f327 | 401 | //Debug.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz); |
OscarLiao | 0:94cf69f1f327 | 402 | |
OscarLiao | 0:94cf69f1f327 | 403 | for(int i=0; i<1000; i++) { |
OscarLiao | 0:94cf69f1f327 | 404 | read_IMU(); //note GXdrift = 0 at this moment |
OscarLiao | 0:94cf69f1f327 | 405 | gDIR[0][0] = gDIR[0][0] - Ax; |
OscarLiao | 0:94cf69f1f327 | 406 | gDIR[0][1] = gDIR[0][1] - Ay; |
OscarLiao | 0:94cf69f1f327 | 407 | gDIR[0][2] = gDIR[0][2] - Az; |
OscarLiao | 0:94cf69f1f327 | 408 | wait_ms(2); |
OscarLiao | 0:94cf69f1f327 | 409 | } |
OscarLiao | 0:94cf69f1f327 | 410 | Adx = gDIR[0][0] /1000.0f; |
OscarLiao | 0:94cf69f1f327 | 411 | Ady = gDIR[0][1] /1000.0f; |
OscarLiao | 0:94cf69f1f327 | 412 | Adz = gDIR[0][2] /1000.0f - 1.0f; |
OscarLiao | 0:94cf69f1f327 | 413 | |
OscarLiao | 0:94cf69f1f327 | 414 | |
OscarLiao | 0:94cf69f1f327 | 415 | // pc.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz); |
OscarLiao | 0:94cf69f1f327 | 416 | gDIR[0][0] = 0; |
OscarLiao | 0:94cf69f1f327 | 417 | gDIR[0][1] = 0; |
OscarLiao | 0:94cf69f1f327 | 418 | gDIR[0][2] = -1; |
OscarLiao | 0:94cf69f1f327 | 419 | } |
OscarLiao | 0:94cf69f1f327 | 420 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_GXdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
OscarLiao | 0:94cf69f1f327 | 421 | |
OscarLiao | 0:94cf69f1f327 | 422 | |
OscarLiao | 0:94cf69f1f327 | 423 | |
OscarLiao | 0:94cf69f1f327 | 424 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
OscarLiao | 0:94cf69f1f327 | 425 | void read_IMU(void) //read IMU data give raw data |
OscarLiao | 0:94cf69f1f327 | 426 | { |
OscarLiao | 0:94cf69f1f327 | 427 | //Wx |
OscarLiao | 0:94cf69f1f327 | 428 | SPI_CSG = 0; //start spi talking Wx |
OscarLiao | 0:94cf69f1f327 | 429 | spi.write(0xE8); //read B11101000 read/multi/address |
OscarLiao | 0:94cf69f1f327 | 430 | low_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 431 | high_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 432 | Buff = high_byte << 8 |low_byte; |
OscarLiao | 0:94cf69f1f327 | 433 | SPI_CSG = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 434 | // Wx = Buff * Gpx + Gdx; |
OscarLiao | 0:94cf69f1f327 | 435 | Wx = lpf(Buff * Gpx + Gdx, Wx, 48.0f); |
OscarLiao | 0:94cf69f1f327 | 436 | //Wy |
OscarLiao | 0:94cf69f1f327 | 437 | SPI_CSG = 0; //start spi talking Wx |
OscarLiao | 0:94cf69f1f327 | 438 | spi.write(0xEA); //read B11101010 read/multi/address |
OscarLiao | 0:94cf69f1f327 | 439 | low_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 440 | high_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 441 | Buff = high_byte << 8 |low_byte; |
OscarLiao | 0:94cf69f1f327 | 442 | SPI_CSG = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 443 | // Wy = Buff * Gpy + Gdy; |
OscarLiao | 0:94cf69f1f327 | 444 | Wy = lpf(Buff * Gpy + Gdy, Wy, 48.0f); |
OscarLiao | 0:94cf69f1f327 | 445 | //Wz |
OscarLiao | 0:94cf69f1f327 | 446 | SPI_CSG = 0; //start spi talking Wx |
OscarLiao | 0:94cf69f1f327 | 447 | spi.write(0xEC); //read B11101100 read/multi/address |
OscarLiao | 0:94cf69f1f327 | 448 | low_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 449 | high_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 450 | Buff = high_byte << 8 |low_byte; |
OscarLiao | 0:94cf69f1f327 | 451 | SPI_CSG = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 452 | // Wz = Buff * Gpz + Gdz; |
OscarLiao | 0:94cf69f1f327 | 453 | Wz = lpf(Buff * Gpz + Gdz, Wz, 48.0f); |
OscarLiao | 0:94cf69f1f327 | 454 | //Ax |
OscarLiao | 0:94cf69f1f327 | 455 | SPI_CSXM = 0; //start spi talking Ax |
OscarLiao | 0:94cf69f1f327 | 456 | spi.write(0xE8); //read B11101000 read/multi/address |
OscarLiao | 0:94cf69f1f327 | 457 | low_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 458 | high_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 459 | Buff = high_byte << 8 |low_byte; |
OscarLiao | 0:94cf69f1f327 | 460 | SPI_CSXM = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 461 | Ax = lpf(Buff*Apx + Adx, Ax, 13.0f); |
OscarLiao | 0:94cf69f1f327 | 462 | //Ay |
OscarLiao | 0:94cf69f1f327 | 463 | SPI_CSXM = 0; //start spi talking Ax |
OscarLiao | 0:94cf69f1f327 | 464 | spi.write(0xEA); //read B11101010 read/multi/address |
OscarLiao | 0:94cf69f1f327 | 465 | low_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 466 | high_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 467 | Buff = high_byte << 8 |low_byte; |
OscarLiao | 0:94cf69f1f327 | 468 | SPI_CSXM = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 469 | Ay = lpf(Buff*Apy + Ady, Ay, 13.0f); |
OscarLiao | 0:94cf69f1f327 | 470 | //Az |
OscarLiao | 0:94cf69f1f327 | 471 | SPI_CSXM = 0; //start spi talking Ax |
OscarLiao | 0:94cf69f1f327 | 472 | spi.write(0xEC); //read B11101100 read/multi/address |
OscarLiao | 0:94cf69f1f327 | 473 | low_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 474 | high_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 475 | Buff = high_byte << 8 |low_byte; |
OscarLiao | 0:94cf69f1f327 | 476 | SPI_CSXM = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 477 | Az = lpf(Buff*Apz + Adz, Az, 13.0f); |
OscarLiao | 0:94cf69f1f327 | 478 | //Debug.printf("%.4f,%.4f,%.4f\r", Ax, Ay, Az); |
OscarLiao | 0:94cf69f1f327 | 479 | } |
OscarLiao | 0:94cf69f1f327 | 480 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
OscarLiao | 0:94cf69f1f327 | 481 | |
OscarLiao | 0:94cf69f1f327 | 482 | |
OscarLiao | 0:94cf69f1f327 | 483 | |
OscarLiao | 0:94cf69f1f327 | 484 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓state_update funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
OscarLiao | 0:94cf69f1f327 | 485 | void state_update(void) //estimation of new attitude |
OscarLiao | 0:94cf69f1f327 | 486 | { |
OscarLiao | 0:94cf69f1f327 | 487 | //predict |
OscarLiao | 0:94cf69f1f327 | 488 | gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt; |
OscarLiao | 0:94cf69f1f327 | 489 | gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt; |
OscarLiao | 0:94cf69f1f327 | 490 | gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt; |
OscarLiao | 0:94cf69f1f327 | 491 | |
OscarLiao | 0:94cf69f1f327 | 492 | //update |
OscarLiao | 0:94cf69f1f327 | 493 | gDIR[0][0] = (1-Bet) * gDIR[0][0] + Bet * Ax; |
OscarLiao | 0:94cf69f1f327 | 494 | gDIR[0][1] = (1-Bet) * gDIR[0][1] + Bet * Ay; |
OscarLiao | 0:94cf69f1f327 | 495 | gDIR[0][2] = (1-Bet) * gDIR[0][2] + Bet * Az; |
OscarLiao | 0:94cf69f1f327 | 496 | |
OscarLiao | 0:94cf69f1f327 | 497 | //nutralizing |
OscarLiao | 0:94cf69f1f327 | 498 | gUnity = sqrt( gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2] ); |
OscarLiao | 0:94cf69f1f327 | 499 | for(int i=0; i<3; i++) { |
OscarLiao | 0:94cf69f1f327 | 500 | gDIR[0][i] = gDIR[0][i] / gUnity; |
OscarLiao | 0:94cf69f1f327 | 501 | } |
OscarLiao | 0:94cf69f1f327 | 502 | |
OscarLiao | 0:94cf69f1f327 | 503 | //transfer |
OscarLiao | 0:94cf69f1f327 | 504 | Ele_phy = asin(gDIR[0][0]); |
OscarLiao | 0:94cf69f1f327 | 505 | Til_phy = asin(-gDIR[0][1] / cos(Ele_phy)); |
OscarLiao | 0:94cf69f1f327 | 506 | } |
OscarLiao | 0:94cf69f1f327 | 507 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
OscarLiao | 0:94cf69f1f327 | 508 | |
OscarLiao | 0:94cf69f1f327 | 509 | |
OscarLiao | 0:94cf69f1f327 | 510 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
OscarLiao | 0:94cf69f1f327 | 511 | float lpf(float input, float output_old, float frequency) |
OscarLiao | 0:94cf69f1f327 | 512 | { |
OscarLiao | 0:94cf69f1f327 | 513 | float output = 0; |
OscarLiao | 0:94cf69f1f327 | 514 | output = (output_old + frequency*dt*input) / (1 + frequency*dt); |
OscarLiao | 0:94cf69f1f327 | 515 | return output; |
OscarLiao | 0:94cf69f1f327 | 516 | } |
OscarLiao | 0:94cf69f1f327 | 517 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
OscarLiao | 0:94cf69f1f327 | 518 | |
OscarLiao | 0:94cf69f1f327 | 519 | |
OscarLiao | 0:94cf69f1f327 | 520 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Rx_irq funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 521 | void Rx_irq(void) |
OscarLiao | 0:94cf69f1f327 | 522 | { |
OscarLiao | 0:94cf69f1f327 | 523 | SerBuff[0] = Debug.getc(); |
OscarLiao | 0:94cf69f1f327 | 524 | Debug.putc(SerBuff[0]); |
OscarLiao | 0:94cf69f1f327 | 525 | } |
OscarLiao | 0:94cf69f1f327 | 526 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Rx_irq funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |