Oscar Liao
/
CHICAGO_balance_DEMO
DEMO
main.cpp
- Committer:
- OscarLiao
- Date:
- 2018-12-13
- Revision:
- 0:94cf69f1f327
- Child:
- 1:15dd461fbc2a
File content as of revision 0:94cf69f1f327:
/* This is a program for globlelized controller of whole DOGO F746ZG */ #include "mbed.h" #include "LSM9DS0_SH.h" #define DEBUG 0 #define pi 3.141592f #define d2r 0.01745329f #define Rms 5000 //TT rate #define dt 0.005f #define Task_1_NN 9 #define Task_2_NN 5 #define Task_4_NN 9 //Structure #define Buff_size 16 //Serial Buffer //Initial Position #define init_leg_Angle1 0 #define init_leg_length 0.22f #define init_leg_Angle2 0 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// //╔═════════════════╗ //║ Structure ║ //╚═════════════════╝ DigitalOut LD1(PB_0); //detection DigitalOut LD2(PB_7); //detection DigitalOut LD3(PB_14); //detection InterruptIn button(USER_BUTTON); //Button press; //╔═════════════════╗ //║ Serial ║ //╚═════════════════╝ Serial Debug(PD_8, PD_9); //Serial_3 reg(TX RX) USB port Serial LF_Cmd(PD_5, PD_6); //Serial_2 DigitalOut LF_CS(PD_7); Serial LH_Cmd(PC_12, PD_2); //Serial_5 DigitalOut LH_CS(PG_2); Serial RF_Cmd(PG_14, PC_7); //Serial_6 DigitalOut RF_CS(PG_9); Serial RH_Cmd(PE_8, PE_7); //Serial_7 DigitalOut RH_CS(PE_10); //╔═════════════════╗ //║ IMU_SPI ║ //╚═════════════════╝ DigitalOut SPI_CSG(PD_14,1); //low for GYRO enable DigitalOut SPI_CSXM(PD_15,1); //low for ACC/MAG enable SPI spi(PA_7, PA_6, PA_5); //MOSI MISO SCLK //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of GPIO registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Varible registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// //╔═════════════════╗ //║ Structure ║ //╚═════════════════╝ Ticker TT; //call a timer uint8_t Task_1_count = 0; //1st prior task count uint8_t Task_2_count = 0; //2nd prior task count uint8_t Task_3_count = 0; //3nd prior task count uint8_t Task_4_count = 0; //24nd prior task count uint8_t Flag_1 = 0; //1st prior task flag uint8_t Flag_2 = 0; //2nd prior task flag uint8_t Flag_3 = 0; //3nd prior task flag uint8_t Flag_4 = 0; //4nd prior task flag //╔═════════════════╗ //║ I/O Serial ║ //╚═════════════════╝ uint8_t SerBuff[Buff_size]; uint8_t Recieve_index = 0; //╔═════════════════╗ //║ Leg Command ║ //╚═════════════════╝ float LF_q_0_E[3] = { init_leg_Angle1, init_leg_length, init_leg_Angle2 }; float LH_q_0_E[3] = { init_leg_Angle1, init_leg_length, init_leg_Angle2 }; float RF_q_0_E[3] = { init_leg_Angle1, init_leg_length, init_leg_Angle2 }; float RH_q_0_E[3] = { init_leg_Angle1, init_leg_length, init_leg_Angle2 }; uint8_t LF_Cmd_Hex[3] = {0x00, 0x00, 0x00}; uint8_t LH_Cmd_Hex[3] = {0x00, 0x00, 0x00}; uint8_t RF_Cmd_Hex[3] = {0x00, 0x00, 0x00}; uint8_t RH_Cmd_Hex[3] = {0x00, 0x00, 0x00}; //╔═════════════════╗ //║ IMU_SPI ║ //╚═════════════════╝ short low_byte = 0x00; //buffer short high_byte = 0x00; short Buff = 0x00; float Wx = 0.0; float Wy = 0.0; float Wz = 0.0; float Ax = 0.0; float Ay = 0.0; float Az = 0.0; float gDIR[1][3] = { //g vector's direction , required unitconstrain {0.0,0.0,0.0}, //X Y Z }; float Bet = 0.002; //confidence of Acc data float gUnity = 0.0; float Gdx = 0.0; float Gdy = 0.0; float Gdz = 0.0; float Adx = 0.0; float Ady = 0.0; float Adz = 0.0; float Ele_phy = 0.0; //estimation of top plate attitide float Til_phy = 0.0; float Ele_phy_int = 0.0; float Til_phy_int = 0.0; float Ele_control = 0.0; float Til_control = 0.0; //╔═════════════════╗ //║ Balance ║ //╚═════════════════╝ float Kp = 0.04; float Ki = 0.0; float Kd = 0.0; float Pu = 0.0; float Iu = 0.0; float Du = 0.0; float Ele_phy_old = 0.0; //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Function registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// void init_TIMER(); //set TT_main() rate void TT_main(); //timebase function rated by TT void init_IMU(); //initialize IMU void init_GXdrift(); //read GXdrift at start up void read_IMU(); //read IMU data give raw data void state_update(); //estimation of new attitude float lpf(float input, float output_old, float frequency); //lpf discrete void pressed(); void Balance(); void Rx_irq(); //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡main funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// int main() { init_IMU(); //initialize IMU init_GXdrift(); //read GXdrift at start up Debug.baud(115200); //set baud rate LF_Cmd.baud(115200); LH_Cmd.baud(115200); RF_Cmd.baud(115200); RH_Cmd.baud(115200); wait_ms(10); init_TIMER(); //start TT_main Debug.attach(&Rx_irq,Serial::RxIrq); //Start recieving message LF_CS = 1; wait_ms(200); button.fall(&pressed); //button pressed while(1) { //main() loop // if(Debug.readable()) { // SerBuff[Recieve_index] = Debug.getc(); // Debug.putc(SerBuff[Recieve_index]); // Recieve_index = Recieve_index + 1; // } // if(device.readable()) { // pc.putc(device.getc()); // } //Balance if(Flag_1 == 1) { //Initial Position LF_q_0_E[1] = init_leg_length; LH_q_0_E[1] = init_leg_length; RF_q_0_E[1] = init_leg_length; RH_q_0_E[1] = init_leg_length; Balance(); Flag_1 = 0; } //Send Command if(Flag_4 == 1) { LF_Cmd_Hex[0] = (LF_q_0_E[0]+pi/4)/(pi/2)*255; LF_Cmd_Hex[1] = ((LF_q_0_E[1]-0.1)/0.2)*255; LF_Cmd_Hex[2] = (LF_q_0_E[2]+pi/4)/(pi/2)*255; LH_Cmd_Hex[0] = (LH_q_0_E[0]+pi/4)/(pi/2)*255; LH_Cmd_Hex[1] = ((LH_q_0_E[1]-0.1)/0.2)*255; LH_Cmd_Hex[2] = (LH_q_0_E[2]+pi/4)/(pi/2)*255; RF_Cmd_Hex[0] = (RF_q_0_E[0]+pi/4)/(pi/2)*255; RF_Cmd_Hex[1] = ((RF_q_0_E[1]-0.1)/0.2)*255; RF_Cmd_Hex[2] = (RF_q_0_E[2]+pi/4)/(pi/2)*255; RH_Cmd_Hex[0] = (RH_q_0_E[0]+pi/4)/(pi/2)*255; RH_Cmd_Hex[1] = ((RH_q_0_E[1]-0.1)/0.2)*255; RH_Cmd_Hex[2] = (RH_q_0_E[2]+pi/4)/(pi/2)*255; //Debug.printf("%d, %d, %d\r", LH_Cmd_Hex[0], LH_Cmd_Hex[1], LH_Cmd_Hex[2]); //LED LD1 = 1; //Left Front Leg LF_CS = 0; wait_us(20); LF_Cmd.putc(LF_Cmd_Hex[0]); LF_Cmd.putc(LF_Cmd_Hex[1]); LF_Cmd.putc(LF_Cmd_Hex[2]); LF_Cmd.putc('Q'); wait_us(180); LF_CS = 1; //Left Hind Leg LH_CS = 0; wait_us(20); LH_Cmd.putc(LH_Cmd_Hex[0]); LH_Cmd.putc(LH_Cmd_Hex[1]); LH_Cmd.putc(LH_Cmd_Hex[2]); LH_Cmd.putc('Q'); wait_us(180); LH_CS = 1; //Right Front Leg RF_CS = 0; wait_us(20); RF_Cmd.putc(RF_Cmd_Hex[0]); RF_Cmd.putc(RF_Cmd_Hex[1]); RF_Cmd.putc(RF_Cmd_Hex[2]); RF_Cmd.putc('Q'); wait_us(180); RF_CS = 1; //Right Hind Leg RH_CS = 0; wait_us(20); RH_Cmd.putc(RH_Cmd_Hex[0]); RH_Cmd.putc(RH_Cmd_Hex[1]); RH_Cmd.putc(RH_Cmd_Hex[2]); RH_Cmd.putc('Q'); wait_us(180); RH_CS = 1; LD1 = 0; Flag_4 = 0; } } } //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of main funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Timebase funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// void init_TIMER() //set TT_main{} rate { TT.attach_us(&TT_main, Rms); } void TT_main() //interrupt function by TT { read_IMU(); //read IMU data give raw data state_update(); //estimation of new attitude Task_1_count = Task_1_count + 1; if(Task_1_count > Task_1_NN) { Task_1_count = 0; //Task triggering Flag_1 = 1; } Task_4_count = Task_4_count + 1; if(Task_4_count > Task_4_NN) { Task_4_count = 0; //Task triggering Flag_4 = 1; } } //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Timebase funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Button funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// void pressed(void) { Ki = 0.005; Kd = 0.001; } void released(void) { } //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of button funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Balance funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// void Balance(void) { Pu = Kp*Ele_phy; Iu += Ki*Ele_phy; Du = Kd*(Ele_phy-Ele_phy_old)/(dt*(Task_1_count+1)); LF_q_0_E[1] = LF_q_0_E[1] - (Pu+Iu+Du); RF_q_0_E[1] = RF_q_0_E[1] - (Pu+Iu+Du); LH_q_0_E[1] = LH_q_0_E[1] + (Pu+Iu+Du); RH_q_0_E[1] = RH_q_0_E[1] + (Pu+Iu+Du); LF_q_0_E[2] = - Ele_phy; RF_q_0_E[2] = - Ele_phy; LH_q_0_E[2] = - Ele_phy; RH_q_0_E[2] = - Ele_phy; //Update Ele_phy_old = Ele_phy; Debug.printf("%.3f, %.3f, %.3f\r", Pu, Iu, Du); } //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Balance funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// void init_IMU(void) //initialize { //gloable config SPI_CSXM = 1; //high as init for disable SPI SPI_CSG = 1; spi.format(8, 3); //byte width, spi mode spi.frequency(4000000); //8MHz //for GYRO config SPI_CSG = 0; //start spi talking spi.write(CTRL_REG1_G); spi.write(0x9F); //data rate 380 Hz/ cut off 25 Hz SPI_CSG = 1; //end spi talking SPI_CSG = 0; //start spi talking spi.write(CTRL_REG4_G); spi.write(0x10); //Scle 500dps SPI_CSG = 1; //end spi talking //for ACC config SPI_CSXM = 0; //start spi talking spi.write(CTRL_REG1_XM); spi.write(0x87); //data rate 400 Hz/ Enable SPI_CSXM = 1; //end spi talking SPI_CSXM = 0; //start spi talking spi.write(CTRL_REG2_XM); spi.write(0xC8); //cut off 50 Hz/ Scale +-4g SPI_CSXM = 1; //end spi talking } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_G/Xdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// void init_GXdrift(void) //initialize { for(int i=0; i<1000; i++) { read_IMU(); //note GXdrift = 0 at this moment gDIR[0][0] = gDIR[0][0] - Wx; gDIR[0][1] = gDIR[0][1] - Wy; gDIR[0][2] = gDIR[0][2] - Wz; wait_ms(2); } Gdx = gDIR[0][0] /1000.0f; Gdy = gDIR[0][1] /1000.0f; Gdz = gDIR[0][2] /1000.0f; //Debug.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz); for(int i=0; i<1000; i++) { read_IMU(); //note GXdrift = 0 at this moment gDIR[0][0] = gDIR[0][0] - Ax; gDIR[0][1] = gDIR[0][1] - Ay; gDIR[0][2] = gDIR[0][2] - Az; wait_ms(2); } Adx = gDIR[0][0] /1000.0f; Ady = gDIR[0][1] /1000.0f; Adz = gDIR[0][2] /1000.0f - 1.0f; // pc.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz); gDIR[0][0] = 0; gDIR[0][1] = 0; gDIR[0][2] = -1; } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_GXdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// void read_IMU(void) //read IMU data give raw data { //Wx SPI_CSG = 0; //start spi talking Wx spi.write(0xE8); //read B11101000 read/multi/address low_byte = spi.write(0); high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSG = 1; //end spi talking // Wx = Buff * Gpx + Gdx; Wx = lpf(Buff * Gpx + Gdx, Wx, 48.0f); //Wy SPI_CSG = 0; //start spi talking Wx spi.write(0xEA); //read B11101010 read/multi/address low_byte = spi.write(0); high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSG = 1; //end spi talking // Wy = Buff * Gpy + Gdy; Wy = lpf(Buff * Gpy + Gdy, Wy, 48.0f); //Wz SPI_CSG = 0; //start spi talking Wx spi.write(0xEC); //read B11101100 read/multi/address low_byte = spi.write(0); high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSG = 1; //end spi talking // Wz = Buff * Gpz + Gdz; Wz = lpf(Buff * Gpz + Gdz, Wz, 48.0f); //Ax SPI_CSXM = 0; //start spi talking Ax spi.write(0xE8); //read B11101000 read/multi/address low_byte = spi.write(0); high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSXM = 1; //end spi talking Ax = lpf(Buff*Apx + Adx, Ax, 13.0f); //Ay SPI_CSXM = 0; //start spi talking Ax spi.write(0xEA); //read B11101010 read/multi/address low_byte = spi.write(0); high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSXM = 1; //end spi talking Ay = lpf(Buff*Apy + Ady, Ay, 13.0f); //Az SPI_CSXM = 0; //start spi talking Ax spi.write(0xEC); //read B11101100 read/multi/address low_byte = spi.write(0); high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSXM = 1; //end spi talking Az = lpf(Buff*Apz + Adz, Az, 13.0f); //Debug.printf("%.4f,%.4f,%.4f\r", Ax, Ay, Az); } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓state_update funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// void state_update(void) //estimation of new attitude { //predict gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt; gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt; gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt; //update gDIR[0][0] = (1-Bet) * gDIR[0][0] + Bet * Ax; gDIR[0][1] = (1-Bet) * gDIR[0][1] + Bet * Ay; gDIR[0][2] = (1-Bet) * gDIR[0][2] + Bet * Az; //nutralizing gUnity = sqrt( gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2] ); for(int i=0; i<3; i++) { gDIR[0][i] = gDIR[0][i] / gUnity; } //transfer Ele_phy = asin(gDIR[0][0]); Til_phy = asin(-gDIR[0][1] / cos(Ele_phy)); } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// float lpf(float input, float output_old, float frequency) { float output = 0; output = (output_old + frequency*dt*input) / (1 + frequency*dt); return output; } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Rx_irq funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// void Rx_irq(void) { SerBuff[0] = Debug.getc(); Debug.putc(SerBuff[0]); } //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Rx_irq funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//