Oscar Liao
/
CHICAGO_balance_test_PID_copy
balance
main.cpp@1:15dd461fbc2a, 2018-12-21 (annotated)
- Committer:
- OscarLiao
- Date:
- Fri Dec 21 06:27:26 2018 +0000
- Revision:
- 1:15dd461fbc2a
- Parent:
- 0:94cf69f1f327
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 | 1:15dd461fbc2a | 24 | #define init_leg_length 0.23f |
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 | 1:15dd461fbc2a | 139 | float pitchU[3] = {0, 0, 0}; |
OscarLiao | 1:15dd461fbc2a | 140 | float pitchE[3] = {0, 0, 0}; |
OscarLiao | 1:15dd461fbc2a | 141 | float rollU = 0; |
OscarLiao | 0:94cf69f1f327 | 142 | |
OscarLiao | 0:94cf69f1f327 | 143 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 144 | |
OscarLiao | 0:94cf69f1f327 | 145 | |
OscarLiao | 0:94cf69f1f327 | 146 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Function registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 147 | void init_TIMER(); //set TT_main() rate |
OscarLiao | 0:94cf69f1f327 | 148 | void TT_main(); //timebase function rated by TT |
OscarLiao | 0:94cf69f1f327 | 149 | |
OscarLiao | 0:94cf69f1f327 | 150 | void init_IMU(); //initialize IMU |
OscarLiao | 0:94cf69f1f327 | 151 | void init_GXdrift(); //read GXdrift at start up |
OscarLiao | 0:94cf69f1f327 | 152 | void read_IMU(); //read IMU data give raw data |
OscarLiao | 0:94cf69f1f327 | 153 | void state_update(); //estimation of new attitude |
OscarLiao | 0:94cf69f1f327 | 154 | |
OscarLiao | 0:94cf69f1f327 | 155 | float lpf(float input, float output_old, float frequency); //lpf discrete |
OscarLiao | 0:94cf69f1f327 | 156 | |
OscarLiao | 0:94cf69f1f327 | 157 | void pressed(); |
OscarLiao | 0:94cf69f1f327 | 158 | void Balance(); |
OscarLiao | 0:94cf69f1f327 | 159 | |
OscarLiao | 0:94cf69f1f327 | 160 | void Rx_irq(); |
OscarLiao | 0:94cf69f1f327 | 161 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 162 | |
OscarLiao | 0:94cf69f1f327 | 163 | |
OscarLiao | 0:94cf69f1f327 | 164 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡main funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 165 | int main() |
OscarLiao | 0:94cf69f1f327 | 166 | { |
OscarLiao | 0:94cf69f1f327 | 167 | init_IMU(); //initialize IMU |
OscarLiao | 0:94cf69f1f327 | 168 | init_GXdrift(); //read GXdrift at start up |
OscarLiao | 0:94cf69f1f327 | 169 | |
OscarLiao | 0:94cf69f1f327 | 170 | Debug.baud(115200); //set baud rate |
OscarLiao | 0:94cf69f1f327 | 171 | LF_Cmd.baud(115200); |
OscarLiao | 0:94cf69f1f327 | 172 | LH_Cmd.baud(115200); |
OscarLiao | 0:94cf69f1f327 | 173 | RF_Cmd.baud(115200); |
OscarLiao | 0:94cf69f1f327 | 174 | RH_Cmd.baud(115200); |
OscarLiao | 0:94cf69f1f327 | 175 | |
OscarLiao | 0:94cf69f1f327 | 176 | wait_ms(10); |
OscarLiao | 0:94cf69f1f327 | 177 | init_TIMER(); //start TT_main |
OscarLiao | 0:94cf69f1f327 | 178 | |
OscarLiao | 0:94cf69f1f327 | 179 | Debug.attach(&Rx_irq,Serial::RxIrq); //Start recieving message |
OscarLiao | 0:94cf69f1f327 | 180 | LF_CS = 1; |
OscarLiao | 0:94cf69f1f327 | 181 | |
OscarLiao | 0:94cf69f1f327 | 182 | wait_ms(200); |
OscarLiao | 0:94cf69f1f327 | 183 | |
OscarLiao | 0:94cf69f1f327 | 184 | button.fall(&pressed); //button pressed |
OscarLiao | 0:94cf69f1f327 | 185 | |
OscarLiao | 0:94cf69f1f327 | 186 | while(1) { //main() loop |
OscarLiao | 0:94cf69f1f327 | 187 | |
OscarLiao | 0:94cf69f1f327 | 188 | // if(Debug.readable()) { |
OscarLiao | 0:94cf69f1f327 | 189 | // SerBuff[Recieve_index] = Debug.getc(); |
OscarLiao | 0:94cf69f1f327 | 190 | // Debug.putc(SerBuff[Recieve_index]); |
OscarLiao | 0:94cf69f1f327 | 191 | // Recieve_index = Recieve_index + 1; |
OscarLiao | 0:94cf69f1f327 | 192 | // } |
OscarLiao | 0:94cf69f1f327 | 193 | |
OscarLiao | 0:94cf69f1f327 | 194 | // if(device.readable()) { |
OscarLiao | 0:94cf69f1f327 | 195 | // pc.putc(device.getc()); |
OscarLiao | 0:94cf69f1f327 | 196 | // } |
OscarLiao | 0:94cf69f1f327 | 197 | //Balance |
OscarLiao | 0:94cf69f1f327 | 198 | if(Flag_1 == 1) { |
OscarLiao | 0:94cf69f1f327 | 199 | |
OscarLiao | 0:94cf69f1f327 | 200 | //Initial Position |
OscarLiao | 0:94cf69f1f327 | 201 | LF_q_0_E[1] = init_leg_length; |
OscarLiao | 0:94cf69f1f327 | 202 | LH_q_0_E[1] = init_leg_length; |
OscarLiao | 0:94cf69f1f327 | 203 | RF_q_0_E[1] = init_leg_length; |
OscarLiao | 0:94cf69f1f327 | 204 | RH_q_0_E[1] = init_leg_length; |
OscarLiao | 0:94cf69f1f327 | 205 | |
OscarLiao | 0:94cf69f1f327 | 206 | Balance(); |
OscarLiao | 0:94cf69f1f327 | 207 | Flag_1 = 0; |
OscarLiao | 0:94cf69f1f327 | 208 | } |
OscarLiao | 0:94cf69f1f327 | 209 | |
OscarLiao | 0:94cf69f1f327 | 210 | //Send Command |
OscarLiao | 0:94cf69f1f327 | 211 | if(Flag_4 == 1) { |
OscarLiao | 0:94cf69f1f327 | 212 | |
OscarLiao | 0:94cf69f1f327 | 213 | LF_Cmd_Hex[0] = (LF_q_0_E[0]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 214 | LF_Cmd_Hex[1] = ((LF_q_0_E[1]-0.1)/0.2)*255; |
OscarLiao | 0:94cf69f1f327 | 215 | LF_Cmd_Hex[2] = (LF_q_0_E[2]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 216 | |
OscarLiao | 0:94cf69f1f327 | 217 | LH_Cmd_Hex[0] = (LH_q_0_E[0]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 218 | LH_Cmd_Hex[1] = ((LH_q_0_E[1]-0.1)/0.2)*255; |
OscarLiao | 0:94cf69f1f327 | 219 | LH_Cmd_Hex[2] = (LH_q_0_E[2]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 220 | |
OscarLiao | 0:94cf69f1f327 | 221 | RF_Cmd_Hex[0] = (RF_q_0_E[0]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 222 | RF_Cmd_Hex[1] = ((RF_q_0_E[1]-0.1)/0.2)*255; |
OscarLiao | 0:94cf69f1f327 | 223 | RF_Cmd_Hex[2] = (RF_q_0_E[2]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 224 | |
OscarLiao | 0:94cf69f1f327 | 225 | RH_Cmd_Hex[0] = (RH_q_0_E[0]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 226 | RH_Cmd_Hex[1] = ((RH_q_0_E[1]-0.1)/0.2)*255; |
OscarLiao | 0:94cf69f1f327 | 227 | RH_Cmd_Hex[2] = (RH_q_0_E[2]+pi/4)/(pi/2)*255; |
OscarLiao | 0:94cf69f1f327 | 228 | |
OscarLiao | 0:94cf69f1f327 | 229 | //Debug.printf("%d, %d, %d\r", LH_Cmd_Hex[0], LH_Cmd_Hex[1], LH_Cmd_Hex[2]); |
OscarLiao | 0:94cf69f1f327 | 230 | //LED |
OscarLiao | 0:94cf69f1f327 | 231 | LD1 = 1; |
OscarLiao | 0:94cf69f1f327 | 232 | |
OscarLiao | 0:94cf69f1f327 | 233 | //Left Front Leg |
OscarLiao | 0:94cf69f1f327 | 234 | LF_CS = 0; |
OscarLiao | 0:94cf69f1f327 | 235 | wait_us(20); |
OscarLiao | 0:94cf69f1f327 | 236 | LF_Cmd.putc(LF_Cmd_Hex[0]); |
OscarLiao | 0:94cf69f1f327 | 237 | LF_Cmd.putc(LF_Cmd_Hex[1]); |
OscarLiao | 0:94cf69f1f327 | 238 | LF_Cmd.putc(LF_Cmd_Hex[2]); |
OscarLiao | 0:94cf69f1f327 | 239 | LF_Cmd.putc('Q'); |
OscarLiao | 0:94cf69f1f327 | 240 | wait_us(180); |
OscarLiao | 0:94cf69f1f327 | 241 | LF_CS = 1; |
OscarLiao | 0:94cf69f1f327 | 242 | |
OscarLiao | 0:94cf69f1f327 | 243 | //Left Hind Leg |
OscarLiao | 0:94cf69f1f327 | 244 | LH_CS = 0; |
OscarLiao | 0:94cf69f1f327 | 245 | wait_us(20); |
OscarLiao | 0:94cf69f1f327 | 246 | LH_Cmd.putc(LH_Cmd_Hex[0]); |
OscarLiao | 0:94cf69f1f327 | 247 | LH_Cmd.putc(LH_Cmd_Hex[1]); |
OscarLiao | 0:94cf69f1f327 | 248 | LH_Cmd.putc(LH_Cmd_Hex[2]); |
OscarLiao | 0:94cf69f1f327 | 249 | LH_Cmd.putc('Q'); |
OscarLiao | 0:94cf69f1f327 | 250 | wait_us(180); |
OscarLiao | 0:94cf69f1f327 | 251 | LH_CS = 1; |
OscarLiao | 0:94cf69f1f327 | 252 | |
OscarLiao | 0:94cf69f1f327 | 253 | //Right Front Leg |
OscarLiao | 0:94cf69f1f327 | 254 | RF_CS = 0; |
OscarLiao | 0:94cf69f1f327 | 255 | wait_us(20); |
OscarLiao | 0:94cf69f1f327 | 256 | RF_Cmd.putc(RF_Cmd_Hex[0]); |
OscarLiao | 0:94cf69f1f327 | 257 | RF_Cmd.putc(RF_Cmd_Hex[1]); |
OscarLiao | 0:94cf69f1f327 | 258 | RF_Cmd.putc(RF_Cmd_Hex[2]); |
OscarLiao | 0:94cf69f1f327 | 259 | RF_Cmd.putc('Q'); |
OscarLiao | 0:94cf69f1f327 | 260 | wait_us(180); |
OscarLiao | 0:94cf69f1f327 | 261 | RF_CS = 1; |
OscarLiao | 0:94cf69f1f327 | 262 | |
OscarLiao | 0:94cf69f1f327 | 263 | //Right Hind Leg |
OscarLiao | 0:94cf69f1f327 | 264 | RH_CS = 0; |
OscarLiao | 0:94cf69f1f327 | 265 | wait_us(20); |
OscarLiao | 0:94cf69f1f327 | 266 | RH_Cmd.putc(RH_Cmd_Hex[0]); |
OscarLiao | 0:94cf69f1f327 | 267 | RH_Cmd.putc(RH_Cmd_Hex[1]); |
OscarLiao | 0:94cf69f1f327 | 268 | RH_Cmd.putc(RH_Cmd_Hex[2]); |
OscarLiao | 0:94cf69f1f327 | 269 | RH_Cmd.putc('Q'); |
OscarLiao | 0:94cf69f1f327 | 270 | wait_us(180); |
OscarLiao | 0:94cf69f1f327 | 271 | RH_CS = 1; |
OscarLiao | 0:94cf69f1f327 | 272 | |
OscarLiao | 0:94cf69f1f327 | 273 | LD1 = 0; |
OscarLiao | 0:94cf69f1f327 | 274 | |
OscarLiao | 0:94cf69f1f327 | 275 | Flag_4 = 0; |
OscarLiao | 0:94cf69f1f327 | 276 | } |
OscarLiao | 0:94cf69f1f327 | 277 | } |
OscarLiao | 0:94cf69f1f327 | 278 | |
OscarLiao | 0:94cf69f1f327 | 279 | |
OscarLiao | 0:94cf69f1f327 | 280 | } |
OscarLiao | 0:94cf69f1f327 | 281 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of main funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 282 | |
OscarLiao | 0:94cf69f1f327 | 283 | |
OscarLiao | 0:94cf69f1f327 | 284 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Timebase funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 285 | void init_TIMER() //set TT_main{} rate |
OscarLiao | 0:94cf69f1f327 | 286 | { |
OscarLiao | 0:94cf69f1f327 | 287 | TT.attach_us(&TT_main, Rms); |
OscarLiao | 0:94cf69f1f327 | 288 | } |
OscarLiao | 0:94cf69f1f327 | 289 | void TT_main() //interrupt function by TT |
OscarLiao | 0:94cf69f1f327 | 290 | { |
OscarLiao | 0:94cf69f1f327 | 291 | read_IMU(); //read IMU data give raw data |
OscarLiao | 0:94cf69f1f327 | 292 | state_update(); //estimation of new attitude |
OscarLiao | 0:94cf69f1f327 | 293 | |
OscarLiao | 0:94cf69f1f327 | 294 | Task_1_count = Task_1_count + 1; |
OscarLiao | 0:94cf69f1f327 | 295 | if(Task_1_count > Task_1_NN) { |
OscarLiao | 0:94cf69f1f327 | 296 | Task_1_count = 0; //Task triggering |
OscarLiao | 0:94cf69f1f327 | 297 | Flag_1 = 1; |
OscarLiao | 0:94cf69f1f327 | 298 | } |
OscarLiao | 0:94cf69f1f327 | 299 | |
OscarLiao | 0:94cf69f1f327 | 300 | Task_4_count = Task_4_count + 1; |
OscarLiao | 0:94cf69f1f327 | 301 | if(Task_4_count > Task_4_NN) { |
OscarLiao | 0:94cf69f1f327 | 302 | Task_4_count = 0; //Task triggering |
OscarLiao | 0:94cf69f1f327 | 303 | Flag_4 = 1; |
OscarLiao | 0:94cf69f1f327 | 304 | } |
OscarLiao | 0:94cf69f1f327 | 305 | } |
OscarLiao | 0:94cf69f1f327 | 306 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Timebase funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 307 | |
OscarLiao | 0:94cf69f1f327 | 308 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Button funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 309 | void pressed(void) |
OscarLiao | 0:94cf69f1f327 | 310 | { |
OscarLiao | 1:15dd461fbc2a | 311 | |
OscarLiao | 0:94cf69f1f327 | 312 | } |
OscarLiao | 0:94cf69f1f327 | 313 | |
OscarLiao | 0:94cf69f1f327 | 314 | void released(void) |
OscarLiao | 0:94cf69f1f327 | 315 | { |
OscarLiao | 0:94cf69f1f327 | 316 | |
OscarLiao | 0:94cf69f1f327 | 317 | } |
OscarLiao | 0:94cf69f1f327 | 318 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of button funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 319 | |
OscarLiao | 0:94cf69f1f327 | 320 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Balance funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 321 | |
OscarLiao | 0:94cf69f1f327 | 322 | void Balance(void) |
OscarLiao | 0:94cf69f1f327 | 323 | { |
OscarLiao | 1:15dd461fbc2a | 324 | //update pitch controller |
OscarLiao | 1:15dd461fbc2a | 325 | pitchE[0] = Ele_phy; |
OscarLiao | 1:15dd461fbc2a | 326 | pitchU[0] = 0.5*(0.0663*pitchU[1]+ 0.9337*pitchU[2]- 0.13635*(pitchE[0]+0.3535*pitchE[1]-0.6465*pitchE[2])); |
OscarLiao | 1:15dd461fbc2a | 327 | |
OscarLiao | 1:15dd461fbc2a | 328 | pitchE[2] = pitchE[1]; |
OscarLiao | 1:15dd461fbc2a | 329 | pitchE[1] = pitchE[0]; |
OscarLiao | 1:15dd461fbc2a | 330 | pitchU[2] = pitchU[1]; |
OscarLiao | 1:15dd461fbc2a | 331 | pitchU[1] = pitchU[0]; |
OscarLiao | 0:94cf69f1f327 | 332 | |
OscarLiao | 1:15dd461fbc2a | 333 | rollU = 0.05*Til_phy; |
OscarLiao | 1:15dd461fbc2a | 334 | |
OscarLiao | 1:15dd461fbc2a | 335 | // LF_q_0_E[0] = Til_phy; |
OscarLiao | 1:15dd461fbc2a | 336 | // RF_q_0_E[0] = -Til_phy; |
OscarLiao | 1:15dd461fbc2a | 337 | // LH_q_0_E[0] = Til_phy; |
OscarLiao | 1:15dd461fbc2a | 338 | // RH_q_0_E[0] = -Til_phy; |
OscarLiao | 1:15dd461fbc2a | 339 | |
OscarLiao | 1:15dd461fbc2a | 340 | LF_q_0_E[1] = LF_q_0_E[1] + pitchU[0] + rollU; |
OscarLiao | 1:15dd461fbc2a | 341 | RF_q_0_E[1] = RF_q_0_E[1] + pitchU[0] - rollU; |
OscarLiao | 1:15dd461fbc2a | 342 | LH_q_0_E[1] = LH_q_0_E[1] - pitchU[0] + rollU; |
OscarLiao | 1:15dd461fbc2a | 343 | RH_q_0_E[1] = RH_q_0_E[1] - pitchU[0] - rollU; |
OscarLiao | 0:94cf69f1f327 | 344 | |
OscarLiao | 0:94cf69f1f327 | 345 | LF_q_0_E[2] = - Ele_phy; |
OscarLiao | 0:94cf69f1f327 | 346 | RF_q_0_E[2] = - Ele_phy; |
OscarLiao | 0:94cf69f1f327 | 347 | LH_q_0_E[2] = - Ele_phy; |
OscarLiao | 0:94cf69f1f327 | 348 | RH_q_0_E[2] = - Ele_phy; |
OscarLiao | 0:94cf69f1f327 | 349 | |
OscarLiao | 1:15dd461fbc2a | 350 | Debug.printf("%.3f, %.3f\r", Til_phy, rollU); |
OscarLiao | 0:94cf69f1f327 | 351 | } |
OscarLiao | 0:94cf69f1f327 | 352 | |
OscarLiao | 0:94cf69f1f327 | 353 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Balance funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |
OscarLiao | 0:94cf69f1f327 | 354 | |
OscarLiao | 0:94cf69f1f327 | 355 | |
OscarLiao | 0:94cf69f1f327 | 356 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
OscarLiao | 0:94cf69f1f327 | 357 | void init_IMU(void) //initialize |
OscarLiao | 0:94cf69f1f327 | 358 | { |
OscarLiao | 0:94cf69f1f327 | 359 | //gloable config |
OscarLiao | 0:94cf69f1f327 | 360 | SPI_CSXM = 1; //high as init for disable SPI |
OscarLiao | 0:94cf69f1f327 | 361 | SPI_CSG = 1; |
OscarLiao | 0:94cf69f1f327 | 362 | spi.format(8, 3); //byte width, spi mode |
OscarLiao | 0:94cf69f1f327 | 363 | spi.frequency(4000000); //8MHz |
OscarLiao | 0:94cf69f1f327 | 364 | |
OscarLiao | 0:94cf69f1f327 | 365 | //for GYRO config |
OscarLiao | 0:94cf69f1f327 | 366 | SPI_CSG = 0; //start spi talking |
OscarLiao | 0:94cf69f1f327 | 367 | spi.write(CTRL_REG1_G); |
OscarLiao | 0:94cf69f1f327 | 368 | spi.write(0x9F); //data rate 380 Hz/ cut off 25 Hz |
OscarLiao | 0:94cf69f1f327 | 369 | SPI_CSG = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 370 | |
OscarLiao | 0:94cf69f1f327 | 371 | SPI_CSG = 0; //start spi talking |
OscarLiao | 0:94cf69f1f327 | 372 | spi.write(CTRL_REG4_G); |
OscarLiao | 0:94cf69f1f327 | 373 | spi.write(0x10); //Scle 500dps |
OscarLiao | 0:94cf69f1f327 | 374 | SPI_CSG = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 375 | |
OscarLiao | 0:94cf69f1f327 | 376 | //for ACC config |
OscarLiao | 0:94cf69f1f327 | 377 | SPI_CSXM = 0; //start spi talking |
OscarLiao | 0:94cf69f1f327 | 378 | spi.write(CTRL_REG1_XM); |
OscarLiao | 0:94cf69f1f327 | 379 | spi.write(0x87); //data rate 400 Hz/ Enable |
OscarLiao | 0:94cf69f1f327 | 380 | SPI_CSXM = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 381 | |
OscarLiao | 0:94cf69f1f327 | 382 | SPI_CSXM = 0; //start spi talking |
OscarLiao | 0:94cf69f1f327 | 383 | spi.write(CTRL_REG2_XM); |
OscarLiao | 0:94cf69f1f327 | 384 | spi.write(0xC8); //cut off 50 Hz/ Scale +-4g |
OscarLiao | 0:94cf69f1f327 | 385 | SPI_CSXM = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 386 | } |
OscarLiao | 0:94cf69f1f327 | 387 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
OscarLiao | 0:94cf69f1f327 | 388 | |
OscarLiao | 0:94cf69f1f327 | 389 | |
OscarLiao | 0:94cf69f1f327 | 390 | |
OscarLiao | 0:94cf69f1f327 | 391 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_G/Xdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
OscarLiao | 0:94cf69f1f327 | 392 | void init_GXdrift(void) //initialize |
OscarLiao | 0:94cf69f1f327 | 393 | { |
OscarLiao | 0:94cf69f1f327 | 394 | for(int i=0; i<1000; i++) { |
OscarLiao | 0:94cf69f1f327 | 395 | read_IMU(); //note GXdrift = 0 at this moment |
OscarLiao | 0:94cf69f1f327 | 396 | gDIR[0][0] = gDIR[0][0] - Wx; |
OscarLiao | 0:94cf69f1f327 | 397 | gDIR[0][1] = gDIR[0][1] - Wy; |
OscarLiao | 0:94cf69f1f327 | 398 | gDIR[0][2] = gDIR[0][2] - Wz; |
OscarLiao | 0:94cf69f1f327 | 399 | wait_ms(2); |
OscarLiao | 0:94cf69f1f327 | 400 | } |
OscarLiao | 0:94cf69f1f327 | 401 | |
OscarLiao | 0:94cf69f1f327 | 402 | Gdx = gDIR[0][0] /1000.0f; |
OscarLiao | 0:94cf69f1f327 | 403 | Gdy = gDIR[0][1] /1000.0f; |
OscarLiao | 0:94cf69f1f327 | 404 | Gdz = gDIR[0][2] /1000.0f; |
OscarLiao | 0:94cf69f1f327 | 405 | //Debug.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz); |
OscarLiao | 0:94cf69f1f327 | 406 | |
OscarLiao | 0:94cf69f1f327 | 407 | for(int i=0; i<1000; i++) { |
OscarLiao | 0:94cf69f1f327 | 408 | read_IMU(); //note GXdrift = 0 at this moment |
OscarLiao | 0:94cf69f1f327 | 409 | gDIR[0][0] = gDIR[0][0] - Ax; |
OscarLiao | 0:94cf69f1f327 | 410 | gDIR[0][1] = gDIR[0][1] - Ay; |
OscarLiao | 0:94cf69f1f327 | 411 | gDIR[0][2] = gDIR[0][2] - Az; |
OscarLiao | 0:94cf69f1f327 | 412 | wait_ms(2); |
OscarLiao | 0:94cf69f1f327 | 413 | } |
OscarLiao | 0:94cf69f1f327 | 414 | Adx = gDIR[0][0] /1000.0f; |
OscarLiao | 0:94cf69f1f327 | 415 | Ady = gDIR[0][1] /1000.0f; |
OscarLiao | 0:94cf69f1f327 | 416 | Adz = gDIR[0][2] /1000.0f - 1.0f; |
OscarLiao | 0:94cf69f1f327 | 417 | |
OscarLiao | 0:94cf69f1f327 | 418 | |
OscarLiao | 0:94cf69f1f327 | 419 | // pc.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz); |
OscarLiao | 0:94cf69f1f327 | 420 | gDIR[0][0] = 0; |
OscarLiao | 0:94cf69f1f327 | 421 | gDIR[0][1] = 0; |
OscarLiao | 0:94cf69f1f327 | 422 | gDIR[0][2] = -1; |
OscarLiao | 0:94cf69f1f327 | 423 | } |
OscarLiao | 0:94cf69f1f327 | 424 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_GXdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
OscarLiao | 0:94cf69f1f327 | 425 | |
OscarLiao | 0:94cf69f1f327 | 426 | |
OscarLiao | 0:94cf69f1f327 | 427 | |
OscarLiao | 0:94cf69f1f327 | 428 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
OscarLiao | 0:94cf69f1f327 | 429 | void read_IMU(void) //read IMU data give raw data |
OscarLiao | 0:94cf69f1f327 | 430 | { |
OscarLiao | 0:94cf69f1f327 | 431 | //Wx |
OscarLiao | 0:94cf69f1f327 | 432 | SPI_CSG = 0; //start spi talking Wx |
OscarLiao | 0:94cf69f1f327 | 433 | spi.write(0xE8); //read B11101000 read/multi/address |
OscarLiao | 0:94cf69f1f327 | 434 | low_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 435 | high_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 436 | Buff = high_byte << 8 |low_byte; |
OscarLiao | 0:94cf69f1f327 | 437 | SPI_CSG = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 438 | // Wx = Buff * Gpx + Gdx; |
OscarLiao | 0:94cf69f1f327 | 439 | Wx = lpf(Buff * Gpx + Gdx, Wx, 48.0f); |
OscarLiao | 0:94cf69f1f327 | 440 | //Wy |
OscarLiao | 0:94cf69f1f327 | 441 | SPI_CSG = 0; //start spi talking Wx |
OscarLiao | 0:94cf69f1f327 | 442 | spi.write(0xEA); //read B11101010 read/multi/address |
OscarLiao | 0:94cf69f1f327 | 443 | low_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 444 | high_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 445 | Buff = high_byte << 8 |low_byte; |
OscarLiao | 0:94cf69f1f327 | 446 | SPI_CSG = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 447 | // Wy = Buff * Gpy + Gdy; |
OscarLiao | 0:94cf69f1f327 | 448 | Wy = lpf(Buff * Gpy + Gdy, Wy, 48.0f); |
OscarLiao | 0:94cf69f1f327 | 449 | //Wz |
OscarLiao | 0:94cf69f1f327 | 450 | SPI_CSG = 0; //start spi talking Wx |
OscarLiao | 0:94cf69f1f327 | 451 | spi.write(0xEC); //read B11101100 read/multi/address |
OscarLiao | 0:94cf69f1f327 | 452 | low_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 453 | high_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 454 | Buff = high_byte << 8 |low_byte; |
OscarLiao | 0:94cf69f1f327 | 455 | SPI_CSG = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 456 | // Wz = Buff * Gpz + Gdz; |
OscarLiao | 0:94cf69f1f327 | 457 | Wz = lpf(Buff * Gpz + Gdz, Wz, 48.0f); |
OscarLiao | 0:94cf69f1f327 | 458 | //Ax |
OscarLiao | 0:94cf69f1f327 | 459 | SPI_CSXM = 0; //start spi talking Ax |
OscarLiao | 0:94cf69f1f327 | 460 | spi.write(0xE8); //read B11101000 read/multi/address |
OscarLiao | 0:94cf69f1f327 | 461 | low_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 462 | high_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 463 | Buff = high_byte << 8 |low_byte; |
OscarLiao | 0:94cf69f1f327 | 464 | SPI_CSXM = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 465 | Ax = lpf(Buff*Apx + Adx, Ax, 13.0f); |
OscarLiao | 0:94cf69f1f327 | 466 | //Ay |
OscarLiao | 0:94cf69f1f327 | 467 | SPI_CSXM = 0; //start spi talking Ax |
OscarLiao | 0:94cf69f1f327 | 468 | spi.write(0xEA); //read B11101010 read/multi/address |
OscarLiao | 0:94cf69f1f327 | 469 | low_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 470 | high_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 471 | Buff = high_byte << 8 |low_byte; |
OscarLiao | 0:94cf69f1f327 | 472 | SPI_CSXM = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 473 | Ay = lpf(Buff*Apy + Ady, Ay, 13.0f); |
OscarLiao | 0:94cf69f1f327 | 474 | //Az |
OscarLiao | 0:94cf69f1f327 | 475 | SPI_CSXM = 0; //start spi talking Ax |
OscarLiao | 0:94cf69f1f327 | 476 | spi.write(0xEC); //read B11101100 read/multi/address |
OscarLiao | 0:94cf69f1f327 | 477 | low_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 478 | high_byte = spi.write(0); |
OscarLiao | 0:94cf69f1f327 | 479 | Buff = high_byte << 8 |low_byte; |
OscarLiao | 0:94cf69f1f327 | 480 | SPI_CSXM = 1; //end spi talking |
OscarLiao | 0:94cf69f1f327 | 481 | Az = lpf(Buff*Apz + Adz, Az, 13.0f); |
OscarLiao | 0:94cf69f1f327 | 482 | //Debug.printf("%.4f,%.4f,%.4f\r", Ax, Ay, Az); |
OscarLiao | 0:94cf69f1f327 | 483 | } |
OscarLiao | 0:94cf69f1f327 | 484 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
OscarLiao | 0:94cf69f1f327 | 485 | |
OscarLiao | 0:94cf69f1f327 | 486 | |
OscarLiao | 0:94cf69f1f327 | 487 | |
OscarLiao | 0:94cf69f1f327 | 488 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓state_update funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
OscarLiao | 0:94cf69f1f327 | 489 | void state_update(void) //estimation of new attitude |
OscarLiao | 0:94cf69f1f327 | 490 | { |
OscarLiao | 0:94cf69f1f327 | 491 | //predict |
OscarLiao | 0:94cf69f1f327 | 492 | gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt; |
OscarLiao | 0:94cf69f1f327 | 493 | gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt; |
OscarLiao | 0:94cf69f1f327 | 494 | gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt; |
OscarLiao | 0:94cf69f1f327 | 495 | |
OscarLiao | 0:94cf69f1f327 | 496 | //update |
OscarLiao | 0:94cf69f1f327 | 497 | gDIR[0][0] = (1-Bet) * gDIR[0][0] + Bet * Ax; |
OscarLiao | 0:94cf69f1f327 | 498 | gDIR[0][1] = (1-Bet) * gDIR[0][1] + Bet * Ay; |
OscarLiao | 0:94cf69f1f327 | 499 | gDIR[0][2] = (1-Bet) * gDIR[0][2] + Bet * Az; |
OscarLiao | 0:94cf69f1f327 | 500 | |
OscarLiao | 0:94cf69f1f327 | 501 | //nutralizing |
OscarLiao | 0:94cf69f1f327 | 502 | gUnity = sqrt( gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2] ); |
OscarLiao | 0:94cf69f1f327 | 503 | for(int i=0; i<3; i++) { |
OscarLiao | 0:94cf69f1f327 | 504 | gDIR[0][i] = gDIR[0][i] / gUnity; |
OscarLiao | 0:94cf69f1f327 | 505 | } |
OscarLiao | 0:94cf69f1f327 | 506 | |
OscarLiao | 0:94cf69f1f327 | 507 | //transfer |
OscarLiao | 0:94cf69f1f327 | 508 | Ele_phy = asin(gDIR[0][0]); |
OscarLiao | 0:94cf69f1f327 | 509 | Til_phy = asin(-gDIR[0][1] / cos(Ele_phy)); |
OscarLiao | 0:94cf69f1f327 | 510 | } |
OscarLiao | 0:94cf69f1f327 | 511 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
OscarLiao | 0:94cf69f1f327 | 512 | |
OscarLiao | 0:94cf69f1f327 | 513 | |
OscarLiao | 0:94cf69f1f327 | 514 | //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// |
OscarLiao | 0:94cf69f1f327 | 515 | float lpf(float input, float output_old, float frequency) |
OscarLiao | 0:94cf69f1f327 | 516 | { |
OscarLiao | 0:94cf69f1f327 | 517 | float output = 0; |
OscarLiao | 0:94cf69f1f327 | 518 | output = (output_old + frequency*dt*input) / (1 + frequency*dt); |
OscarLiao | 0:94cf69f1f327 | 519 | return output; |
OscarLiao | 0:94cf69f1f327 | 520 | } |
OscarLiao | 0:94cf69f1f327 | 521 | //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |
OscarLiao | 0:94cf69f1f327 | 522 | |
OscarLiao | 0:94cf69f1f327 | 523 | |
OscarLiao | 0:94cf69f1f327 | 524 | //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Rx_irq funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// |
OscarLiao | 0:94cf69f1f327 | 525 | void Rx_irq(void) |
OscarLiao | 0:94cf69f1f327 | 526 | { |
OscarLiao | 0:94cf69f1f327 | 527 | SerBuff[0] = Debug.getc(); |
OscarLiao | 0:94cf69f1f327 | 528 | Debug.putc(SerBuff[0]); |
OscarLiao | 0:94cf69f1f327 | 529 | } |
OscarLiao | 0:94cf69f1f327 | 530 | //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Rx_irq funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// |