Oscar Liao
/
CHICAGO_balance_test_PID_trot
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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