![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
trot
Revision 1:ddca0613a53e, committed 2018-12-21
- Comitter:
- OscarLiao
- Date:
- Fri Dec 21 09:31:08 2018 +0000
- Parent:
- 0:94cf69f1f327
- Commit message:
- trot
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Dec 13 14:27:35 2018 +0000 +++ b/main.cpp Fri Dec 21 09:31:08 2018 +0000 @@ -21,9 +21,14 @@ //Initial Position #define init_leg_Angle1 0 -#define init_leg_length 0.22f +#define init_leg_length 0.23f #define init_leg_Angle2 0 +//Trot gait +#define trot_step_height 0.02 //step height in meters +#define airborne_pct 30 //leg airborne time percentage 0~100 +#define ground_pct 20 //leg stay on ground time percentage 0~100 + #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// @@ -89,13 +94,13 @@ init_leg_Angle1, init_leg_length, init_leg_Angle2 }; float LH_q_0_E[3] = { - init_leg_Angle1, init_leg_length, init_leg_Angle2 + init_leg_Angle1, init_leg_length, init_leg_Angle2-0.349066 }; 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 + init_leg_Angle1, init_leg_length, init_leg_Angle2-0.349066 }; uint8_t LF_Cmd_Hex[3] = {0x00, 0x00, 0x00}; @@ -136,13 +141,39 @@ //╔═════════════════╗ //║ 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; +float pitchU[3] = {0, 0, 0}; +float pitchE[3] = {0, 0, 0}; +float rollU = 0; + +//╔═════════════════╗ +//║ Trot ║ +//╚═════════════════╝ +bool trotingFlag = false; + +float period = 0; +float move_pct = 0; +float liftTime = 0; +float dropTime = 0; +float airborneTime = 0; +float groundTime = 0; +float liftSpeed = 0; +float dropSpeed = 0; + +int liftCounter = 0; +int dropCounter = 0; +int airCounter = 0; +int groundCounter = 0; + +bool liftBool = false; +bool dropBool = false; +bool airborneBool = false; +bool groundBool = false; + +enum legs{LF, RF, RH, LH}; +bool trotDiagonal = false; +int trotLeg[2] = {LF, RH}; + +bool press = false; //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// @@ -161,6 +192,9 @@ void pressed(); void Balance(); +void trot(float freq); +void move_leg(int leg, int type, float speed); + void Rx_irq(); //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// @@ -201,18 +235,36 @@ //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(); +// //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; + if(press) + { + trot(0.5); + Balance(); + } + else + { + 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; + } + + + Debug.printf("%.3f, %.3f\r", LF_q_0_E[1], RF_q_0_E[1]); Flag_1 = 0; } //Send Command - if(Flag_4 == 1) { + if(Flag_4 == 1) { + + LF_q_0_E[1] = constrain(LF_q_0_E[1],0.1 ,0.3);//safty constrain + LH_q_0_E[1] = constrain(LH_q_0_E[1],0.1 ,0.3);//safty constrain + RF_q_0_E[1] = constrain(RF_q_0_E[1],0.1 ,0.3);//safty constrain + RH_q_0_E[1] = constrain(RH_q_0_E[1],0.1 ,0.3);//safty constrain 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; @@ -312,8 +364,7 @@ //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Button funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// void pressed(void) { - Ki = 0.005; - Kd = 0.001; + press = true; } void released(void) @@ -326,28 +377,151 @@ void Balance(void) { - Pu = Kp*Ele_phy; - Iu += Ki*Ele_phy; - Du = Kd*(Ele_phy-Ele_phy_old)/(dt*(Task_1_count+1)); + //update pitch controller + pitchE[0] = Ele_phy; + pitchU[0] = 0.2*(0.0663*pitchU[1]+ 0.9337*pitchU[2]- 0.13635*(pitchE[0]+0.3535*pitchE[1]-0.6465*pitchE[2])); + + pitchE[2] = pitchE[1]; + pitchE[1] = pitchE[0]; + pitchU[2] = pitchU[1]; + pitchU[1] = pitchU[0]; - 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); + rollU = 0.05*Til_phy; + +// LF_q_0_E[0] = Til_phy; +// RF_q_0_E[0] = -Til_phy; +// LH_q_0_E[0] = Til_phy; +// RH_q_0_E[0] = -Til_phy; + + LF_q_0_E[1] = LF_q_0_E[1] + pitchU[0] + rollU; + RF_q_0_E[1] = RF_q_0_E[1] + pitchU[0] - rollU; + LH_q_0_E[1] = LH_q_0_E[1] - pitchU[0] + rollU; + RH_q_0_E[1] = RH_q_0_E[1] - pitchU[0] - rollU; 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); + //Debug.printf("%.3f, %.3f\r", Ele_phy, pitchU[0]); } //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Balance funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// +//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Trot funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// + +void trot(float freq) +{ + if(!trotingFlag) + { + period = 1/freq; + move_pct = 100.0f - airborne_pct - ground_pct; + liftTime = period*((move_pct/2)/100.0f); + dropTime = liftTime; + airborneTime = period*(airborne_pct/100.0f); + groundTime = period*(ground_pct/100.0f); + liftSpeed = trot_step_height/liftTime; + dropSpeed = trot_step_height/dropTime; + + liftCounter = liftTime/(dt*(Task_1_NN+1)); + dropCounter = liftCounter; + airCounter = airborneTime/(dt*(Task_1_NN+1)); + groundCounter = groundTime/(dt*(Task_1_NN+1)); + + //switch legs + if(trotDiagonal) + { + trotLeg[0] = LF; + trotLeg[1] = RH; + } + else + { + trotLeg[0] = RF; + trotLeg[1] = LH; + } + + //start troting + liftBool = true; + trotingFlag = true; + } + //Debug.printf("%d, %d, %d, %d\r", liftCounter, airCounter, dropCounter, groundCounter); + //lift leg + if(liftBool) + { + move_leg(trotLeg[0], 1, -liftSpeed); + move_leg(trotLeg[1], 1, -liftSpeed); + liftCounter--; + if(liftCounter == 0) + { + liftBool = false; + airborneBool = true; + } + } + + //stay airborne + if(airborneBool) + { + airCounter--; + if(airCounter == 0) + { + airborneBool = false; + dropBool = true; + } + } + + //drop leg + if(dropBool) + { + move_leg(trotLeg[0], 1, dropSpeed); + move_leg(trotLeg[1], 1, dropSpeed); + dropCounter--; + if(dropCounter == 0) + { + dropBool = false; + groundBool = true; + } + } + + //stay on ground + if(groundBool) + { + groundCounter--; + if(groundCounter == 0) + { + groundBool = false; + trotingFlag = false; + trotDiagonal = !trotDiagonal; //switch legs + } + } +} + +//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Trot funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// + +//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Move leg funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// + +void move_leg(int leg, int type, float speed) +{ + switch(leg) + { + case LF: + LF_q_0_E[type] += speed*(dt*(Task_1_NN+1)); + break; + case RF: + RF_q_0_E[type] += speed*(dt*(Task_1_NN+1)); + break; + case LH: + LH_q_0_E[type] += speed*(dt*(Task_1_NN+1)); + break; + case RH: + RH_q_0_E[type] += speed*(dt*(Task_1_NN+1)); + break; + } + + //Debug.printf("%.3f, %.3f\r", Ele_phy, u[0]); +} + +//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Move leg funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// + //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// void init_IMU(void) //initialize