diff --git a/include/bioloid_registers.h b/include/bioloid_registers.h
index 3c550ef0ace010bcd7720f7afb6ae498c0d1a2d9..25129989e673341bed07e64f46a2dedf63e94680 100644
--- a/include/bioloid_registers.h
+++ b/include/bioloid_registers.h
@@ -5,238 +5,251 @@
extern "C" {
#endif
-#define DEVICE_MODEL_OFFSET ((unsigned short int)0x0000)
-#define FIRMWARE_VERSION_OFFSET ((unsigned short int)0x0002)
-#define DEVICE_ID_OFFSET ((unsigned short int)0x0003)
-#define BAUDRATE_OFFSET ((unsigned short int)0x0004)
-#define RETURN_DELAY_OFFSET ((unsigned short int)0x0005)
-#define MM_PERIOD_OFFSET ((unsigned short int)0x0006)
-#define GYRO_FB_ADC_CH_OFFSET ((unsigned short int)0x0008)
-#define GYRO_LR_ADC_CH_OFFSET ((unsigned short int)0x0009)
-#define RETURN_LEVEL_OFFSET ((unsigned short int)0x0010)
+#define DEVICE_MODEL_OFFSET ((unsigned short int)0x0000)
+#define FIRMWARE_VERSION_OFFSET ((unsigned short int)0x0002)
+#define DEVICE_ID_OFFSET ((unsigned short int)0x0003)
+#define BAUDRATE_OFFSET ((unsigned short int)0x0004)
+#define RETURN_DELAY_OFFSET ((unsigned short int)0x0005)
+#define MM_PERIOD_OFFSET ((unsigned short int)0x0006)
+#define MM_BAL_KNEE_GAIN_OFFSET ((unsigned short int)0x0008)
+#define MM_BAL_ANKLE_ROLL_GAIN_OFFSET ((unsigned short int)0x000A)
+#define MM_BAL_ANKLE_PITCH_GAIN_OFFSET ((unsigned short int)0x000C)
+#define MM_BAL_HIP_ROLL_GAIN_OFFSET ((unsigned short int)0x000E)
+#define RETURN_LEVEL_OFFSET ((unsigned short int)0x0010)
+#define GYRO_FB_ADC_CH_OFFSET ((unsigned short int)0x0011)
+#define GYRO_LR_ADC_CH_OFFSET ((unsigned short int)0x0012)
-#define LAST_EEPROM_OFFSET ((unsigned short int)0x0020)
+#define LAST_EEPROM_OFFSET ((unsigned short int)0x0020)
typedef enum {
- BIOLOID_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET,
- BIOLOID_MODEL_NUMBER_H = DEVICE_MODEL_OFFSET+1,
- BIOLOID_VERSION = FIRMWARE_VERSION_OFFSET,
- BIOLOID_ID = DEVICE_ID_OFFSET,
- BIOLOID_BAUD_RATE = BAUDRATE_OFFSET,
- BIOLOID_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET,
- BIOLOID_MM_PERIOD_L = MM_PERIOD_OFFSET,
- BIOLOID_MM_PERIOD_H = MM_PERIOD_OFFSET+1,
- BIOLIOD_GYRO_FB_ADC_CH = GYRO_FB_ADC_CH_OFFSET,
- BIOLIOD_GYRO_LR_ADC_CH = GYRO_LR_ADC_CH_OFFSET,
- BIOLOID_RETURN_LEVEL = RETURN_LEVEL_OFFSET,
- BIOLOID_USER1_LED_CNTRL = 0x20, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | blink | toggle | value | internally used
- BIOLOID_USER1_LED_PERIOD_L = 0x21,
- BIOLOID_USER1_LED_PERIOD_H = 0x22,
- BIOLOID_USER2_LED_CNTRL = 0x23, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | blink | toggle | value | internally used
- BIOLOID_USER2_LED_PERIOD_L = 0x24,
- BIOLOID_USER2_LED_PERIOD_H = 0x25,
- BIOLOID_RXD_LED_CNTRL = 0x26, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | blink | toggle | value | internally used
- BIOLOID_RXD_LED_PERIOD_L = 0x27,
- BIOLOID_RXD_LED_PERIOD_H = 0x28,
- BIOLOID_TXD_LED_CNTRL = 0x29, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | blink | toggle | value | internally used
- BIOLOID_TXD_LED_PERIOD_L = 0x2A,
- BIOLOID_TXD_LED_PERIOD_H = 0x2B,
- BIOLOID_USER_PB_CNTRL = 0x2C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | interrupt flag | enable interrupt | value | internally used
- BIOLOID_RESET_PB_CNTRL = 0x2D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | interrupt flag | enable interrupt | value | internally used
- BIOLOID_START_PB_CNTRL = 0x2E, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | interrupt flag | enable interrupt | value | internally used
- BIOLOID_MODE_PB_CNTRL = 0x2F, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | interrupt flag | enable interrupt | value | internally used
- BIOLOID_ADC_CNTRL = 0x30, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | | | | Enable
- BIOLOID_ADC_PERIOD = 0x31,
- BIOLOID_ADC_CH1_L = 0x32,
- BIOLOID_ADC_CH1_H = 0x33,
- BIOLOID_ADC_CH2_L = 0x34,
- BIOLOID_ADC_CH2_H = 0x35,
- BIOLOID_ADC_CH3_L = 0x36,
- BIOLOID_ADC_CH3_H = 0x37,
- BIOLOID_ADC_CH4_L = 0x38,
- BIOLOID_ADC_CH4_H = 0x39,
- BIOLOID_ADC_TEMP_L = 0x3A,
- BIOLOID_ADC_TEMP_H = 0x3B,
- BIOLOID_ADC_CH6_L = 0x3C,
- BIOLOID_ADC_CH6_H = 0x3D,
- BIOLOID_ADC_VREF_L = 0x3E,
- BIOLOID_ADC_VREF_H = 0x3F,
- BIOLOID_ADC_CH8_L = 0x40,
- BIOLOID_ADC_CH8_H = 0x41,
- BIOLOID_ZIGBEE_CNTRL = 0x42, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | | | Enable | Enable power
- BIOLOID_ZIGBEE_BAUDRATE = 0x43,
- BIOLOID_ZIGBEE_OWN_ID_L = 0x44,
- BIOLOID_ZIGBEE_OWN_ID_H = 0x45,
- BIOLOID_ZIGBEE_REM_ID_L = 0x46,
- BIOLOID_ZIGBEE_REM_ID_H = 0x47,
- BIOLOID_MM_NUM_SERVOS = 0x48,
- BIOLOID_MM_CNTRL = 0x49, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | | Enable power | Enable balance | Enable manager
- BIOLOID_MM_MODULE_EN0 = 0x4A, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0i
- // Enable servo 0 | assigned module | Enable servo 1 | assigned module
- // | 000 -> none |
- // | 001 -> action |
- // | 010 -> walk |
- // | 011 -> joints |
- BIOLOID_MM_MODULE_EN1 = 0x4B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 2 | assigned module | Enable servo 3 | assigned module
- BIOLOID_MM_MODULE_EN2 = 0x4C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 4 | assigned module | Enable servo 5 | assigned module
- BIOLOID_MM_MODULE_EN3 = 0x4D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 6 | assigned module | Enable servo 7 | assigned module
- BIOLOID_MM_MODULE_EN4 = 0x4E, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 8 | assigned module | Enable servo 9 | assigned module
- BIOLOID_MM_MODULE_EN5 = 0x4F, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 10 | assigned module | Enable servo 11 | assigned module
- BIOLOID_MM_MODULE_EN6 = 0x50, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 12 | assigned module | Enable servo 13 | assigned module
- BIOLOID_MM_MODULE_EN7 = 0x51, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 14 | assigned module | Enable servo 15 | assigned module
- BIOLOID_MM_MODULE_EN8 = 0x52, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 16 | assigned module | Enable servo 17 | assigned module
- BIOLOID_MM_MODULE_EN9 = 0x53, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 18 | assigned module | Enable servo 19 | assigned module
- BIOLOID_MM_MODULE_EN10 = 0x54, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 20 | assigned module | Enable servo 21 | assigned module
- BIOLOID_MM_MODULE_EN11 = 0x55, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 22 | assigned module | Enable servo 23 | assigned module
- BIOLOID_MM_MODULE_EN12 = 0x56, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 24 | assigned module | Enable servo 25 | assigned module
- BIOLOID_MM_MODULE_EN13 = 0x57, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 26 | assigned module | Enable servo 27 | assigned module
- BIOLOID_MM_MODULE_EN14 = 0x58, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 28 | assigned module | Enable servo 29 | assigned module
- BIOLOID_MM_MODULE_EN15 = 0x59, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // Enable servo 30 | assigned module | Enable servo 31 | assigned module
- BIOLOID_MM_SERVO0_CUR_POS_L = 0x5A, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO0_CUR_POS_H = 0x5B,
- BIOLOID_MM_SERVO1_CUR_POS_L = 0x5C, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO1_CUR_POS_H = 0x5D,
- BIOLOID_MM_SERVO2_CUR_POS_L = 0x5E, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO2_CUR_POS_H = 0x5F,
- BIOLOID_MM_SERVO3_CUR_POS_L = 0x60, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO3_CUR_POS_H = 0x61,
- BIOLOID_MM_SERVO4_CUR_POS_L = 0x62, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO4_CUR_POS_H = 0x63,
- BIOLOID_MM_SERVO5_CUR_POS_L = 0x64, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO5_CUR_POS_H = 0x65,
- BIOLOID_MM_SERVO6_CUR_POS_L = 0x66, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO6_CUR_POS_H = 0x67,
- BIOLOID_MM_SERVO7_CUR_POS_L = 0x68, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO7_CUR_POS_H = 0x69,
- BIOLOID_MM_SERVO8_CUR_POS_L = 0x6A, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO8_CUR_POS_H = 0x6B,
- BIOLOID_MM_SERVO9_CUR_POS_L = 0x6C, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO9_CUR_POS_H = 0x6D,
- BIOLOID_MM_SERVO10_CUR_POS_L = 0x6E, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO10_CUR_POS_H = 0x6F,
- BIOLOID_MM_SERVO11_CUR_POS_L = 0x70, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO11_CUR_POS_H = 0x71,
- BIOLOID_MM_SERVO12_CUR_POS_L = 0x72, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO12_CUR_POS_H = 0x73,
- BIOLOID_MM_SERVO13_CUR_POS_L = 0x74, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO13_CUR_POS_H = 0x75,
- BIOLOID_MM_SERVO14_CUR_POS_L = 0x76, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO14_CUR_POS_H = 0x77,
- BIOLOID_MM_SERVO15_CUR_POS_L = 0x78, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO15_CUR_POS_H = 0x79,
- BIOLOID_MM_SERVO16_CUR_POS_L = 0x7A, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO16_CUR_POS_H = 0x7B,
- BIOLOID_MM_SERVO17_CUR_POS_L = 0x7C, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO17_CUR_POS_H = 0x7D,
- BIOLOID_MM_SERVO18_CUR_POS_L = 0x7E, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO18_CUR_POS_H = 0x7F,
- BIOLOID_MM_SERVO19_CUR_POS_L = 0x80, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO19_CUR_POS_H = 0x81,
- BIOLOID_MM_SERVO20_CUR_POS_L = 0x82, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO20_CUR_POS_H = 0x83,
- BIOLOID_MM_SERVO21_CUR_POS_L = 0x84, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO21_CUR_POS_H = 0x85,
- BIOLOID_MM_SERVO22_CUR_POS_L = 0x86, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO22_CUR_POS_H = 0x87,
- BIOLOID_MM_SERVO23_CUR_POS_L = 0x88, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO23_CUR_POS_H = 0x89,
- BIOLOID_MM_SERVO24_CUR_POS_L = 0x8A, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO24_CUR_POS_H = 0x8B,
- BIOLOID_MM_SERVO25_CUR_POS_L = 0x8C, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO25_CUR_POS_H = 0x8D,
- BIOLOID_MM_SERVO26_CUR_POS_L = 0x8E, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO26_CUR_POS_H = 0x8F,
- BIOLOID_MM_SERVO27_CUR_POS_L = 0x90, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO27_CUR_POS_H = 0x91,
- BIOLOID_MM_SERVO28_CUR_POS_L = 0x92, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO28_CUR_POS_H = 0x93,
- BIOLOID_MM_SERVO29_CUR_POS_L = 0x94, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO29_CUR_POS_H = 0x95,
- BIOLOID_MM_SERVO30_CUR_POS_L = 0x96, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO30_CUR_POS_H = 0x97,
- BIOLOID_MM_SERVO31_CUR_POS_L = 0x98, // angle in fixed point format 9|7
- BIOLOID_MM_SERVO31_CUR_POS_H = 0x99,
- BIOLOID_ACTION_PAGE = 0x9A,
- BIOLOID_ACTION_CNTRL = 0x9B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | page running | interrupt flag | enable interrupt | stop page | start page
- BIOLOID_GYRO_CNTRL = 0x9C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | enable fall int | enable fall det | enable calibrate int | calibrate
- BIOLOID_GYRO_STATUS = 0x9D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- //
- BIOLOID_GYRO_FWD_FALL_THD = 0x9E,
- BIOLOID_GYRO_BWD_FALL_THD = 0x9F,
- BIOLOID_GYRO_LEFT_FALL_THD = 0xA0,
- BOILOID_GYRO_RIGHT_FALL_THD = 0xA1
+ BIOLOID_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET,
+ BIOLOID_MODEL_NUMBER_H = DEVICE_MODEL_OFFSET+1,
+ BIOLOID_VERSION = FIRMWARE_VERSION_OFFSET,
+ BIOLOID_ID = DEVICE_ID_OFFSET,
+ BIOLOID_BAUD_RATE = BAUDRATE_OFFSET,
+ BIOLOID_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET,
+ BIOLOID_MM_PERIOD_L = MM_PERIOD_OFFSET,
+ BIOLOID_MM_PERIOD_H = MM_PERIOD_OFFSET+1,
+ BIOLOID_MM_BAL_KNEE_GAIN_L = MM_BAL_KNEE_GAIN_OFFSET,// fixed point format 0|16
+ BIOLOID_MM_BAL_KNEE_GAIN_H = MM_BAL_KNEE_GAIN_OFFSET+1,
+ BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_L = MM_BAL_ANKLE_ROLL_GAIN_OFFSET,// fixed point format 0|16
+ BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_H = MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
+ BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_L = MM_BAL_ANKLE_PITCH_GAIN_OFFSET,// fixed point format 0|16
+ BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_H = MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
+ BIOLOID_MM_BAL_HIP_ROLL_GAIN_L = MM_BAL_HIP_ROLL_GAIN_OFFSET,// fixed point format 0|16
+ BIOLOID_MM_BAL_HIP_ROLL_GAIN_H = MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
+ BIOLOID_RETURN_LEVEL = RETURN_LEVEL_OFFSET,
+ BIOLIOD_GYRO_FB_ADC_CH = GYRO_FB_ADC_CH_OFFSET,
+ BIOLIOD_GYRO_LR_ADC_CH = GYRO_LR_ADC_CH_OFFSET,
+ BIOLOID_USER1_LED_CNTRL = 0x20, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | blink | toggle | value | internally used
+ BIOLOID_USER1_LED_PERIOD_L = 0x21,
+ BIOLOID_USER1_LED_PERIOD_H = 0x22,
+ BIOLOID_USER2_LED_CNTRL = 0x23, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | blink | toggle | value | internally used
+ BIOLOID_USER2_LED_PERIOD_L = 0x24,
+ BIOLOID_USER2_LED_PERIOD_H = 0x25,
+ BIOLOID_RXD_LED_CNTRL = 0x26, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | blink | toggle | value | internally used
+ BIOLOID_RXD_LED_PERIOD_L = 0x27,
+ BIOLOID_RXD_LED_PERIOD_H = 0x28,
+ BIOLOID_TXD_LED_CNTRL = 0x29, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | blink | toggle | value | internally used
+ BIOLOID_TXD_LED_PERIOD_L = 0x2A,
+ BIOLOID_TXD_LED_PERIOD_H = 0x2B,
+ BIOLOID_USER_PB_CNTRL = 0x2C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | interrupt flag | enable interrupt | value | internally used
+ BIOLOID_RESET_PB_CNTRL = 0x2D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | interrupt flag | enable interrupt | value | internally used
+ BIOLOID_START_PB_CNTRL = 0x2E, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | interrupt flag | enable interrupt | value | internally used
+ BIOLOID_MODE_PB_CNTRL = 0x2F, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | interrupt flag | enable interrupt | value | internally used
+ BIOLOID_ADC_CNTRL = 0x30, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | | | | Enable
+ BIOLOID_ADC_PERIOD = 0x31,
+ BIOLOID_ADC_CH1_L = 0x32,
+ BIOLOID_ADC_CH1_H = 0x33,
+ BIOLOID_ADC_CH2_L = 0x34,
+ BIOLOID_ADC_CH2_H = 0x35,
+ BIOLOID_ADC_CH3_L = 0x36,
+ BIOLOID_ADC_CH3_H = 0x37,
+ BIOLOID_ADC_CH4_L = 0x38,
+ BIOLOID_ADC_CH4_H = 0x39,
+ BIOLOID_ADC_TEMP_L = 0x3A,
+ BIOLOID_ADC_TEMP_H = 0x3B,
+ BIOLOID_ADC_CH6_L = 0x3C,
+ BIOLOID_ADC_CH6_H = 0x3D,
+ BIOLOID_ADC_VREF_L = 0x3E,
+ BIOLOID_ADC_VREF_H = 0x3F,
+ BIOLOID_ADC_CH8_L = 0x40,
+ BIOLOID_ADC_CH8_H = 0x41,
+ BIOLOID_ZIGBEE_CNTRL = 0x42, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | | | Enable | Enable power
+ BIOLOID_ZIGBEE_BAUDRATE = 0x43,
+ BIOLOID_ZIGBEE_OWN_ID_L = 0x44,
+ BIOLOID_ZIGBEE_OWN_ID_H = 0x45,
+ BIOLOID_ZIGBEE_REM_ID_L = 0x46,
+ BIOLOID_ZIGBEE_REM_ID_H = 0x47,
+ BIOLOID_MM_NUM_SERVOS = 0x48,
+ BIOLOID_MM_CNTRL = 0x49, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | | Enable power | Enable balance | Enable manager
+ BIOLOID_MM_MODULE_EN0 = 0x4A, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0i
+ // Enable servo 0 | assigned module | Enable servo 1 | assigned module
+ // | 000 -> none |
+ // | 001 -> action |
+ // | 010 -> walk |
+ // | 011 -> joints |
+ BIOLOID_MM_MODULE_EN1 = 0x4B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 2 | assigned module | Enable servo 3 | assigned module
+ BIOLOID_MM_MODULE_EN2 = 0x4C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 4 | assigned module | Enable servo 5 | assigned module
+ BIOLOID_MM_MODULE_EN3 = 0x4D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 6 | assigned module | Enable servo 7 | assigned module
+ BIOLOID_MM_MODULE_EN4 = 0x4E, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 8 | assigned module | Enable servo 9 | assigned module
+ BIOLOID_MM_MODULE_EN5 = 0x4F, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 10 | assigned module | Enable servo 11 | assigned module
+ BIOLOID_MM_MODULE_EN6 = 0x50, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 12 | assigned module | Enable servo 13 | assigned module
+ BIOLOID_MM_MODULE_EN7 = 0x51, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 14 | assigned module | Enable servo 15 | assigned module
+ BIOLOID_MM_MODULE_EN8 = 0x52, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 16 | assigned module | Enable servo 17 | assigned module
+ BIOLOID_MM_MODULE_EN9 = 0x53, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 18 | assigned module | Enable servo 19 | assigned module
+ BIOLOID_MM_MODULE_EN10 = 0x54, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 20 | assigned module | Enable servo 21 | assigned module
+ BIOLOID_MM_MODULE_EN11 = 0x55, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 22 | assigned module | Enable servo 23 | assigned module
+ BIOLOID_MM_MODULE_EN12 = 0x56, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 24 | assigned module | Enable servo 25 | assigned module
+ BIOLOID_MM_MODULE_EN13 = 0x57, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 26 | assigned module | Enable servo 27 | assigned module
+ BIOLOID_MM_MODULE_EN14 = 0x58, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 28 | assigned module | Enable servo 29 | assigned module
+ BIOLOID_MM_MODULE_EN15 = 0x59, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // Enable servo 30 | assigned module | Enable servo 31 | assigned module
+ BIOLOID_MM_SERVO0_CUR_POS_L = 0x5A, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO0_CUR_POS_H = 0x5B,
+ BIOLOID_MM_SERVO1_CUR_POS_L = 0x5C, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO1_CUR_POS_H = 0x5D,
+ BIOLOID_MM_SERVO2_CUR_POS_L = 0x5E, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO2_CUR_POS_H = 0x5F,
+ BIOLOID_MM_SERVO3_CUR_POS_L = 0x60, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO3_CUR_POS_H = 0x61,
+ BIOLOID_MM_SERVO4_CUR_POS_L = 0x62, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO4_CUR_POS_H = 0x63,
+ BIOLOID_MM_SERVO5_CUR_POS_L = 0x64, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO5_CUR_POS_H = 0x65,
+ BIOLOID_MM_SERVO6_CUR_POS_L = 0x66, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO6_CUR_POS_H = 0x67,
+ BIOLOID_MM_SERVO7_CUR_POS_L = 0x68, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO7_CUR_POS_H = 0x69,
+ BIOLOID_MM_SERVO8_CUR_POS_L = 0x6A, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO8_CUR_POS_H = 0x6B,
+ BIOLOID_MM_SERVO9_CUR_POS_L = 0x6C, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO9_CUR_POS_H = 0x6D,
+ BIOLOID_MM_SERVO10_CUR_POS_L = 0x6E, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO10_CUR_POS_H = 0x6F,
+ BIOLOID_MM_SERVO11_CUR_POS_L = 0x70, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO11_CUR_POS_H = 0x71,
+ BIOLOID_MM_SERVO12_CUR_POS_L = 0x72, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO12_CUR_POS_H = 0x73,
+ BIOLOID_MM_SERVO13_CUR_POS_L = 0x74, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO13_CUR_POS_H = 0x75,
+ BIOLOID_MM_SERVO14_CUR_POS_L = 0x76, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO14_CUR_POS_H = 0x77,
+ BIOLOID_MM_SERVO15_CUR_POS_L = 0x78, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO15_CUR_POS_H = 0x79,
+ BIOLOID_MM_SERVO16_CUR_POS_L = 0x7A, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO16_CUR_POS_H = 0x7B,
+ BIOLOID_MM_SERVO17_CUR_POS_L = 0x7C, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO17_CUR_POS_H = 0x7D,
+ BIOLOID_MM_SERVO18_CUR_POS_L = 0x7E, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO18_CUR_POS_H = 0x7F,
+ BIOLOID_MM_SERVO19_CUR_POS_L = 0x80, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO19_CUR_POS_H = 0x81,
+ BIOLOID_MM_SERVO20_CUR_POS_L = 0x82, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO20_CUR_POS_H = 0x83,
+ BIOLOID_MM_SERVO21_CUR_POS_L = 0x84, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO21_CUR_POS_H = 0x85,
+ BIOLOID_MM_SERVO22_CUR_POS_L = 0x86, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO22_CUR_POS_H = 0x87,
+ BIOLOID_MM_SERVO23_CUR_POS_L = 0x88, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO23_CUR_POS_H = 0x89,
+ BIOLOID_MM_SERVO24_CUR_POS_L = 0x8A, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO24_CUR_POS_H = 0x8B,
+ BIOLOID_MM_SERVO25_CUR_POS_L = 0x8C, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO25_CUR_POS_H = 0x8D,
+ BIOLOID_MM_SERVO26_CUR_POS_L = 0x8E, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO26_CUR_POS_H = 0x8F,
+ BIOLOID_MM_SERVO27_CUR_POS_L = 0x90, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO27_CUR_POS_H = 0x91,
+ BIOLOID_MM_SERVO28_CUR_POS_L = 0x92, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO28_CUR_POS_H = 0x93,
+ BIOLOID_MM_SERVO29_CUR_POS_L = 0x94, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO29_CUR_POS_H = 0x95,
+ BIOLOID_MM_SERVO30_CUR_POS_L = 0x96, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO30_CUR_POS_H = 0x97,
+ BIOLOID_MM_SERVO31_CUR_POS_L = 0x98, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO31_CUR_POS_H = 0x99,
+ BIOLOID_ACTION_PAGE = 0x9A,
+ BIOLOID_ACTION_CNTRL = 0x9B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | page running | interrupt flag | enable interrupt | stop page | start page
+ BIOLOID_GYRO_CNTRL = 0x9C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | enable fall int | enable fall det | enable calibrate int | calibrate
+ BIOLOID_GYRO_STATUS = 0x9D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ //
+ BIOLOID_GYRO_FWD_FALL_THD = 0x9E,
+ BIOLOID_GYRO_BWD_FALL_THD = 0x9F,
+ BIOLOID_GYRO_LEFT_FALL_THD = 0xA0,
+ BOILOID_GYRO_RIGHT_FALL_THD = 0xA1
} bioloid_registers;
-#define GPIO_BASE_ADDRESS 0x20
-#define GPIO_MEM_LENGTH 16
-#define GPIO_INT_USED 0x01
-#define GPIO_VALUE 0x02
-#define GPIO_TOGGLE 0x04
-#define GPIO_BLINK 0x08
-#define GPIO_INT_EN 0x02
-#define GPIO_INT_FLAG 0x04
+#define GPIO_BASE_ADDRESS 0x20
+#define GPIO_MEM_LENGTH 16
+#define GPIO_INT_USED 0x01
+#define GPIO_VALUE 0x02
+#define GPIO_TOGGLE 0x04
+#define GPIO_BLINK 0x08
+#define GPIO_INT_EN 0x02
+#define GPIO_INT_FLAG 0x04
-#define ADC_BASE_ADDRESS 0x30
-#define ADC_MEM_LENGTH 18
-#define ADC_ENABLE 0x01
+#define ADC_BASE_ADDRESS 0x30
+#define ADC_MEM_LENGTH 18
+#define ADC_ENABLE 0x01
-#define ZIGBEE_BASE_ADDRESS 0x42
-#define ZIGBEE_MEM_LENGTH 6
-#define ZIGBEE_EN_PWR 0x01
-#define ZIGBEE_ENABLE 0x02
+#define ZIGBEE_BASE_ADDRESS 0x42
+#define ZIGBEE_MEM_LENGTH 6
+#define ZIGBEE_EN_PWR 0x01
+#define ZIGBEE_ENABLE 0x02
-#define MANAGER_BASE_ADDRESS 0x48
-#define MANAGER_MEM_LENGTH 50
-#define MANAGER_ENABLE 0x01
-#define MANAGER_EN_BAL 0x02
-#define MANAGER_EN_PWR 0x04
-#define MANAGER_EVEN_SER_EN 0x80
-#define MANAGER_EVEN_SER_MOD 0x70
-#define MANAGER_ODD_SER_EN 0x08
-#define MANAGER_ODD_SER_MOD 0x07
+#define MANAGER_BASE_ADDRESS 0x48
+#define MANAGER_MEM_LENGTH 50
+#define MANAGER_EEPROM_LENGTH 10
+#define MANAGER_ENABLE 0x01
+#define MANAGER_EN_BAL 0x02
+#define MANAGER_EN_PWR 0x04
+#define MANAGER_EVEN_SER_EN 0x80
+#define MANAGER_EVEN_SER_MOD 0x70
+#define MANAGER_ODD_SER_EN 0x08
+#define MANAGER_ODD_SER_MOD 0x07
-#define ACTION_BASE_ADDRESS 0x9A
-#define ACTION_MEM_LENGTH 2
-#define ACTION_START 0x01
-#define ACTION_STOP 0x02
-#define ACTION_INT_EN 0x04
-#define ACTION_INT_FLAG 0x08
-#define ACTION_STATUS 0x10
+#define ACTION_BASE_ADDRESS 0x9A
+#define ACTION_MEM_LENGTH 2
+#define ACTION_START 0x01
+#define ACTION_STOP 0x02
+#define ACTION_INT_EN 0x04
+#define ACTION_INT_FLAG 0x08
+#define ACTION_STATUS 0x10
-#define GYRO_BASE_ADDRESS 0x9C
-#define GYRO_MEM_LENGTH 6
-#define GYRO_CALIBRATE 0x01
-#define GYRO_EN_CAL_INT 0x02
-#define GYRO_EN_FALL_DET 0x04
-#define GYRO_EN_FALL_DET_INT 0x08
+#define GYRO_BASE_ADDRESS 0x9C
+#define GYRO_MEM_LENGTH 6
+#define GYRO_CALIBRATE 0x01
+#define GYRO_EN_CAL_INT 0x02
+#define GYRO_EN_FALL_DET 0x04
+#define GYRO_EN_FALL_DET_INT 0x08
#ifdef __cplusplus
}
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 24441227d7f133f7cadb49a9356d8acbaeb9c736..3a1172a98a5fab9fb4db683993a1aee323eb00bc 100644
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -7,10 +7,31 @@ extern "C" {
#include "stm32f4xx_hal.h"
-typedef enum{MM_NONE = 0,
- MM_ACTION = 1,
- MM_WALKING = 2,
- MM_JOINTS = 3} TModules;
+typedef enum {
+ R_SHOULDER_PITCH = 1,
+ L_SHOULDER_PITCH = 2,
+ R_SHOULDER_ROLL = 3,
+ L_SHOULDER_ROLL = 4,
+ R_ELBOW = 5,
+ L_ELBOW = 6,
+ R_HIP_YAW = 7,
+ L_HIP_YAW = 8,
+ R_HIP_ROLL = 9,
+ L_HIP_ROLL = 10,
+ R_HIP_PITCH = 11,
+ L_HIP_PITCH = 12,
+ R_KNEE = 13,
+ L_KNEE = 14,
+ R_ANKLE_PITCH = 15,
+ L_ANKLE_PITCH = 16,
+ R_ANKLE_ROLL = 17,
+ L_ANKLE_ROLL = 18} servo_id_t;
+
+
+typedef enum {MM_NONE = 0,
+ MM_ACTION = 1,
+ MM_WALKING = 2,
+ MM_JOINTS = 3} TModules;
typedef struct
{
diff --git a/src/bioloid_gyro.c b/src/bioloid_gyro.c
index 8587e386394a972bb131586462b654723f43b1b7..7c4d3dab8f5edfd6e95365d192e6fd4877bf81e0 100644
--- a/src/bioloid_gyro.c
+++ b/src/bioloid_gyro.c
@@ -36,6 +36,8 @@ void gyro_calibrate(void)
gyro_fb_offset=0x0000;
gyro_lr_offset=0x0000;
+ // dummy delay to allow enough time to the ADC to get new data
+ HAL_Delay(2*adc_get_period());
for(i=0;i<GYRO_MAX_CAL_SAMPLES;i++)
{
gyro_fb_offset+=adc_get_channel_raw(fb_ch);
diff --git a/src/eeprom.c b/src/eeprom.c
index 74e9ec522249cda4119266d3b321fa4a9bf4f687..68155b94ad2c0608c3ccdb1388c184cecd3545e3 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -42,19 +42,24 @@
/* Includes ------------------------------------------------------------------*/
#include "eeprom.h"
#include "bioloid_registers.h"
+#include "adc_dma.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
-#define EEPROM_SIZE 0x09
-#define DEFAULT_DEVICE_MODEL 0x7300
-#define DEFAULT_FIRMWARE_VERSION 0x0001
-#define DEFAULT_DEVICE_ID 0x0002
-#define DEFAULT_BAUDRATE 0x0001
-#define DEFAULT_RETURN_DELAY 0x0000
-#define DEFAULT_MM_PERIOD 0x01FF
-#define DEFAULT_GYRO_FB_ADC_CH 0x0002
-#define DEFAULT_GYRO_LR_ADC_CH 0x0001
-#define DEFAULT_RETURN_LEVEL 0x0002
+#define EEPROM_SIZE 0x20
+#define DEFAULT_DEVICE_MODEL 0x7300
+#define DEFAULT_FIRMWARE_VERSION 0x0001
+#define DEFAULT_DEVICE_ID 0x0002
+#define DEFAULT_BAUDRATE 0x0001
+#define DEFAULT_RETURN_DELAY 0x0000
+#define DEFAULT_MM_PERIOD 0x01FF
+#define DEFAULT_BAL_KNEE_GAIN 0x04BE // 1/54 in fixed point format 0|16
+#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0x0CCD // 1/20
+#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0x0E39 // 1/18
+#define DEFAULT_BAL_HIP_ROLL_GAIN 0x0666 // 1/40
+#define DEFAULT_GYRO_FB_ADC_CH (uint16_t)ADC_CH1
+#define DEFAULT_GYRO_LR_ADC_CH (uint16_t)ADC_CH2
+#define DEFAULT_RETURN_LEVEL 0x0002
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
@@ -80,12 +85,28 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
MM_PERIOD_OFFSET,
DEFAULT_MM_PERIOD>>8,
MM_PERIOD_OFFSET+1,
+ DEFAULT_BAL_KNEE_GAIN&0xFF,
+ MM_BAL_KNEE_GAIN_OFFSET,
+ DEFAULT_BAL_KNEE_GAIN>>8,
+ MM_BAL_KNEE_GAIN_OFFSET+1,
+ DEFAULT_BAL_ANKLE_ROLL_GAIN&0xFF,
+ MM_BAL_ANKLE_ROLL_GAIN_OFFSET,
+ DEFAULT_BAL_ANKLE_ROLL_GAIN>>8,
+ MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
+ DEFAULT_BAL_ANKLE_PITCH_GAIN&0xFF,
+ MM_BAL_ANKLE_PITCH_GAIN_OFFSET,
+ DEFAULT_BAL_ANKLE_PITCH_GAIN>>8,
+ MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
+ DEFAULT_BAL_HIP_ROLL_GAIN&0xFF,
+ MM_BAL_HIP_ROLL_GAIN_OFFSET,
+ DEFAULT_BAL_HIP_ROLL_GAIN>>8,
+ MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
+ DEFAULT_RETURN_LEVEL,
+ RETURN_LEVEL_OFFSET,
DEFAULT_GYRO_FB_ADC_CH,
GYRO_FB_ADC_CH_OFFSET,
DEFAULT_GYRO_LR_ADC_CH,
- GYRO_LR_ADC_CH_OFFSET,
- DEFAULT_RETURN_LEVEL,
- RETURN_LEVEL_OFFSET};// return level
+ GYRO_LR_ADC_CH_OFFSET};// return level
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
diff --git a/src/motion_manager.c b/src/motion_manager.c
index dfed4f3c7596ce748a6eec40c9b2cd4e393e4b52..1c430c285ccb92951665886f790483d085a392c3 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -106,25 +106,31 @@ void manager_get_target_position(void)
void manager_balance(void)
{
adc_ch_t fb_ch=gyro_get_fb_adc_channel(),lr_ch=gyro_get_lr_adc_channel();
+ int16_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain;
uint16_t fb_offset,lr_offset;
int16_t fb_value,lr_value;
if(manager_balance_enabled==0x01)// balance is enabled
{
+ // get the balance gains
+ knee_gain=ram_data[BIOLOID_MM_BAL_KNEE_GAIN_L]+(ram_data[BIOLOID_MM_BAL_KNEE_GAIN_H]*256);
+ ankle_roll_gain=ram_data[BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_L]+(ram_data[BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_H]<<8);
+ ankle_pitch_gain=ram_data[BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_L]+(ram_data[BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_H]<<8);
+ hip_roll_gain=ram_data[BIOLOID_MM_BAL_HIP_ROLL_GAIN_L]+(ram_data[BIOLOID_MM_BAL_HIP_ROLL_GAIN_H]<<8);
// get the offsets of the gyroscope calibration
gyro_get_offsets(&fb_offset,&lr_offset);
// get the values of the gyroscope
fb_value=adc_get_channel_raw(fb_ch)-fb_offset;
lr_value=adc_get_channel_raw(lr_ch)-lr_offset;
// compensate the servo angle values
- manager_balance_offset[13]=(fb_value*manager_servos[13].max_angle)/(manager_servos[13].encoder_resolution*54);
- manager_balance_offset[15]=(fb_value*manager_servos[15].max_angle)/(manager_servos[15].encoder_resolution*18);
- manager_balance_offset[14]=-(fb_value*manager_servos[14].max_angle)/(manager_servos[14].encoder_resolution*54);
- manager_balance_offset[16]=-(fb_value*manager_servos[16].max_angle)/(manager_servos[16].encoder_resolution*18);
- manager_balance_offset[9]=(lr_value*manager_servos[9].max_angle)/(manager_servos[9].encoder_resolution*40);
- manager_balance_offset[10]=(lr_value*manager_servos[10].max_angle)/(manager_servos[10].encoder_resolution*40);
- manager_balance_offset[17]=-(lr_value*manager_servos[17].max_angle)/(manager_servos[17].encoder_resolution*20);
- manager_balance_offset[18]=-(lr_value*manager_servos[18].max_angle)/(manager_servos[18].encoder_resolution*20);
+ manager_balance_offset[R_KNEE]=(((fb_value*manager_servos[R_KNEE].max_angle)/manager_servos[R_KNEE].encoder_resolution)*knee_gain)/65536;
+ manager_balance_offset[R_ANKLE_PITCH]=(((fb_value*manager_servos[R_ANKLE_PITCH].max_angle)/manager_servos[R_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)/65536;
+ manager_balance_offset[L_KNEE]=-(((fb_value*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)/65536;
+ manager_balance_offset[L_ANKLE_PITCH]=-(((fb_value*manager_servos[L_ANKLE_PITCH].max_angle)/manager_servos[L_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)/65536;
+ manager_balance_offset[R_HIP_ROLL]=(((lr_value*manager_servos[R_HIP_ROLL].max_angle)/manager_servos[R_HIP_ROLL].encoder_resolution)*hip_roll_gain)/65536;
+ manager_balance_offset[L_HIP_ROLL]=(((lr_value*manager_servos[L_HIP_ROLL].max_angle)/manager_servos[L_HIP_ROLL].encoder_resolution)*hip_roll_gain)/65536;
+ manager_balance_offset[R_ANKLE_ROLL]=-(((lr_value*manager_servos[R_ANKLE_ROLL].max_angle)/manager_servos[R_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)/65536;
+ manager_balance_offset[L_ANKLE_ROLL]=-(((lr_value*manager_servos[L_ANKLE_ROLL].max_angle)/manager_servos[L_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)/65536;
}
}
@@ -456,7 +462,7 @@ inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
uint8_t manager_in_range(unsigned short int address, unsigned short int length)
{
if(ram_in_window(MANAGER_BASE_ADDRESS,MANAGER_MEM_LENGTH,address,length) ||
- ram_in_window(BIOLOID_MM_PERIOD_L,BIOLOID_MM_PERIOD_H,address,length))
+ ram_in_window(BIOLOID_MM_PERIOD_L,MANAGER_EEPROM_LENGTH,address,length))
return 0x01;
else
return 0x00;
diff --git a/src/ram.c b/src/ram.c
index 7d49db96fccbe5d383208610011af1fb552f5454..046ecab0f6a13b1c3644b929e768cb68a7fde03e 100644
--- a/src/ram.c
+++ b/src/ram.c
@@ -26,8 +26,28 @@ void ram_init(void)
ram_data[MM_PERIOD_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0)
ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET,&eeprom_data)==0)
+ ram_data[MM_BAL_KNEE_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET+1,&eeprom_data)==0)
+ ram_data[MM_BAL_KNEE_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET,&eeprom_data)==0)
+ ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,&eeprom_data)==0)
+ ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET,&eeprom_data)==0)
+ ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,&eeprom_data)==0)
+ ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET,&eeprom_data)==0)
+ ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET+1,&eeprom_data)==0)
+ ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data)==0)
ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)eeprom_data;
+ if(EE_ReadVariable(GYRO_FB_ADC_CH_OFFSET,&eeprom_data)==0)
+ ram_data[GYRO_FB_ADC_CH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(GYRO_LR_ADC_CH_OFFSET,&eeprom_data)==0)
+ ram_data[GYRO_LR_ADC_CH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
}
inline void ram_read_byte(uint8_t address,uint8_t *data)