Commit 65a93f73 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved some minor bugs related to the balance feature.

parent 583e3ac3
......@@ -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
}
......
......@@ -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
{
......
......@@ -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);
......
......@@ -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 ---------------------------------------------------------*/
......
......@@ -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