Oscar Liao
DEMO
Diff: main.cpp
- Revision:
- 2:385c9c8b1b5b
- Parent:
- 1:15dd461fbc2a
--- a/main.cpp Fri Dec 21 06:27:26 2018 +0000
+++ b/main.cpp Thu Dec 27 15:50:06 2018 +0000
@@ -13,7 +13,7 @@
#define Rms 5000 //TT rate
#define dt 0.005f
#define Task_1_NN 9
-#define Task_2_NN 5
+#define Task_2_NN 99
#define Task_4_NN 9
//Structure
@@ -98,10 +98,16 @@
init_leg_Angle1, init_leg_length, init_leg_Angle2
};
+float K_c_1 = 2000;
+float K_c_2 = 8;
+float B_c = 0;
+
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};
+uint8_t K_Cmd_Hex[3] = {0x00, 0x00, 0x00};
+uint8_t B_Cmd_Hex[3] = {0x00, 0x00, 0x00};
//╔═════════════════╗
//║ IMU_SPI ║
@@ -140,6 +146,13 @@
float pitchE[3] = {0, 0, 0};
float rollU = 0;
+//╔═════════════════╗
+//║ DEMO ║
+//╚═════════════════╝
+int mode_number = 7;
+int demo_mode = 0;
+bool press = false;
+
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
@@ -155,8 +168,10 @@
float lpf(float input, float output_old, float frequency); //lpf discrete
void pressed();
+void released();
void Balance();
+
void Rx_irq();
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
@@ -182,7 +197,7 @@
wait_ms(200);
button.fall(&pressed); //button pressed
-
+
while(1) { //main() loop
// if(Debug.readable()) {
@@ -194,8 +209,9 @@
// if(device.readable()) {
// pc.putc(device.getc());
// }
+
//Balance
- if(Flag_1 == 1) {
+ if((Flag_1 == 1) && (demo_mode == 0)) {
//Initial Position
LF_q_0_E[1] = init_leg_length;
@@ -206,10 +222,60 @@
Balance();
Flag_1 = 0;
}
+ //Debug.printf("%d\r", demo_mode);
+ //SEA DEMO
+ if((Flag_2 == 1) && (demo_mode != 0)) {
+
+ //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;
+
+ K_Cmd_Hex[0] = 0;
+ K_Cmd_Hex[1] = (K_c_1/4000)*255;
+ K_Cmd_Hex[2] = K_c_2;
+
+ B_Cmd_Hex[0] = 0;
+ B_Cmd_Hex[1] = ((B_c+1)/6)*255;
+ B_Cmd_Hex[2] = ((B_c+1)/6)*255;
+
+ LD2 = 1;
+
+ //Left Front Leg Spring
+ LF_CS = 0;
+ wait_us(20);
+ LF_Cmd.putc(K_Cmd_Hex[0]);
+ LF_Cmd.putc(K_Cmd_Hex[1]);
+ LF_Cmd.putc(K_Cmd_Hex[2]);
+ LF_Cmd.putc('K');
+ wait_us(180);
+ LF_CS = 1;
+
+ wait_us(1000);
+
+ //Left Front Leg Damping
+ LF_CS = 0;
+ wait_us(20);
+ LF_Cmd.putc(B_Cmd_Hex[0]);
+ LF_Cmd.putc(B_Cmd_Hex[1]);
+ LF_Cmd.putc(B_Cmd_Hex[2]);
+ LF_Cmd.putc('B');
+ wait_us(180);
+ LF_CS = 1;
+
+ LD2 = 0;
+ Flag_2 = 0;
+ }
//Send Command
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;
LF_Cmd_Hex[2] = (LF_q_0_E[2]+pi/4)/(pi/2)*255;
@@ -297,6 +363,12 @@
Flag_1 = 1;
}
+ Task_2_count = Task_2_count + 1;
+ if(Task_2_count > Task_2_NN) {
+ Task_2_count = 0; //Task triggering
+ Flag_2 = 1;
+ }
+
Task_4_count = Task_4_count + 1;
if(Task_4_count > Task_4_NN) {
Task_4_count = 0; //Task triggering
@@ -308,7 +380,87 @@
//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Button funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void pressed(void)
{
-
+ button.fall(NULL);
+ demo_mode++;
+ demo_mode %= mode_number;
+ switch(demo_mode)
+ {
+ case 0:
+ K_c_1 = 2000;
+ K_c_2 = 8;
+ B_c = 0;
+
+ K_Cmd_Hex[0] = 0;
+ K_Cmd_Hex[1] = (K_c_1/4000)*255;
+ K_Cmd_Hex[2] = K_c_2;
+
+ B_Cmd_Hex[0] = 0;
+ B_Cmd_Hex[1] = ((B_c+1)/6)*255;
+ B_Cmd_Hex[2] = ((B_c+1)/6)*255;
+
+ for(int i= 0; i<5; i++)
+ {
+ LD2 = 1;
+
+ //Left Front Leg Spring
+ LF_CS = 0;
+ wait_us(20);
+ LF_Cmd.putc(K_Cmd_Hex[0]);
+ LF_Cmd.putc(K_Cmd_Hex[1]);
+ LF_Cmd.putc(K_Cmd_Hex[2]);
+ LF_Cmd.putc('K');
+ wait_us(180);
+ LF_CS = 1;
+
+ wait_us(1000);
+
+ //Left Front Leg Damping
+ LF_CS = 0;
+ wait_us(20);
+ LF_Cmd.putc(B_Cmd_Hex[0]);
+ LF_Cmd.putc(B_Cmd_Hex[1]);
+ LF_Cmd.putc(B_Cmd_Hex[2]);
+ LF_Cmd.putc('B');
+ wait_us(180);
+ LF_CS = 1;
+
+ LD2 = 0;
+ wait(0.5);
+ }
+ break;
+ case 1:
+ K_c_1 = 2000;
+ K_c_2 = 8;
+ B_c = 0;
+ break;
+ case 2:
+ K_c_1 = 500;
+ K_c_2 = 8;
+ B_c = 0;
+ break;
+ case 3:
+ K_c_1 = 500;
+ K_c_2 = 8;
+ B_c = 3;
+ break;
+ case 4:
+ K_c_1 = 0;
+ K_c_2 = 8;
+ B_c = 0;
+ break;
+ case 5:
+ K_c_1 = 0;
+ K_c_2 = 8;
+ B_c = 3;
+ break;
+ case 6:
+ K_c_1 = 0;
+ K_c_2 = 0;
+ B_c = -1;
+ break;
+ }
+ wait(1);
+ button.fall(&pressed);
}
void released(void)
@@ -323,14 +475,14 @@
{
//update pitch controller
pitchE[0] = Ele_phy;
- pitchU[0] = 0.5*(0.0663*pitchU[1]+ 0.9337*pitchU[2]- 0.13635*(pitchE[0]+0.3535*pitchE[1]-0.6465*pitchE[2]));
+ pitchU[0] = 1*(0.0663*pitchU[1]+ 0.9337*pitchU[2]- 0.3*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];
- rollU = 0.05*Til_phy;
+// rollU = 0.05*Til_phy;
// LF_q_0_E[0] = Til_phy;
// RF_q_0_E[0] = -Til_phy;
@@ -347,7 +499,7 @@
LH_q_0_E[2] = - Ele_phy;
RH_q_0_E[2] = - Ele_phy;
- Debug.printf("%.3f, %.3f\r", Til_phy, rollU);
+ //Debug.printf("%.3f, %.3f\r", Til_phy, rollU);
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Balance funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//