diff --git a/include/bioloid_registers.h b/include/bioloid_registers.h
index 25129989e673341bed07e64f46a2dedf63e94680..54f7994babffa26bd1b10409f811af1846fbc203 100644
--- a/include/bioloid_registers.h
+++ b/include/bioloid_registers.h
@@ -16,10 +16,42 @@ extern "C" {
#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 MM_SERVO0_OFFSET ((unsigned short int)0x0011)
+#define MM_SERVO1_OFFSET ((unsigned short int)0x0012)
+#define MM_SERVO2_OFFSET ((unsigned short int)0x0013)
+#define MM_SERVO3_OFFSET ((unsigned short int)0x0014)
+#define MM_SERVO4_OFFSET ((unsigned short int)0x0015)
+#define MM_SERVO5_OFFSET ((unsigned short int)0x0016)
+#define MM_SERVO6_OFFSET ((unsigned short int)0x0017)
+#define MM_SERVO7_OFFSET ((unsigned short int)0x0018)
+#define MM_SERVO8_OFFSET ((unsigned short int)0x0019)
+#define MM_SERVO9_OFFSET ((unsigned short int)0x001A)
+#define MM_SERVO10_OFFSET ((unsigned short int)0x001B)
+#define MM_SERVO11_OFFSET ((unsigned short int)0x001C)
+#define MM_SERVO12_OFFSET ((unsigned short int)0x001D)
+#define MM_SERVO13_OFFSET ((unsigned short int)0x001E)
+#define MM_SERVO14_OFFSET ((unsigned short int)0x001F)
+#define MM_SERVO15_OFFSET ((unsigned short int)0x0020)
+#define MM_SERVO16_OFFSET ((unsigned short int)0x0021)
+#define MM_SERVO17_OFFSET ((unsigned short int)0x0022)
+#define MM_SERVO18_OFFSET ((unsigned short int)0x0023)
+#define MM_SERVO19_OFFSET ((unsigned short int)0x0024)
+#define MM_SERVO20_OFFSET ((unsigned short int)0x0025)
+#define MM_SERVO21_OFFSET ((unsigned short int)0x0026)
+#define MM_SERVO22_OFFSET ((unsigned short int)0x0027)
+#define MM_SERVO23_OFFSET ((unsigned short int)0x0028)
+#define MM_SERVO24_OFFSET ((unsigned short int)0x0029)
+#define MM_SERVO25_OFFSET ((unsigned short int)0x002A)
+#define MM_SERVO26_OFFSET ((unsigned short int)0x002B)
+#define MM_SERVO27_OFFSET ((unsigned short int)0x002C)
+#define MM_SERVO28_OFFSET ((unsigned short int)0x002D)
+#define MM_SERVO29_OFFSET ((unsigned short int)0x002E)
+#define MM_SERVO30_OFFSET ((unsigned short int)0x002F)
+#define MM_SERVO31_OFFSET ((unsigned short int)0x0030)
+#define GYRO_FB_ADC_CH_OFFSET ((unsigned short int)0x0031)
+#define GYRO_LR_ADC_CH_OFFSET ((unsigned short int)0x0032)
-#define LAST_EEPROM_OFFSET ((unsigned short int)0x0020)
+#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
typedef enum {
BIOLOID_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET,
@@ -39,175 +71,207 @@ typedef enum {
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,
+ BIOLOID_MM_SERVO0_OFFSET = MM_SERVO0_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO1_OFFSET = MM_SERVO1_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO2_OFFSET = MM_SERVO2_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO3_OFFSET = MM_SERVO3_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO4_OFFSET = MM_SERVO4_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO5_OFFSET = MM_SERVO5_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO6_OFFSET = MM_SERVO6_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO7_OFFSET = MM_SERVO7_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO8_OFFSET = MM_SERVO8_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO9_OFFSET = MM_SERVO9_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO10_OFFSET = MM_SERVO10_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO11_OFFSET = MM_SERVO11_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO12_OFFSET = MM_SERVO12_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO13_OFFSET = MM_SERVO13_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO14_OFFSET = MM_SERVO14_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO15_OFFSET = MM_SERVO15_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO16_OFFSET = MM_SERVO16_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO17_OFFSET = MM_SERVO17_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO18_OFFSET = MM_SERVO18_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO19_OFFSET = MM_SERVO19_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO20_OFFSET = MM_SERVO20_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO21_OFFSET = MM_SERVO21_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO22_OFFSET = MM_SERVO22_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO23_OFFSET = MM_SERVO23_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO24_OFFSET = MM_SERVO24_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO25_OFFSET = MM_SERVO25_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO26_OFFSET = MM_SERVO26_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO27_OFFSET = MM_SERVO27_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO28_OFFSET = MM_SERVO28_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO29_OFFSET = MM_SERVO29_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO30_OFFSET = MM_SERVO30_OFFSET, // angle offset in fixed point format 1-3|4
+ BIOLOID_MM_SERVO31_OFFSET = MM_SERVO31_OFFSET, // angle offset in fixed point format 1-3|4
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_USER1_LED_CNTRL = 0x0100, // 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 = 0x0101,
+ BIOLOID_USER1_LED_PERIOD_H = 0x0102,
+ BIOLOID_USER2_LED_CNTRL = 0x0103, // 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 = 0x0104,
+ BIOLOID_USER2_LED_PERIOD_H = 0x0105,
+ BIOLOID_RXD_LED_CNTRL = 0x0106, // 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 = 0x0107,
+ BIOLOID_RXD_LED_PERIOD_H = 0x0108,
+ BIOLOID_TXD_LED_CNTRL = 0x0109, // 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 = 0x010A,
+ BIOLOID_TXD_LED_PERIOD_H = 0x010B,
+ BIOLOID_USER_PB_CNTRL = 0x010C, // 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 = 0x010D, // 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 = 0x010E, // 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 = 0x010F, // 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 = 0x0110, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | | | | Enable
+ BIOLOID_ADC_PERIOD = 0x0111,
+ BIOLOID_ADC_CH1_L = 0x0112,
+ BIOLOID_ADC_CH1_H = 0x0113,
+ BIOLOID_ADC_CH2_L = 0x0114,
+ BIOLOID_ADC_CH2_H = 0x0115,
+ BIOLOID_ADC_CH3_L = 0x0116,
+ BIOLOID_ADC_CH3_H = 0x0117,
+ BIOLOID_ADC_CH4_L = 0x0118,
+ BIOLOID_ADC_CH4_H = 0x0119,
+ BIOLOID_ADC_TEMP_L = 0x011A,
+ BIOLOID_ADC_TEMP_H = 0x011B,
+ BIOLOID_ADC_CH6_L = 0x011C,
+ BIOLOID_ADC_CH6_H = 0x011D,
+ BIOLOID_ADC_VREF_L = 0x011E,
+ BIOLOID_ADC_VREF_H = 0x011F,
+ BIOLOID_ADC_CH8_L = 0x0120,
+ BIOLOID_ADC_CH8_H = 0x0121,
+ BIOLOID_ZIGBEE_CNTRL = 0x0122, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | | | Enable | Enable power
+ BIOLOID_ZIGBEE_BAUDRATE = 0x0123,
+ BIOLOID_ZIGBEE_OWN_ID_L = 0x0124,
+ BIOLOID_ZIGBEE_OWN_ID_H = 0x0125,
+ BIOLOID_ZIGBEE_REM_ID_L = 0x0126,
+ BIOLOID_ZIGBEE_REM_ID_H = 0x0127,
+ BIOLOID_MM_NUM_SERVOS = 0x0128,
+ BIOLOID_MM_CNTRL = 0x0129, // 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 = 0x012A, // 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 = 0x012B, // 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 = 0x012C, // 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 = 0x012D, // 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 = 0x012E, // 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 = 0x012F, // 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 = 0x0130, // 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 = 0x0131, // 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 = 0x0132, // 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 = 0x0133, // 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 = 0x0134, // 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 = 0x0135, // 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 = 0x0136, // 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 = 0x0137, // 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 = 0x0138, // 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 = 0x0139, // 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 = 0x013A, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO0_CUR_POS_H = 0x013B,
+ BIOLOID_MM_SERVO1_CUR_POS_L = 0x013C, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO1_CUR_POS_H = 0x013D,
+ BIOLOID_MM_SERVO2_CUR_POS_L = 0x013E, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO2_CUR_POS_H = 0x013F,
+ BIOLOID_MM_SERVO3_CUR_POS_L = 0x0140, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO3_CUR_POS_H = 0x0141,
+ BIOLOID_MM_SERVO4_CUR_POS_L = 0x0142, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO4_CUR_POS_H = 0x0143,
+ BIOLOID_MM_SERVO5_CUR_POS_L = 0x0144, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO5_CUR_POS_H = 0x0145,
+ BIOLOID_MM_SERVO6_CUR_POS_L = 0x0146, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO6_CUR_POS_H = 0x0147,
+ BIOLOID_MM_SERVO7_CUR_POS_L = 0x0148, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO7_CUR_POS_H = 0x0149,
+ BIOLOID_MM_SERVO8_CUR_POS_L = 0x014A, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO8_CUR_POS_H = 0x014B,
+ BIOLOID_MM_SERVO9_CUR_POS_L = 0x014C, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO9_CUR_POS_H = 0x014D,
+ BIOLOID_MM_SERVO10_CUR_POS_L = 0x014E, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO10_CUR_POS_H = 0x014F,
+ BIOLOID_MM_SERVO11_CUR_POS_L = 0x0150, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO11_CUR_POS_H = 0x0151,
+ BIOLOID_MM_SERVO12_CUR_POS_L = 0x0152, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO12_CUR_POS_H = 0x0153,
+ BIOLOID_MM_SERVO13_CUR_POS_L = 0x0154, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO13_CUR_POS_H = 0x0155,
+ BIOLOID_MM_SERVO14_CUR_POS_L = 0x0156, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO14_CUR_POS_H = 0x0157,
+ BIOLOID_MM_SERVO15_CUR_POS_L = 0x0158, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO15_CUR_POS_H = 0x0159,
+ BIOLOID_MM_SERVO16_CUR_POS_L = 0x015A, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO16_CUR_POS_H = 0x015B,
+ BIOLOID_MM_SERVO17_CUR_POS_L = 0x015C, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO17_CUR_POS_H = 0x015D,
+ BIOLOID_MM_SERVO18_CUR_POS_L = 0x015E, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO18_CUR_POS_H = 0x015F,
+ BIOLOID_MM_SERVO19_CUR_POS_L = 0x0160, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO19_CUR_POS_H = 0x0161,
+ BIOLOID_MM_SERVO20_CUR_POS_L = 0x0162, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO20_CUR_POS_H = 0x0163,
+ BIOLOID_MM_SERVO21_CUR_POS_L = 0x0164, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO21_CUR_POS_H = 0x0165,
+ BIOLOID_MM_SERVO22_CUR_POS_L = 0x0166, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO22_CUR_POS_H = 0x0167,
+ BIOLOID_MM_SERVO23_CUR_POS_L = 0x0168, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO23_CUR_POS_H = 0x0169,
+ BIOLOID_MM_SERVO24_CUR_POS_L = 0x016A, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO24_CUR_POS_H = 0x016B,
+ BIOLOID_MM_SERVO25_CUR_POS_L = 0x016C, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO25_CUR_POS_H = 0x016D,
+ BIOLOID_MM_SERVO26_CUR_POS_L = 0x016E, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO26_CUR_POS_H = 0x016F,
+ BIOLOID_MM_SERVO27_CUR_POS_L = 0x0170, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO27_CUR_POS_H = 0x0171,
+ BIOLOID_MM_SERVO28_CUR_POS_L = 0x0172, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO28_CUR_POS_H = 0x0173,
+ BIOLOID_MM_SERVO29_CUR_POS_L = 0x0174, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO29_CUR_POS_H = 0x0175,
+ BIOLOID_MM_SERVO30_CUR_POS_L = 0x0176, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO30_CUR_POS_H = 0x0177,
+ BIOLOID_MM_SERVO31_CUR_POS_L = 0x0178, // angle in fixed point format 9|7
+ BIOLOID_MM_SERVO31_CUR_POS_H = 0x0179,
+ BIOLOID_ACTION_PAGE = 0x017A,
+ BIOLOID_ACTION_CNTRL = 0x017B, // 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 = 0x017C, // 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 = 0x017D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ //
+ BIOLOID_GYRO_FWD_FALL_THD = 0x017E,
+ BIOLOID_GYRO_BWD_FALL_THD = 0x017F,
+ BIOLOID_GYRO_LEFT_FALL_THD = 0x0180,
+ BOILOID_GYRO_RIGHT_FALL_THD = 0x0181
} bioloid_registers;
-#define GPIO_BASE_ADDRESS 0x20
+#define GPIO_BASE_ADDRESS 0x0100
#define GPIO_MEM_LENGTH 16
#define GPIO_INT_USED 0x01
#define GPIO_VALUE 0x02
@@ -216,17 +280,17 @@ typedef enum {
#define GPIO_INT_EN 0x02
#define GPIO_INT_FLAG 0x04
-#define ADC_BASE_ADDRESS 0x30
+#define ADC_BASE_ADDRESS 0x0110
#define ADC_MEM_LENGTH 18
#define ADC_ENABLE 0x01
-#define ZIGBEE_BASE_ADDRESS 0x42
+#define ZIGBEE_BASE_ADDRESS 0x0122
#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_BASE_ADDRESS 0x0128
+#define MANAGER_MEM_LENGTH 82
#define MANAGER_EEPROM_LENGTH 10
#define MANAGER_ENABLE 0x01
#define MANAGER_EN_BAL 0x02
@@ -236,7 +300,7 @@ typedef enum {
#define MANAGER_ODD_SER_EN 0x08
#define MANAGER_ODD_SER_MOD 0x07
-#define ACTION_BASE_ADDRESS 0x9A
+#define ACTION_BASE_ADDRESS 0x017A
#define ACTION_MEM_LENGTH 2
#define ACTION_START 0x01
#define ACTION_STOP 0x02
@@ -244,7 +308,7 @@ typedef enum {
#define ACTION_INT_FLAG 0x08
#define ACTION_STATUS 0x10
-#define GYRO_BASE_ADDRESS 0x9C
+#define GYRO_BASE_ADDRESS 0x017C
#define GYRO_MEM_LENGTH 6
#define GYRO_CALIBRATE 0x01
#define GYRO_EN_CAL_INT 0x02
diff --git a/include/dyn_servos.h b/include/dyn_servos.h
index 0ed9a2b2db3adcfbda4037b51d568da12086228e..ffe5416ea1ddee5716363fcbd523ccfe49be08d2 100644
--- a/include/dyn_servos.h
+++ b/include/dyn_servos.h
@@ -8,6 +8,7 @@ extern "C" {
// servo models
#define SERVO_AX12A 0x000C
#define SERVO_AX12W 0x012C
+#define SERVO_AX12N 0xF00C
#define SERVO_AX18A 0x0012
#define SERVO_MX28 0x001D
#define SERVO_RX24F 0x0018
@@ -45,14 +46,14 @@ typedef enum{
P_HIGH_CALIBRATION_H = 23,
P_TORQUE_ENABLE = 24,
P_LED = 25,
- P_CW_COMPLIANCE_MARGIN = 26,
- P_CCW_COMPLIANCE_MARGIN = 27,
- P_CW_COMPLIANCE_SLOPE = 28,
- P_CCW_COMPLIANCE_SLOPE = 29,
- P_D_GAIN = 26,
- P_I_GAIN = 27,
- P_P_GAIN = 28,
- P_RESERVED = 29,
+ P_CW_COMPLIANCE_MARGIN = 26,// servos with compliance
+ P_CCW_COMPLIANCE_MARGIN = 27,// servos with compliance
+ P_CW_COMPLIANCE_SLOPE = 28,// servos with compliance
+ P_CCW_COMPLIANCE_SLOPE = 29,// servos with compliance
+ P_D_GAIN = 26,// servos with PID
+ P_I_GAIN = 27,// servos with PID
+ P_P_GAIN = 28,// servos with PID
+ P_RESERVED = 29,// servos with PID
P_GOAL_POSITION_L = 30,
P_GOAL_POSITION_H = 31,
P_MOVING_SPEED_L = 32,
@@ -71,8 +72,8 @@ typedef enum{
P_PAUSE_TIME = 45,
P_MOVING = 46,
P_LOCK = 47,
- P_PUNCH_L = 48,
- P_PUNCH_H = 49,
+ P_PUNCH_L = 48,// servos with compliance
+ P_PUNCH_H = 49,// servos with compliance
P_RESERVED4 = 50,
P_RESERVED5 = 51,
P_POT_L = 52,
diff --git a/include/eeprom.h b/include/eeprom.h
index d1edf80f83f8aea550f16274e909670a5c7111e5..bb184e86d8847e8f63129b9896b9163d7d99f8e0 100755
--- a/include/eeprom.h
+++ b/include/eeprom.h
@@ -83,7 +83,7 @@ extern "C" {
#define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */
-#define NB_OF_VAR ((uint8_t)0x03)
+#define NB_OF_VAR ((uint8_t)0x33)
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
diff --git a/include/ram.h b/include/ram.h
index 4d53763ff5c159e053f216b0bb98de6373098626..c69e8c5a7d22aa6729154e357c1b1d123b969a32 100644
--- a/include/ram.h
+++ b/include/ram.h
@@ -14,21 +14,21 @@ extern "C" {
#define RAM_BAD_BIT -3
#define RAM_BAD_ACCESS -4
-#define RAM_SIZE 256
+#define RAM_SIZE 512
extern uint8_t ram_data[RAM_SIZE];
void ram_init(void);
-inline void ram_read_byte(uint8_t address, uint8_t *data);
-inline void ram_read_word(uint8_t address, uint16_t *data);
-uint8_t ram_read_table(uint8_t address, uint8_t length,uint8_t *data);
-uint8_t ram_set_bit(uint8_t address, uint8_t bit);
-uint8_t ram_clear_bit(uint8_t address, uint8_t bit);
-uint8_t ram_write_byte(uint8_t address, uint8_t data);
-uint8_t ram_write_word(uint8_t address, uint16_t data);
-uint8_t ram_write_table(uint8_t address, uint8_t length,uint8_t *data);
-inline uint8_t ram_in_range(uint8_t reg,uint8_t address,uint8_t length);
-uint8_t ram_in_window(uint8_t start_reg,uint8_t reg_length,uint8_t start_address,uint8_t address_length);
+inline void ram_read_byte(uint16_t address, uint8_t *data);
+inline void ram_read_word(uint16_t address, uint16_t *data);
+uint8_t ram_read_table(uint16_t address, uint16_t length,uint8_t *data);
+uint8_t ram_set_bit(uint16_t address, uint8_t bit);
+uint8_t ram_clear_bit(uint16_t address, uint8_t bit);
+uint8_t ram_write_byte(uint16_t address, uint8_t data);
+uint8_t ram_write_word(uint16_t address, uint16_t data);
+uint8_t ram_write_table(uint16_t address, uint16_t length,uint8_t *data);
+inline uint8_t ram_in_range(uint16_t reg,uint16_t address,uint16_t length);
+uint8_t ram_in_window(uint16_t start_reg,uint16_t reg_length,uint16_t start_address,uint16_t address_length);
#ifdef __cplusplus
}
diff --git a/src/bioloid_dyn_slave.c b/src/bioloid_dyn_slave.c
index 86d6f8ffff9525794a0ebe5b975c3538e701c5d3..b2a0edfd9a9a3adb67f6326be462ce8eff90c42b 100644
--- a/src/bioloid_dyn_slave.c
+++ b/src/bioloid_dyn_slave.c
@@ -47,7 +47,7 @@ unsigned char bioloid_on_read(unsigned short int address,unsigned short int leng
unsigned char bioloid_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
{
- unsigned char i,j;
+ unsigned short int i,j;
/* dynamixel slave internal operation registers */
if(ram_in_range(BIOLOID_ID,address,length))
@@ -96,6 +96,11 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
return 0x00;
}
+void bioloid_on_ping(void)
+{
+ gpio_toggle_led(RXD_LED);
+}
+
/* interrupt service routines */
void DYN_SLAVE_TIMER_IRQHandler(void)
{
@@ -139,9 +144,10 @@ void bioloid_dyn_slave_init(void)
priorities.dma_tx_subpriority=0;
usart3_init(&bioloid_dyn_slave_comm,&bioloid_comm_init,&priorities);
- dyn_slave_init(&bioloid_dyn_slave,&bioloid_dyn_slave_comm,ram_data[BIOLOID_ID],DYN_VER1);
+ dyn_slave_init(&bioloid_dyn_slave,&bioloid_dyn_slave_comm,ram_data[BIOLOID_ID],DYN_VER2);
bioloid_dyn_slave.on_read=bioloid_on_read;
bioloid_dyn_slave.on_write=bioloid_on_write;
+ bioloid_dyn_slave.on_ping=bioloid_on_ping;
dyn_slave_set_return_delay(&bioloid_dyn_slave,ram_data[BIOLOID_RETURN_DELAY_TIME]);
dyn_slave_set_return_level(&bioloid_dyn_slave,ram_data[BIOLOID_RETURN_LEVEL]);
diff --git a/src/eeprom.c b/src/eeprom.c
index 2f1c990eeff5762b655c1ba6b313c11e9e4d2e41..5d965d6f0befaadad4f5aee206c194d73f8e8aa4 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -60,6 +60,38 @@
#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
+#define DEFAULT_SERVO0_OFFSET 0x0000
+#define DEFAULT_SERVO1_OFFSET 0x0000
+#define DEFAULT_SERVO2_OFFSET 0x0000
+#define DEFAULT_SERVO3_OFFSET 0x0000
+#define DEFAULT_SERVO4_OFFSET 0x0000
+#define DEFAULT_SERVO5_OFFSET 0x0000
+#define DEFAULT_SERVO6_OFFSET 0x0000
+#define DEFAULT_SERVO7_OFFSET 0x0000
+#define DEFAULT_SERVO8_OFFSET 0x0000
+#define DEFAULT_SERVO9_OFFSET 0x0000
+#define DEFAULT_SERVO10_OFFSET 0x0000
+#define DEFAULT_SERVO11_OFFSET 0x0000
+#define DEFAULT_SERVO12_OFFSET 0x0000
+#define DEFAULT_SERVO13_OFFSET 0x0000
+#define DEFAULT_SERVO14_OFFSET 0x0000
+#define DEFAULT_SERVO15_OFFSET 0x0000
+#define DEFAULT_SERVO16_OFFSET 0x0000
+#define DEFAULT_SERVO17_OFFSET 0x0000
+#define DEFAULT_SERVO18_OFFSET 0x0000
+#define DEFAULT_SERVO19_OFFSET 0x0000
+#define DEFAULT_SERVO20_OFFSET 0x0000
+#define DEFAULT_SERVO21_OFFSET 0x0000
+#define DEFAULT_SERVO22_OFFSET 0x0000
+#define DEFAULT_SERVO23_OFFSET 0x0000
+#define DEFAULT_SERVO24_OFFSET 0x0000
+#define DEFAULT_SERVO25_OFFSET 0x0000
+#define DEFAULT_SERVO26_OFFSET 0x0000
+#define DEFAULT_SERVO27_OFFSET 0x0000
+#define DEFAULT_SERVO28_OFFSET 0x0000
+#define DEFAULT_SERVO29_OFFSET 0x0000
+#define DEFAULT_SERVO30_OFFSET 0x0000
+#define DEFAULT_SERVO31_OFFSET 0x0000
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
@@ -67,46 +99,58 @@
uint16_t DataVar = 0;
/* Virtual address defined by the user: 0xFFFF value is prohibited */
-uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
- 0xFFFF,
- DEFAULT_DEVICE_MODEL&0xFF,// model number LSB
- DEVICE_MODEL_OFFSET,
- DEFAULT_DEVICE_MODEL>>8,// model number MSB
- DEVICE_MODEL_OFFSET+1,
- DEFAULT_FIRMWARE_VERSION,// firmware version
- FIRMWARE_VERSION_OFFSET,
- DEFAULT_DEVICE_ID,// default device id
- DEVICE_ID_OFFSET,
- DEFAULT_BAUDRATE,// default baudrate
- BAUDRATE_OFFSET,
- DEFAULT_RETURN_DELAY,// return delay time
- RETURN_DELAY_OFFSET,
- DEFAULT_MM_PERIOD&0xFF,
- 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};// return level
+uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, 0xFFFF,
+ DEFAULT_DEVICE_MODEL&0xFF, DEVICE_MODEL_OFFSET,// model number LSB
+ DEFAULT_DEVICE_MODEL>>8, DEVICE_MODEL_OFFSET+1,// model number MSB
+ DEFAULT_FIRMWARE_VERSION, FIRMWARE_VERSION_OFFSET,// firmware version
+ DEFAULT_DEVICE_ID, DEVICE_ID_OFFSET,// default device id
+ DEFAULT_BAUDRATE, BAUDRATE_OFFSET,// default baudrate
+ DEFAULT_RETURN_DELAY, RETURN_DELAY_OFFSET,// return delay time
+ DEFAULT_MM_PERIOD&0xFF, 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,// return level
+ DEFAULT_SERVO0_OFFSET, MM_SERVO0_OFFSET,
+ DEFAULT_SERVO1_OFFSET, MM_SERVO1_OFFSET,
+ DEFAULT_SERVO2_OFFSET, MM_SERVO2_OFFSET,
+ DEFAULT_SERVO3_OFFSET, MM_SERVO3_OFFSET,
+ DEFAULT_SERVO4_OFFSET, MM_SERVO4_OFFSET,
+ DEFAULT_SERVO5_OFFSET, MM_SERVO5_OFFSET,
+ DEFAULT_SERVO6_OFFSET, MM_SERVO6_OFFSET,
+ DEFAULT_SERVO7_OFFSET, MM_SERVO7_OFFSET,
+ DEFAULT_SERVO8_OFFSET, MM_SERVO8_OFFSET,
+ DEFAULT_SERVO9_OFFSET, MM_SERVO9_OFFSET,
+ DEFAULT_SERVO10_OFFSET, MM_SERVO10_OFFSET,
+ DEFAULT_SERVO11_OFFSET, MM_SERVO11_OFFSET,
+ DEFAULT_SERVO12_OFFSET, MM_SERVO12_OFFSET,
+ DEFAULT_SERVO13_OFFSET, MM_SERVO13_OFFSET,
+ DEFAULT_SERVO14_OFFSET, MM_SERVO14_OFFSET,
+ DEFAULT_SERVO15_OFFSET, MM_SERVO15_OFFSET,
+ DEFAULT_SERVO16_OFFSET, MM_SERVO16_OFFSET,
+ DEFAULT_SERVO17_OFFSET, MM_SERVO17_OFFSET,
+ DEFAULT_SERVO18_OFFSET, MM_SERVO18_OFFSET,
+ DEFAULT_SERVO19_OFFSET, MM_SERVO19_OFFSET,
+ DEFAULT_SERVO20_OFFSET, MM_SERVO20_OFFSET,
+ DEFAULT_SERVO21_OFFSET, MM_SERVO21_OFFSET,
+ DEFAULT_SERVO22_OFFSET, MM_SERVO22_OFFSET,
+ DEFAULT_SERVO23_OFFSET, MM_SERVO23_OFFSET,
+ DEFAULT_SERVO24_OFFSET, MM_SERVO24_OFFSET,
+ DEFAULT_SERVO25_OFFSET, MM_SERVO25_OFFSET,
+ DEFAULT_SERVO26_OFFSET, MM_SERVO26_OFFSET,
+ DEFAULT_SERVO27_OFFSET, MM_SERVO27_OFFSET,
+ DEFAULT_SERVO28_OFFSET, MM_SERVO28_OFFSET,
+ DEFAULT_SERVO29_OFFSET, MM_SERVO29_OFFSET,
+ DEFAULT_SERVO30_OFFSET, MM_SERVO30_OFFSET,
+ DEFAULT_SERVO31_OFFSET, MM_SERVO31_OFFSET,
+ DEFAULT_GYRO_FB_ADC_CH, GYRO_FB_ADC_CH_OFFSET,
+ DEFAULT_GYRO_LR_ADC_CH, GYRO_LR_ADC_CH_OFFSET};
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 3b5a18de1a8d52dcb93542eb0392f8deb00a44ba..86b5b751cb7b1a5bbfd42ca5b5357bdbbdd3bec9 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -5,6 +5,7 @@
#include "action.h"
#include "adc_dma.h"
#include "bioloid_gyro.h"
+#include "gpio.h"
#define MANAGER_TIMER TIM3
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM3_CLK_ENABLE()
@@ -34,13 +35,16 @@ void manager_send_motion_command(void)
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{
- if(manager_servos[i].enabled && manager_servos[i].model!=0x0000)
- {
- servo_ids[num]=manager_servos[i].id;
- manager_servos[i].cw_comp=(1<<(manager_current_slopes[i]&0x0F));
- manager_servos[i].ccw_comp=(1<<((manager_current_slopes[i]&0xF0)>>4));
- write_data[num].data_addr=(uint8_t *)&(manager_servos[i].cw_comp);
- num++;
+ if(manager_servos[i].enabled)
+ {
+ if(manager_servos[i].model==SERVO_AX12A)
+ {
+ servo_ids[num]=manager_servos[i].id;
+ manager_servos[i].cw_comp=(1<<(manager_current_slopes[i]&0x0F));
+ manager_servos[i].ccw_comp=(1<<((manager_current_slopes[i]&0xF0)>>4));
+ write_data[num].data_addr=(uint8_t *)&(manager_servos[i].cw_comp);
+ num++;
+ }
}
}
if(num>0)
@@ -183,7 +187,6 @@ void manager_init(uint16_t period_us)
// enable power to the servos
bioloid_dyn_master_servos_enable_power();
HAL_Delay(1000);
-
// detect the servos connected
dyn_master_scan(&bioloid_dyn_master_servos,&num,servo_ids);
ram_data[BIOLOID_MM_NUM_SERVOS]=num;
@@ -320,6 +323,7 @@ void manager_init(uint16_t period_us)
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
manager_balance_offset[i]=0;
manager_balance_enabled=0x00;
+ gpio_set_led(TXD_LED);
/* initialize action module */
action_init(period_us);
@@ -464,7 +468,8 @@ 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,MANAGER_EEPROM_LENGTH,address,length))
+ ram_in_window(BIOLOID_MM_PERIOD_L,BIOLOID_MM_BAL_HIP_ROLL_GAIN_H,address,length) ||
+ ram_in_window(BIOLOID_MM_SERVO0_OFFSET,BIOLOID_MM_SERVO31_OFFSET,address,length))
return 0x01;
else
return 0x00;
@@ -472,8 +477,8 @@ uint8_t manager_in_range(unsigned short int address, unsigned short int length)
void manager_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
- uint16_t word_value;
- uint8_t byte_value,module,i,j;
+ uint16_t word_value,i,j;
+ uint8_t byte_value,module;
if(ram_in_range(BIOLOID_MM_PERIOD_L,address,length) && ram_in_range(BIOLOID_MM_PERIOD_H,address,length))
{
diff --git a/src/ram.c b/src/ram.c
index 046ecab0f6a13b1c3644b929e768cb68a7fde03e..45069ef7f06a94cc647195685d5b76ea480a55bf 100644
--- a/src/ram.c
+++ b/src/ram.c
@@ -15,13 +15,13 @@ void ram_init(void)
if(EE_ReadVariable(DEVICE_MODEL_OFFSET+1,&eeprom_data)==0)
ram_data[DEVICE_MODEL_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(FIRMWARE_VERSION_OFFSET,&eeprom_data)==0)
- ram_data[FIRMWARE_VERSION_OFFSET]=(uint8_t)eeprom_data;
+ ram_data[FIRMWARE_VERSION_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data)==0)
- ram_data[DEVICE_ID_OFFSET]=(uint8_t)eeprom_data;
+ ram_data[DEVICE_ID_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(BAUDRATE_OFFSET,&eeprom_data)==0)
- ram_data[BAUDRATE_OFFSET]=(uint8_t)eeprom_data;
+ ram_data[BAUDRATE_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data)==0)
- ram_data[RETURN_DELAY_OFFSET]=(uint8_t)eeprom_data;
+ ram_data[RETURN_DELAY_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data)==0)
ram_data[MM_PERIOD_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0)
@@ -43,27 +43,32 @@ void ram_init(void)
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;
+ ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ for(i=0;i<32;i++)
+ {
+ if(EE_ReadVariable(MM_SERVO0_OFFSET+i,&eeprom_data)==0)
+ ram_data[BIOLOID_MM_SERVO0_OFFSET+i]=(uint8_t)(eeprom_data&0x00FF);
+ }
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)
+inline void ram_read_byte(uint16_t address,uint8_t *data)
{
(*data)=ram_data[address];
}
-inline void ram_read_word(uint8_t address, uint16_t *data)
+inline void ram_read_word(uint16_t address, uint16_t *data)
{
(*data)=ram_data[address];
(*data)+=ram_data[address+1]*256;
}
-uint8_t ram_read_table(uint8_t address, uint8_t length,uint8_t *data)
+uint8_t ram_read_table(uint16_t address, uint16_t length,uint8_t *data)
{
- uint8_t i;
+ uint16_t i;
if((address+length)<=(RAM_SIZE-1))
{
@@ -75,7 +80,7 @@ uint8_t ram_read_table(uint8_t address, uint8_t length,uint8_t *data)
return RAM_BAD_ADDRESS;
}
-uint8_t ram_set_bit(uint8_t address, uint8_t bit)
+uint8_t ram_set_bit(uint16_t address, uint8_t bit)
{
if(bit>=0 && bit<8)
{
@@ -86,7 +91,7 @@ uint8_t ram_set_bit(uint8_t address, uint8_t bit)
return RAM_BAD_BIT;
}
-uint8_t ram_clear_bit(uint8_t address, uint8_t bit)
+uint8_t ram_clear_bit(uint16_t address, uint8_t bit)
{
if(bit>=0 && bit<8)
{
@@ -97,14 +102,14 @@ uint8_t ram_clear_bit(uint8_t address, uint8_t bit)
return RAM_BAD_BIT;
}
-uint8_t ram_write_byte(uint8_t address, uint8_t data)
+uint8_t ram_write_byte(uint16_t address, uint8_t data)
{
ram_data[address]=data;
return RAM_SUCCESS;
}
-uint8_t ram_write_word(uint8_t address, uint16_t data)
+uint8_t ram_write_word(uint16_t address, uint16_t data)
{
if(address < (RAM_SIZE-1))
{
@@ -117,9 +122,9 @@ uint8_t ram_write_word(uint8_t address, uint16_t data)
return RAM_BAD_ADDRESS;
}
-uint8_t ram_write_table(uint8_t address, uint8_t length,uint8_t *data)
+uint8_t ram_write_table(uint16_t address, uint16_t length,uint8_t *data)
{
- uint8_t i;
+ uint16_t i;
if((address+length)<RAM_SIZE)
{
@@ -131,7 +136,7 @@ uint8_t ram_write_table(uint8_t address, uint8_t length,uint8_t *data)
return RAM_BAD_ADDRESS;
}
-inline uint8_t ram_in_range(uint8_t reg,uint8_t address,uint8_t length)
+inline uint8_t ram_in_range(uint16_t reg,uint16_t address,uint16_t length)
{
if(reg>=address && reg<(address+length))
return 0x01;
@@ -139,10 +144,10 @@ inline uint8_t ram_in_range(uint8_t reg,uint8_t address,uint8_t length)
return 0x00;
}
-uint8_t ram_in_window(uint8_t start_reg,uint8_t reg_length,uint8_t start_address,uint8_t address_length)
+uint8_t ram_in_window(uint16_t start_reg,uint16_t reg_length,uint16_t start_address,uint16_t address_length)
{
- uint8_t end_reg=start_reg+reg_length;
- uint8_t end_address=start_address+address_length;
+ uint16_t end_reg=start_reg+reg_length;
+ uint16_t end_address=start_address+address_length;
if((start_reg>=start_address && start_reg<end_address) || (end_reg>=start_address && end_reg<end_address) ||
(start_address>=start_reg && start_address<end_reg) || (end_address>=start_reg && end_address<end_reg))