Skip to content

Commit

Permalink
improve icm trot stability
Browse files Browse the repository at this point in the history
  • Loading branch information
@borntoleave
borntoleave committed Jan 14, 2025
1 parent f3e31c5 commit f2eff12
Show file tree
Hide file tree
Showing 5 changed files with 111 additions and 98 deletions.
188 changes: 100 additions & 88 deletions src/InstinctBittleESP.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1095,94 +1095,106 @@ const int8_t trArmL[] PROGMEM = {
32, 33, 50, 52, 7, 4, 10, 4,
};
const int8_t trF[] PROGMEM = {
42, 0, 0, 1,
41, 47, 52, 59, 9, 0, 11, 3,
43, 42, 54, 55, 11, 0, 15, 1,
46, 35, 55, 49, 11, 2, 16, 0,
48, 28, 56, 44, 12, 5, 19, 0,
51, 21, 57, 38, 13, 9, 22, 1,
52, 13, 58, 31, 14, 14, 26, 3,
54, 11, 58, 30, 16, 16, 29, 3,
56, 9, 59, 27, 18, 18, 33, 7,
57, 9, 58, 26, 21, 20, 39, 8,
57, 8, 55, 26, 26, 22, 47, 10,
57, 9, 54, 26, 28, 22, 51, 11,
57, 9, 54, 27, 29, 24, 52, 12,
58, 11, 56, 28, 28, 22, 51, 12,
61, 12, 59, 30, 24, 21, 45, 11,
62, 17, 61, 34, 22, 18, 43, 10,
64, 21, 63, 37, 19, 15, 39, 9,
65, 25, 64, 40, 16, 13, 36, 9,
63, 28, 65, 43, 11, 12, 26, 9,
60, 32, 65, 46, 7, 10, 18, 9,
57, 36, 64, 49, 3, 9, 13, 10,
52, 39, 61, 51, 2, 9, 8, 10,
47, 41, 59, 52, 0, 9, 3, 14,
42, 44, 55, 54, 0, 11, 1, 16,
35, 47, 49, 55, 2, 11, 0, 17,
28, 49, 44, 57, 5, 12, 0, 20,
21, 52, 38, 58, 9, 13, 1, 23,
13, 53, 31, 58, 14, 15, 3, 27,
11, 55, 30, 58, 16, 16, 3, 30,
9, 56, 27, 58, 18, 19, 7, 35,
9, 57, 26, 58, 20, 22, 8, 40,
8, 57, 26, 55, 22, 27, 10, 49,
9, 57, 26, 54, 22, 28, 11, 51,
9, 58, 27, 56, 24, 27, 12, 49,
11, 60, 28, 57, 22, 26, 12, 48,
12, 61, 30, 59, 21, 24, 11, 45,
17, 62, 34, 61, 18, 22, 10, 43,
21, 64, 37, 63, 15, 19, 9, 39,
25, 64, 40, 65, 13, 15, 9, 32,
28, 62, 43, 65, 12, 10, 9, 24,
32, 59, 46, 65, 10, 6, 9, 17,
36, 55, 49, 63, 9, 3, 10, 12,
39, 50, 51, 60, 9, 1, 10, 7,
48, 0, 0, 1,
29, 34, 53, 59, 13, 3, 15, 6,
31, 30, 54, 57, 12, 3, 16, 2,
34, 24, 55, 54, 12, 6, 17, 0,
36, 19, 56, 49, 11, 10, 19, 0,
39, 13, 57, 45, 11, 14, 21, 0,
41, 8, 58, 41, 10, 18, 24, 0,
43, 2, 58, 36, 11, 23, 26, 1,
45, 0, 58, 35, 11, 25, 29, 2,
47, -2, 58, 34, 11, 29, 32, 3,
49, -2, 58, 33, 12, 30, 36, 6,
49, -3, 56, 32, 15, 32, 43, 7,
50, -2, 56, 32, 15, 33, 45, 9,
51, -3, 55, 32, 16, 36, 47, 10,
52, -2, 57, 33, 14, 34, 44, 10,
54, -1, 59, 34, 13, 33, 43, 10,
55, 2, 60, 36, 11, 30, 41, 9,
57, 6, 62, 38, 9, 27, 39, 9,
57, 9, 63, 41, 7, 24, 36, 9,
57, 12, 64, 43, 7, 21, 34, 9,
55, 15, 65, 45, 3, 19, 26, 9,
52, 18, 66, 47, 2, 17, 20, 9,
48, 21, 65, 49, 2, 15, 15, 10,
44, 24, 63, 51, 1, 14, 12, 10,
39, 27, 61, 52, 2, 12, 8, 11,
34, 29, 59, 53, 3, 13, 6, 15,
30, 31, 57, 54, 3, 12, 2, 16,
24, 34, 54, 55, 6, 12, 0, 17,
19, 36, 49, 56, 10, 11, 0, 19,
13, 39, 45, 57, 14, 11, 0, 21,
8, 41, 41, 58, 18, 10, 0, 24,
2, 43, 36, 58, 23, 11, 1, 26,
0, 45, 35, 58, 25, 11, 2, 29,
-2, 47, 34, 58, 29, 11, 3, 32,
-2, 49, 33, 58, 30, 12, 6, 36,
-3, 49, 32, 56, 32, 15, 7, 43,
-2, 50, 32, 56, 33, 15, 9, 45,
-3, 51, 32, 55, 36, 16, 10, 47,
-2, 52, 33, 57, 34, 14, 10, 44,
-1, 54, 34, 59, 33, 13, 10, 43,
2, 55, 36, 60, 30, 11, 9, 41,
6, 57, 38, 62, 27, 9, 9, 39,
9, 57, 41, 63, 24, 7, 9, 36,
12, 57, 43, 64, 21, 7, 9, 34,
15, 55, 45, 65, 19, 3, 9, 26,
18, 52, 47, 66, 17, 2, 9, 20,
21, 48, 49, 65, 15, 2, 10, 15,
24, 44, 51, 63, 14, 1, 10, 12,
27, 39, 52, 61, 12, 2, 11, 8,
};
const int8_t trL[] PROGMEM = {
42, 0, 0, 1,
48, 47, 52, 54, 5, 0, 11, 5,
49, 42, 54, 52, 6, 0, 15, 4,
50, 35, 55, 50, 6, 2, 16, 4,
51, 28, 56, 48, 6, 5, 19, 4,
52, 21, 57, 47, 6, 9, 22, 3,
53, 13, 58, 45, 7, 14, 26, 3,
54, 11, 58, 44, 7, 16, 29, 3,
54, 9, 59, 43, 8, 18, 33, 3,
56, 9, 58, 43, 8, 20, 39, 3,
55, 8, 55, 43, 10, 22, 47, 5,
55, 9, 54, 44, 10, 22, 51, 5,
55, 9, 54, 44, 10, 24, 52, 5,
55, 11, 56, 44, 11, 22, 51, 5,
57, 12, 59, 44, 9, 21, 45, 5,
57, 17, 61, 45, 9, 18, 43, 5,
57, 21, 63, 46, 9, 15, 39, 5,
57, 25, 64, 47, 9, 13, 36, 5,
56, 28, 65, 48, 6, 12, 26, 5,
55, 32, 65, 49, 5, 10, 18, 6,
54, 36, 64, 49, 5, 9, 13, 6,
52, 39, 61, 50, 4, 9, 8, 6,
51, 41, 59, 52, 3, 9, 3, 6,
49, 44, 55, 52, 3, 11, 1, 7,
47, 47, 49, 53, 3, 11, 0, 7,
45, 49, 44, 54, 3, 12, 0, 7,
43, 52, 38, 55, 3, 13, 1, 8,
42, 53, 31, 56, 3, 15, 3, 8,
41, 55, 30, 56, 3, 16, 3, 9,
40, 56, 27, 57, 3, 19, 7, 10,
40, 57, 26, 58, 3, 22, 8, 10,
39, 57, 26, 57, 6, 27, 10, 12,
39, 57, 26, 57, 6, 28, 11, 12,
40, 58, 27, 57, 5, 27, 12, 13,
40, 60, 28, 58, 5, 26, 12, 11,
40, 61, 30, 58, 5, 24, 11, 11,
42, 62, 34, 58, 5, 22, 10, 11,
43, 64, 37, 58, 5, 19, 9, 11,
44, 64, 40, 59, 5, 15, 9, 9,
45, 62, 43, 58, 5, 10, 9, 8,
46, 59, 46, 57, 5, 6, 9, 7,
47, 55, 49, 56, 5, 3, 10, 6,
47, 50, 51, 54, 5, 1, 10, 5,
48, 0, 0, 1,
35, 34, 53, 55, 8, 3, 15, 5,
36, 30, 54, 54, 8, 3, 16, 5,
37, 24, 55, 52, 8, 6, 17, 5,
38, 19, 56, 51, 7, 10, 19, 4,
39, 13, 57, 51, 7, 14, 21, 3,
40, 8, 58, 49, 7, 18, 24, 3,
41, 2, 58, 48, 7, 23, 26, 3,
42, 0, 58, 48, 7, 25, 29, 3,
42, -2, 58, 47, 7, 29, 32, 3,
43, -2, 58, 47, 7, 30, 36, 3,
44, -3, 56, 46, 7, 32, 43, 5,
44, -2, 56, 46, 7, 33, 45, 5,
44, -3, 55, 47, 7, 36, 47, 5,
45, -2, 57, 46, 7, 34, 44, 7,
46, -1, 59, 46, 5, 33, 43, 7,
46, 2, 60, 46, 5, 30, 41, 7,
46, 6, 62, 47, 5, 27, 39, 7,
46, 9, 63, 47, 5, 24, 36, 7,
46, 12, 64, 48, 5, 21, 34, 7,
46, 15, 65, 48, 3, 19, 26, 8,
44, 18, 66, 49, 3, 17, 20, 8,
42, 21, 65, 50, 3, 15, 15, 8,
42, 24, 63, 51, 3, 14, 12, 8,
40, 27, 61, 51, 3, 12, 8, 8,
38, 29, 59, 52, 4, 13, 6, 9,
37, 31, 57, 52, 4, 12, 2, 9,
35, 34, 54, 53, 5, 12, 0, 9,
33, 36, 49, 54, 6, 11, 0, 10,
32, 39, 45, 54, 6, 11, 0, 10,
30, 41, 41, 55, 6, 10, 0, 10,
28, 43, 36, 55, 8, 11, 1, 11,
28, 45, 35, 56, 8, 11, 2, 11,
27, 47, 34, 56, 8, 11, 3, 12,
27, 49, 33, 57, 8, 12, 6, 12,
26, 49, 32, 57, 10, 15, 7, 13,
26, 50, 32, 57, 10, 15, 9, 13,
28, 51, 32, 57, 9, 16, 10, 13,
27, 52, 33, 58, 11, 14, 10, 14,
27, 54, 34, 59, 11, 13, 10, 12,
27, 55, 36, 59, 11, 11, 9, 12,
28, 57, 38, 59, 10, 9, 9, 12,
29, 57, 41, 59, 10, 7, 9, 12,
30, 57, 43, 59, 10, 7, 9, 12,
31, 55, 45, 60, 9, 3, 9, 9,
32, 52, 47, 59, 9, 2, 9, 8,
33, 48, 49, 58, 9, 2, 10, 8,
34, 44, 51, 57, 8, 1, 10, 7,
35, 39, 52, 56, 8, 2, 11, 6,
};
const int8_t vtArmF[] PROGMEM = {
24, 0, 0, 1,
Expand Down Expand Up @@ -2139,7 +2151,7 @@ const int8_t rc[] PROGMEM = {
0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, -88, -43, 67, 100, 42, -35, 42, 42, 32, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, -83, -88, 100, 60, 42, 42, 42, 50, 32, 0, 0, 0,
-8, -20, -11, 0, -1, -1, 0, 0, 18, 18, 18, 18, -14, -14, -14, -14, 8, 0, 0, 0,
-8, -20, -11, 0, -1, -1, 0, 0, 18, 18, 18, 18, -14, -14, -14, -14, 8,10, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 15, 15, 15, 15, 15, 15, 15, 15, 8, 0, 0, 0,
};
const int8_t rl[] PROGMEM = {
Expand Down Expand Up @@ -2241,6 +2253,6 @@ const int8_t zz[] PROGMEM = {
//while the newbilities on progmem are assigned to new addresses
const int8_t* progmemPointer[] = {zero, };
#endif
//the total byte of instincts is 20292
//the total byte of instincts is 20388
//the maximal array size is 933 bytes of wkF.
//Make sure to leave enough memory for SRAM to work properly. Any single skill should be smaller than 400 bytes for safety.
4 changes: 2 additions & 2 deletions src/OpenCat.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@
#else
#define BOARD "B"
#endif
#define DATE "250113" // YYMMDD
#define DATE "250114" // YYMMDD
String SoftwareVersion = "";

#define BIRTHMARK '@' // Send '!' token to reset the birthmark in the EEPROM so that the robot will know to restart and reset
Expand Down Expand Up @@ -441,7 +441,7 @@ byte buzzerVolume;
float amplifierFactor = 100.0; // to fit the actual amplifier range of BiBoard

int delayLong = 20;
int delayMid = 8;
int delayMid = 6;
int delayException = 5;
int delayShort = 3;
int delayStep = 1;
Expand Down
2 changes: 1 addition & 1 deletion src/motion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -353,7 +353,7 @@ float adjust(byte i)
currentAdjust[i] = idealAdjust;
#endif
int thres = (i > 3 && i % 4 < 2) ? 15 : 45;
currentAdjust[i] = max(float(-thres), min(float(thres), currentAdjust[i]));
currentAdjust[i] = max(float(-45), min(float(thres), currentAdjust[i]));
return currentAdjust[i];
}

Expand Down
11 changes: 6 additions & 5 deletions src/reaction.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,9 @@
void dealWithExceptions() {
#ifdef GYRO_PIN
if (gyroBalanceQ) {
if (imuException == -2 || (skill->period == 1 && abs(xyzReal[2]) >= 15) || (skill->period > 1 && abs(xyzReal[2]) >= 20)) {
delay(200);
}
// if (imuException == -2 || (skill->period == 1 && abs(xyzReal[2]) >= 15) || (skill->period > 1 && abs(xyzReal[2]) >= 20)) {
// delay(50);
// }
if (imuException) { // the gyro reaction switch can be toggled on/off by the 'g' token
switch (imuException) {
case -1:
Expand Down Expand Up @@ -1004,12 +1004,13 @@ void reaction() {
analogWrite(PWM_LED_PIN, abs(currentAng[8]));
#endif
skill->perform();
if (skill->period > 1)
if (skill->period > 1){
delay(delayShort + max(0, int(runDelay
#ifdef GYRO_PIN
- gyroBalanceQ * (max(abs(ypr[1]), abs(ypr[2])) / 10) // accelerate gait when tilted
- gyroBalanceQ * (max(abs(ypr[1]), abs(ypr[2])) / 20) // accelerate gait when tilted
#endif
)));
}
if (skill->period < 0) {
if (!strcmp(skill->skillName, "fd")) { // need to optimize logic to combine "rest" and "fold"
shutServos();
Expand Down
4 changes: 2 additions & 2 deletions src/voice.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,13 +105,13 @@ void set_voice(char *cmd) { // send some control command directly to the module
}
byte c = 0;
while (cmd[c] != '\0' && cmd[c] != '~')
;
c++;
cmd[c] = '\0';
SERIAL_VOICE.print("X");
SERIAL_VOICE.println(cmd);
delay(10);
if (cmd[1] != 'a' && cmd[1] != 'b') {
while (!SERIAL_VOICE.available()) { // the serial port has to be re-opened for the first time after rebooting. Don't know why.
if (!SERIAL_VOICE.available()) { // the serial port has to be re-opened for the first time after rebooting. Don't know why.
SERIAL_VOICE.end();
PTLF("Reopen Voice Serial port");
beginVoiceSerial();
Expand Down

0 comments on commit f2eff12

Please sign in to comment.