Oscar Liao / Mbed 2 deprecated CHICAGO_balance_test_PID_copy

balance

Dependencies:   mbed

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?

UserRevisionLine numberNew 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■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//