diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index ae7cf2eb5c534f21661a3268244109854deb6726..4ed272219dcbf6ba48627d0819c094e06e040c92 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -88,106 +88,106 @@ extern "C" {
#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
typedef enum {
- DARWIN_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET,
- DARWIN_MODEL_NUMBER_H = DEVICE_MODEL_OFFSET+1,
- DARWIN_VERSION = FIRMWARE_VERSION_OFFSET,
- DARWIN_ID = DEVICE_ID_OFFSET,
- DARWIN_BAUD_RATE = BAUDRATE_OFFSET,
- DARWIN_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET,
- DARWIN_MM_PERIOD_L = MM_PERIOD_OFFSET,
- DARWIN_MM_PERIOD_H = MM_PERIOD_OFFSET+1,
- DARWIN_MM_BAL_KNEE_GAIN_L = MM_BAL_KNEE_GAIN_OFFSET,// fixed point format 0|16
- DARWIN_MM_BAL_KNEE_GAIN_H = MM_BAL_KNEE_GAIN_OFFSET+1,
- DARWIN_MM_BAL_ANKLE_ROLL_GAIN_L = MM_BAL_ANKLE_ROLL_GAIN_OFFSET,// fixed point format 0|16
- DARWIN_MM_BAL_ANKLE_ROLL_GAIN_H = MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
- DARWIN_MM_BAL_ANKLE_PITCH_GAIN_L = MM_BAL_ANKLE_PITCH_GAIN_OFFSET,// fixed point format 0|16
- DARWIN_MM_BAL_ANKLE_PITCH_GAIN_H = MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
- DARWIN_MM_BAL_HIP_ROLL_GAIN_L = MM_BAL_HIP_ROLL_GAIN_OFFSET,// fixed point format 0|16
- DARWIN_MM_BAL_HIP_ROLL_GAIN_H = MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
- DARWIN_RETURN_LEVEL = RETURN_LEVEL_OFFSET,
- DARWIN_MM_SERVO0_OFFSET = MM_SERVO0_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO1_OFFSET = MM_SERVO1_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO2_OFFSET = MM_SERVO2_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO3_OFFSET = MM_SERVO3_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO4_OFFSET = MM_SERVO4_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO5_OFFSET = MM_SERVO5_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO6_OFFSET = MM_SERVO6_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO7_OFFSET = MM_SERVO7_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO8_OFFSET = MM_SERVO8_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO9_OFFSET = MM_SERVO9_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO10_OFFSET = MM_SERVO10_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO11_OFFSET = MM_SERVO11_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO12_OFFSET = MM_SERVO12_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO13_OFFSET = MM_SERVO13_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO14_OFFSET = MM_SERVO14_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO15_OFFSET = MM_SERVO15_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO16_OFFSET = MM_SERVO16_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO17_OFFSET = MM_SERVO17_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO18_OFFSET = MM_SERVO18_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO19_OFFSET = MM_SERVO19_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO20_OFFSET = MM_SERVO20_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO21_OFFSET = MM_SERVO21_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO22_OFFSET = MM_SERVO22_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO23_OFFSET = MM_SERVO23_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO24_OFFSET = MM_SERVO24_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO25_OFFSET = MM_SERVO25_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO26_OFFSET = MM_SERVO26_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO27_OFFSET = MM_SERVO27_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO28_OFFSET = MM_SERVO28_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO29_OFFSET = MM_SERVO29_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO30_OFFSET = MM_SERVO30_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_MM_SERVO31_OFFSET = MM_SERVO31_OFFSET, // angle offset in fixed point format 1-3|4
- DARWIN_WALK_X_OFFSET = WALK_X_OFFSET,
- DARWIN_WALK_Y_OFFSET = WALK_Y_OFFSET,
- DARWIN_WALK_Z_OFFSET = WALK_Z_OFFSET,
- DARWIN_WALK_ROLL_OFFSET = WALK_ROLL_OFFSET,
- DARWIN_WALK_PITCH_OFFSET = WALK_PITCH_OFFSET,
- DARWIN_WALK_YAW_OFFSET = WALK_YAW_OFFSET,
- DARWIN_WALK_HIP_PITCH_OFF_L = WALK_HIP_PITCH_OFF,
- DARWIN_WALK_HIP_PITCH_OFF_H = WALK_HIP_PITCH_OFF+1,
- DARWIN_WALK_PERIOD_TIME_L = WALK_PERIOD_TIME,
- DARWIN_WALK_PERIOD_TIME_H = WALK_PERIOD_TIME+1,
- DARWIN_WALK_DSP_RATIO = WALK_DSP_RATIO,
- DARWIN_WALK_STEP_FW_BW_RATIO = WALK_STEP_FW_BW_RATIO,
- DARWIN_WALK_FOOT_HEIGHT = WALK_FOOT_HEIGHT,
- DARWIN_WALK_SWING_RIGHT_LEFT = WALK_SWING_RIGHT_LEFT,
- DARWIN_WALK_SWING_TOP_DOWN = WALK_SWING_TOP_DOWN,
- DARWIN_WALK_PELVIS_OFFSET = WALK_PELVIS_OFFSET,
- DARWIN_WALK_ARM_SWING_GAIN = WALK_ARM_SWING_GAIN,
- DARWIN_WALK_MAX_VEL = WALK_MAX_VEL,
- DARWIN_WALK_MAX_ROT_VEL = WALK_MAX_ROT_VEL,
- DARWIN_HEAD_PAN_P_L = HEAD_PAN_P,// constant in fixed point format 0|16
- DARWIN_HEAD_PAN_P_H = HEAD_PAN_P+1,
- DARWIN_HEAD_PAN_I_L = HEAD_PAN_I,// constant in fixed point format 0|16
- DARWIN_HEAD_PAN_I_H = HEAD_PAN_I+1,
- DARWIN_HEAD_PAN_D_L = HEAD_PAN_D,// constant in fixed point format 0|16
- DARWIN_HEAD_PAN_D_H = HEAD_PAN_D+1,
- DARWIN_HEAD_PAN_I_CLAMP_L = HEAD_PAN_I_CLAMP,// max error in fixed point format 9|7
- DARWIN_HEAD_PAN_I_CLAMP_H = HEAD_PAN_I_CLAMP+1,
- DARWIN_HEAD_TILT_P_L = HEAD_TILT_P,// constant in fixed point format 0|16
- DARWIN_HEAD_TILT_P_H = HEAD_TILT_P+1,
- DARWIN_HEAD_TILT_I_L = HEAD_TILT_I,// constant in fixed point format 0|16
- DARWIN_HEAD_TILT_I_H = HEAD_TILT_I+1,
- DARWIN_HEAD_TILT_D_L = HEAD_TILT_D,// constant in fixed point format 0|16
- DARWIN_HEAD_TILT_D_H = HEAD_TILT_D+1,
- DARWIN_HEAD_TILT_I_CLAMP_L = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7
- DARWIN_HEAD_TILT_I_CLAMP_H = HEAD_TILT_I_CLAMP+1,
- DARWIN_GRIPPER_LEFT_ID = GRIPPER_LEFT_ID,
- DARWIN_GRIPPER_LEFT_MAX_ANGLE_L = GRIPPER_LEFT_MAX_ANGLE,
- DARWIN_GRIPPER_LEFT_MAX_ANGLE_H = GRIPPER_LEFT_MAX_ANGLE+1,
- DARWIN_GRIPPER_LEFT_MIN_ANGLE_L = GRIPPER_LEFT_MIN_ANGLE,
- DARWIN_GRIPPER_LEFT_MIN_ANGLE_H = GRIPPER_LEFT_MIN_ANGLE+1,
- DARWIN_GRIPPER_LEFT_MAX_FORCE_L = GRIPPER_LEFT_MAX_FORCE,
- DARWIN_GRIPPER_LEFT_MAX_FORCE_H = GRIPPER_LEFT_MAX_FORCE+1,
- DARWIN_GRIPPER_RIGHT_ID = GRIPPER_RIGHT_ID,
- DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L = GRIPPER_RIGHT_MAX_ANGLE,
- DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H = GRIPPER_RIGHT_MAX_ANGLE+1,
- DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L = GRIPPER_RIGHT_MIN_ANGLE,
- DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H = GRIPPER_RIGHT_MIN_ANGLE+1,
- DARWIN_GRIPPER_RIGHT_MAX_FORCE_L = GRIPPER_RIGHT_MAX_FORCE,
- DARWIN_GRIPPER_RIGHT_MAX_FORCE_H = GRIPPER_RIGHT_MAX_FORCE+1,
- DARWIN_SMART_CHARGER_PERIOD_L = SMART_CHARGER_PERIOD, //en ms
- DARWIN_SMART_CHARGER_PERIOD_H = SMART_CHARGER_PERIOD+1,
+ DARWIN_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET,
+ DARWIN_MODEL_NUMBER_H = DEVICE_MODEL_OFFSET+1,
+ DARWIN_VERSION = FIRMWARE_VERSION_OFFSET,
+ DARWIN_ID = DEVICE_ID_OFFSET,
+ DARWIN_BAUD_RATE = BAUDRATE_OFFSET,
+ DARWIN_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET,
+ DARWIN_MM_PERIOD_L = MM_PERIOD_OFFSET,
+ DARWIN_MM_PERIOD_H = MM_PERIOD_OFFSET+1,
+ DARWIN_MM_BAL_KNEE_GAIN_L = MM_BAL_KNEE_GAIN_OFFSET,// fixed point format 0|16
+ DARWIN_MM_BAL_KNEE_GAIN_H = MM_BAL_KNEE_GAIN_OFFSET+1,
+ DARWIN_MM_BAL_ANKLE_ROLL_GAIN_L = MM_BAL_ANKLE_ROLL_GAIN_OFFSET,// fixed point format 0|16
+ DARWIN_MM_BAL_ANKLE_ROLL_GAIN_H = MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
+ DARWIN_MM_BAL_ANKLE_PITCH_GAIN_L = MM_BAL_ANKLE_PITCH_GAIN_OFFSET,// fixed point format 0|16
+ DARWIN_MM_BAL_ANKLE_PITCH_GAIN_H = MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
+ DARWIN_MM_BAL_HIP_ROLL_GAIN_L = MM_BAL_HIP_ROLL_GAIN_OFFSET,// fixed point format 0|16
+ DARWIN_MM_BAL_HIP_ROLL_GAIN_H = MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
+ DARWIN_RETURN_LEVEL = RETURN_LEVEL_OFFSET,
+ DARWIN_MM_SERVO0_OFFSET = MM_SERVO0_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO1_OFFSET = MM_SERVO1_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO2_OFFSET = MM_SERVO2_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO3_OFFSET = MM_SERVO3_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO4_OFFSET = MM_SERVO4_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO5_OFFSET = MM_SERVO5_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO6_OFFSET = MM_SERVO6_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO7_OFFSET = MM_SERVO7_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO8_OFFSET = MM_SERVO8_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO9_OFFSET = MM_SERVO9_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO10_OFFSET = MM_SERVO10_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO11_OFFSET = MM_SERVO11_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO12_OFFSET = MM_SERVO12_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO13_OFFSET = MM_SERVO13_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO14_OFFSET = MM_SERVO14_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO15_OFFSET = MM_SERVO15_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO16_OFFSET = MM_SERVO16_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO17_OFFSET = MM_SERVO17_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO18_OFFSET = MM_SERVO18_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO19_OFFSET = MM_SERVO19_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO20_OFFSET = MM_SERVO20_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO21_OFFSET = MM_SERVO21_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO22_OFFSET = MM_SERVO22_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO23_OFFSET = MM_SERVO23_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO24_OFFSET = MM_SERVO24_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO25_OFFSET = MM_SERVO25_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO26_OFFSET = MM_SERVO26_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO27_OFFSET = MM_SERVO27_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO28_OFFSET = MM_SERVO28_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO29_OFFSET = MM_SERVO29_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO30_OFFSET = MM_SERVO30_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_MM_SERVO31_OFFSET = MM_SERVO31_OFFSET, // angle offset in fixed point format 1-3|4
+ DARWIN_WALK_X_OFFSET = WALK_X_OFFSET,
+ DARWIN_WALK_Y_OFFSET = WALK_Y_OFFSET,
+ DARWIN_WALK_Z_OFFSET = WALK_Z_OFFSET,
+ DARWIN_WALK_ROLL_OFFSET = WALK_ROLL_OFFSET,
+ DARWIN_WALK_PITCH_OFFSET = WALK_PITCH_OFFSET,
+ DARWIN_WALK_YAW_OFFSET = WALK_YAW_OFFSET,
+ DARWIN_WALK_HIP_PITCH_OFF_L = WALK_HIP_PITCH_OFF,
+ DARWIN_WALK_HIP_PITCH_OFF_H = WALK_HIP_PITCH_OFF+1,
+ DARWIN_WALK_PERIOD_TIME_L = WALK_PERIOD_TIME,
+ DARWIN_WALK_PERIOD_TIME_H = WALK_PERIOD_TIME+1,
+ DARWIN_WALK_DSP_RATIO = WALK_DSP_RATIO,
+ DARWIN_WALK_STEP_FW_BW_RATIO = WALK_STEP_FW_BW_RATIO,
+ DARWIN_WALK_FOOT_HEIGHT = WALK_FOOT_HEIGHT,
+ DARWIN_WALK_SWING_RIGHT_LEFT = WALK_SWING_RIGHT_LEFT,
+ DARWIN_WALK_SWING_TOP_DOWN = WALK_SWING_TOP_DOWN,
+ DARWIN_WALK_PELVIS_OFFSET = WALK_PELVIS_OFFSET,
+ DARWIN_WALK_ARM_SWING_GAIN = WALK_ARM_SWING_GAIN,
+ DARWIN_WALK_MAX_VEL = WALK_MAX_VEL,
+ DARWIN_WALK_MAX_ROT_VEL = WALK_MAX_ROT_VEL,
+ DARWIN_HEAD_PAN_P_L = HEAD_PAN_P,// constant in fixed point format 0|16
+ DARWIN_HEAD_PAN_P_H = HEAD_PAN_P+1,
+ DARWIN_HEAD_PAN_I_L = HEAD_PAN_I,// constant in fixed point format 0|16
+ DARWIN_HEAD_PAN_I_H = HEAD_PAN_I+1,
+ DARWIN_HEAD_PAN_D_L = HEAD_PAN_D,// constant in fixed point format 0|16
+ DARWIN_HEAD_PAN_D_H = HEAD_PAN_D+1,
+ DARWIN_HEAD_PAN_I_CLAMP_L = HEAD_PAN_I_CLAMP,// max error in fixed point format 9|7
+ DARWIN_HEAD_PAN_I_CLAMP_H = HEAD_PAN_I_CLAMP+1,
+ DARWIN_HEAD_TILT_P_L = HEAD_TILT_P,// constant in fixed point format 0|16
+ DARWIN_HEAD_TILT_P_H = HEAD_TILT_P+1,
+ DARWIN_HEAD_TILT_I_L = HEAD_TILT_I,// constant in fixed point format 0|16
+ DARWIN_HEAD_TILT_I_H = HEAD_TILT_I+1,
+ DARWIN_HEAD_TILT_D_L = HEAD_TILT_D,// constant in fixed point format 0|16
+ DARWIN_HEAD_TILT_D_H = HEAD_TILT_D+1,
+ DARWIN_HEAD_TILT_I_CLAMP_L = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7
+ DARWIN_HEAD_TILT_I_CLAMP_H = HEAD_TILT_I_CLAMP+1,
+ DARWIN_GRIPPER_LEFT_ID = GRIPPER_LEFT_ID,
+ DARWIN_GRIPPER_LEFT_MAX_ANGLE_L = GRIPPER_LEFT_MAX_ANGLE,
+ DARWIN_GRIPPER_LEFT_MAX_ANGLE_H = GRIPPER_LEFT_MAX_ANGLE+1,
+ DARWIN_GRIPPER_LEFT_MIN_ANGLE_L = GRIPPER_LEFT_MIN_ANGLE,
+ DARWIN_GRIPPER_LEFT_MIN_ANGLE_H = GRIPPER_LEFT_MIN_ANGLE+1,
+ DARWIN_GRIPPER_LEFT_MAX_FORCE_L = GRIPPER_LEFT_MAX_FORCE,
+ DARWIN_GRIPPER_LEFT_MAX_FORCE_H = GRIPPER_LEFT_MAX_FORCE+1,
+ DARWIN_GRIPPER_RIGHT_ID = GRIPPER_RIGHT_ID,
+ DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L = GRIPPER_RIGHT_MAX_ANGLE,
+ DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H = GRIPPER_RIGHT_MAX_ANGLE+1,
+ DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L = GRIPPER_RIGHT_MIN_ANGLE,
+ DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H = GRIPPER_RIGHT_MIN_ANGLE+1,
+ DARWIN_GRIPPER_RIGHT_MAX_FORCE_L = GRIPPER_RIGHT_MAX_FORCE,
+ DARWIN_GRIPPER_RIGHT_MAX_FORCE_H = GRIPPER_RIGHT_MAX_FORCE+1,
+ DARWIN_SMART_CHARGER_PERIOD_L = SMART_CHARGER_PERIOD, //en ms
+ DARWIN_SMART_CHARGER_PERIOD_H = SMART_CHARGER_PERIOD+1,
//RAM
DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | blink | toggle | value | internally used
@@ -673,7 +673,7 @@ typedef enum {
#define GRIPPER_CLOSE_LEFT 0x08
#define SMART_CHARGER_BASE_ADDRESS 0x0247
-#define SMART_CHARGER_MEM_LENGTH 10
+#define SMART_CHARGER_MEM_LENGTH 11
#define SMART_CHARGER_EEPROM_BASE 0x0062
#define SMART_CHARGER_EEPROM_LENGTH 2
#define SMART_CHARGER_DET 0x01
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index 7ba89ed6261ee183c407170d0a0fff7e1a2587ab..dafe22333ba7cca14136c3498c54dc9a1905e9b5 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -5,84 +5,83 @@
extern "C" {
#endif
-#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 0x1E78 //7800us
-#define DEFAULT_BAL_KNEE_GAIN 0x4CCD // 0.3 in fixed point format 0|16
-#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0xFFFF // 0.99999
-#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0xE666 // 0.9
-#define DEFAULT_BAL_HIP_ROLL_GAIN 0x8000 // 0.5
-#define DEFAULT_RETURN_LEVEL 0x0002
-#define DEFAULT_SERVO0_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO1_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO2_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO3_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO4_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO5_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO6_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO7_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO8_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO9_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO10_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO11_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO12_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO13_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO14_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO15_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO16_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO17_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO18_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO19_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO20_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO21_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO22_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO23_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO24_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO25_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO26_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO27_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO28_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO29_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO30_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_SERVO31_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define DEFAULT_WALK_X_OFFSET 0xFFF6 // -10 mm
-#define DEFAULT_WALK_Y_OFFSET 0x0005 // 5 mm
-#define DEFAULT_WALK_Z_OFFSET 0x0014 // 20 mm
-#define DEFAULT_WALK_ROLL_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
-#define DEFAULT_WALK_PITCH_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
-#define DEFAULT_WALK_YAW_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
-#define DEFAULT_WALK_HIP_PITCH_OFF 0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10
-#define DEFAULT_WALK_PERIOD_TIME 0x0258 // 600 ms
-#define DEFAULT_WALK_DSP_RATIO 0x0019 // 0.1 in fixed point format 0 | 8
-#define DEFAULT_WALK_STEP_FW_BW_RATIO 0x004C // 0.3 in fixed point format 0 | 8
-#define DEFAULT_WALK_FOOT_HEIGHT 0x0028 // 40 mm
-#define DEFAULT_WALK_SWING_RIGHT_LEFT 0x0014 // 20 mm
-#define DEFAULT_WALK_SWING_TOP_DOWN 0x0005 // 5 mm
-#define DEFAULT_WALK_PELVIS_OFFSET 0x0018 // 3 degrees in fixed point format 5 (1+4) | 3
-#define DEFAULT_WALK_ARM_SWING_GAIN 0x0030 // 1.5 in fixed point format 3 | 5
-#define DEFAULT_WALK_MAX_VEL 0x0016 // 20 mm/s
-#define DEFAULT_WALK_MAX_ROT_VEL 0x0040 // 8 degrees/s in fixed point format 5 | 3
-#define DEFAULT_HEAD_PAN_P 0x028F // 0.01 in fixed point format 0|16
-#define DEFAULT_HEAD_PAN_I 0x0000 // 0.0 in fixed point format 0|16
-#define DEFAULT_HEAD_PAN_D 0x0000 // 0.0 in fixed point format 0|16
-#define DEFAULT_HEAD_PAN_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7
-#define DEFAULT_HEAD_TILT_P 0x028F // 0.01 in fixed point format 0|16
-#define DEFAULT_HEAD_TILT_I 0x0000 // 0.0 in fixed point format 0|16
-#define DEFAULT_HEAD_TILT_D 0x0000 // 0.0 in fixed point format 0|16
-#define DEFAULT_HEAD_TILT_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7
-#define DEFAULT_GRIPPER_LEFT_ID 0x0017 // ID 23 for the left gripper
-#define DEFAULT_GRIPPER_LEFT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7
-#define DEFAULT_GRIPPER_LEFT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
-#define DEFAULT_GRIPPER_LEFT_MAX_FORCE 0x03FF // 1023 max force in binary format
-#define DEFAULT_GRIPPER_RIGHT_ID 0x0018 // ID 24 for the left gripper
-#define DEFAULT_GRIPPER_RIGHT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7
-#define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
-#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x03FF // 1023 max force in binary format
-
-#define DEFAULT_SMART_CHARGER_PERIOD 0x05DC //1500 ms (7,8ms*200)
+#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 0x1E78 //7800us
+#define DEFAULT_BAL_KNEE_GAIN 0x4CCD // 0.3 in fixed point format 0|16
+#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0xFFFF // 0.99999
+#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0xE666 // 0.9
+#define DEFAULT_BAL_HIP_ROLL_GAIN 0x8000 // 0.5
+#define DEFAULT_RETURN_LEVEL 0x0002
+#define DEFAULT_SERVO0_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO1_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO2_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO3_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO4_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO5_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO6_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO7_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO8_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO9_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO10_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO11_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO12_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO13_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO14_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO15_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO16_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO17_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO18_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO19_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO20_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO21_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO22_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO23_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO24_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO25_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO26_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO27_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO28_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO29_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO30_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_SERVO31_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
+#define DEFAULT_WALK_X_OFFSET 0xFFF6 // -10 mm
+#define DEFAULT_WALK_Y_OFFSET 0x0005 // 5 mm
+#define DEFAULT_WALK_Z_OFFSET 0x0014 // 20 mm
+#define DEFAULT_WALK_ROLL_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_WALK_PITCH_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_WALK_YAW_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_WALK_HIP_PITCH_OFF 0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10
+#define DEFAULT_WALK_PERIOD_TIME 0x0258 // 600 ms
+#define DEFAULT_WALK_DSP_RATIO 0x0019 // 0.1 in fixed point format 0 | 8
+#define DEFAULT_WALK_STEP_FW_BW_RATIO 0x004C // 0.3 in fixed point format 0 | 8
+#define DEFAULT_WALK_FOOT_HEIGHT 0x0028 // 40 mm
+#define DEFAULT_WALK_SWING_RIGHT_LEFT 0x0014 // 20 mm
+#define DEFAULT_WALK_SWING_TOP_DOWN 0x0005 // 5 mm
+#define DEFAULT_WALK_PELVIS_OFFSET 0x0018 // 3 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_WALK_ARM_SWING_GAIN 0x0030 // 1.5 in fixed point format 3 | 5
+#define DEFAULT_WALK_MAX_VEL 0x0016 // 20 mm/s
+#define DEFAULT_WALK_MAX_ROT_VEL 0x0040 // 8 degrees/s in fixed point format 5 | 3
+#define DEFAULT_HEAD_PAN_P 0x028F // 0.01 in fixed point format 0|16
+#define DEFAULT_HEAD_PAN_I 0x0000 // 0.0 in fixed point format 0|16
+#define DEFAULT_HEAD_PAN_D 0x0000 // 0.0 in fixed point format 0|16
+#define DEFAULT_HEAD_PAN_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7
+#define DEFAULT_HEAD_TILT_P 0x028F // 0.01 in fixed point format 0|16
+#define DEFAULT_HEAD_TILT_I 0x0000 // 0.0 in fixed point format 0|16
+#define DEFAULT_HEAD_TILT_D 0x0000 // 0.0 in fixed point format 0|16
+#define DEFAULT_HEAD_TILT_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7
+#define DEFAULT_GRIPPER_LEFT_ID 0x0017 // ID 23 for the left gripper
+#define DEFAULT_GRIPPER_LEFT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7
+#define DEFAULT_GRIPPER_LEFT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
+#define DEFAULT_GRIPPER_LEFT_MAX_FORCE 0x03FF // 1023 max force in binary format
+#define DEFAULT_GRIPPER_RIGHT_ID 0x0018 // ID 24 for the left gripper
+#define DEFAULT_GRIPPER_RIGHT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7
+#define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
+#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x03FF // 1023 max force in binary format
+#define DEFAULT_SMART_CHARGER_PERIOD 0x05DC //1500 ms
diff --git a/src/eeprom.c b/src/eeprom.c
index d511af23b9cf8ebb04bf8cd314b10cfea15fc435..4fa6f8e2edc57ca67b5fee4864dcde49817fbf91 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -53,108 +53,108 @@
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, 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_WALK_X_OFFSET, WALK_X_OFFSET,
- DEFAULT_WALK_Y_OFFSET, WALK_Y_OFFSET,
- DEFAULT_WALK_Z_OFFSET, WALK_Z_OFFSET,
- DEFAULT_WALK_ROLL_OFFSET, WALK_ROLL_OFFSET,
- DEFAULT_WALK_PITCH_OFFSET, WALK_PITCH_OFFSET,
- DEFAULT_WALK_YAW_OFFSET, WALK_YAW_OFFSET,
- DEFAULT_WALK_HIP_PITCH_OFF&0xFF, WALK_HIP_PITCH_OFF,
- DEFAULT_WALK_HIP_PITCH_OFF>>8, WALK_HIP_PITCH_OFF+1,
- DEFAULT_WALK_PERIOD_TIME&0xFF, WALK_PERIOD_TIME,
- DEFAULT_WALK_PERIOD_TIME>>8, WALK_PERIOD_TIME+1,
- DEFAULT_WALK_DSP_RATIO, WALK_DSP_RATIO,
- DEFAULT_WALK_STEP_FW_BW_RATIO, WALK_STEP_FW_BW_RATIO,
- DEFAULT_WALK_FOOT_HEIGHT, WALK_FOOT_HEIGHT,
- DEFAULT_WALK_SWING_RIGHT_LEFT, WALK_SWING_RIGHT_LEFT,
- DEFAULT_WALK_SWING_TOP_DOWN, WALK_SWING_TOP_DOWN,
- DEFAULT_WALK_PELVIS_OFFSET, WALK_PELVIS_OFFSET,
- DEFAULT_WALK_ARM_SWING_GAIN, WALK_ARM_SWING_GAIN,
- DEFAULT_WALK_MAX_VEL, WALK_MAX_VEL,
- DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL,
- DEFAULT_HEAD_PAN_P&0xFF, HEAD_PAN_P,
- DEFAULT_HEAD_PAN_P>>8, HEAD_PAN_P+1,
- DEFAULT_HEAD_PAN_I&0xFF, HEAD_PAN_I,
- DEFAULT_HEAD_PAN_I>>8, HEAD_PAN_I+1,
- DEFAULT_HEAD_PAN_D&0xFF, HEAD_PAN_D,
- DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1,
- DEFAULT_HEAD_PAN_I_CLAMP&0xFF, HEAD_PAN_I_CLAMP,
- DEFAULT_HEAD_PAN_I_CLAMP>>8, HEAD_PAN_I_CLAMP+1,
- DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1,
- DEFAULT_HEAD_TILT_P&0xFF, HEAD_TILT_P,
- DEFAULT_HEAD_TILT_P>>8, HEAD_TILT_P+1,
- DEFAULT_HEAD_TILT_I&0xFF, HEAD_TILT_I,
- DEFAULT_HEAD_TILT_I>>8, HEAD_TILT_I+1,
- DEFAULT_HEAD_TILT_D&0xFF, HEAD_TILT_D,
- DEFAULT_HEAD_TILT_D>>8, HEAD_TILT_D+1,
- DEFAULT_HEAD_TILT_I_CLAMP&0xFF, HEAD_TILT_I_CLAMP,
- DEFAULT_HEAD_TILT_I_CLAMP>>8, HEAD_TILT_I_CLAMP+1,
- DEFAULT_GRIPPER_LEFT_ID, GRIPPER_LEFT_ID,
- DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF, GRIPPER_LEFT_MAX_ANGLE,
- DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8, GRIPPER_LEFT_MAX_ANGLE+1,
- DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF, GRIPPER_LEFT_MIN_ANGLE,
- DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8, GRIPPER_LEFT_MIN_ANGLE+1,
- DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF, GRIPPER_LEFT_MAX_FORCE,
- DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8, GRIPPER_LEFT_MAX_FORCE+1,
- DEFAULT_GRIPPER_RIGHT_ID, GRIPPER_RIGHT_ID,
- DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF, GRIPPER_RIGHT_MAX_ANGLE,
- DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8, GRIPPER_RIGHT_MAX_ANGLE+1,
- DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF, GRIPPER_RIGHT_MIN_ANGLE,
- DEFAULT_GRIPPER_RIGHT_MIN_ANGLE>>8, GRIPPER_RIGHT_MIN_ANGLE+1,
- DEFAULT_GRIPPER_RIGHT_MAX_FORCE&0xFF, GRIPPER_RIGHT_MAX_FORCE,
- DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8, GRIPPER_RIGHT_MAX_FORCE+1,
- DEFAULT_SMART_CHARGER_PERIOD&0xFF, SMART_CHARGER_PERIOD,
- DEFAULT_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1};
+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_WALK_X_OFFSET, WALK_X_OFFSET,
+ DEFAULT_WALK_Y_OFFSET, WALK_Y_OFFSET,
+ DEFAULT_WALK_Z_OFFSET, WALK_Z_OFFSET,
+ DEFAULT_WALK_ROLL_OFFSET, WALK_ROLL_OFFSET,
+ DEFAULT_WALK_PITCH_OFFSET, WALK_PITCH_OFFSET,
+ DEFAULT_WALK_YAW_OFFSET, WALK_YAW_OFFSET,
+ DEFAULT_WALK_HIP_PITCH_OFF&0xFF, WALK_HIP_PITCH_OFF,
+ DEFAULT_WALK_HIP_PITCH_OFF>>8, WALK_HIP_PITCH_OFF+1,
+ DEFAULT_WALK_PERIOD_TIME&0xFF, WALK_PERIOD_TIME,
+ DEFAULT_WALK_PERIOD_TIME>>8, WALK_PERIOD_TIME+1,
+ DEFAULT_WALK_DSP_RATIO, WALK_DSP_RATIO,
+ DEFAULT_WALK_STEP_FW_BW_RATIO, WALK_STEP_FW_BW_RATIO,
+ DEFAULT_WALK_FOOT_HEIGHT, WALK_FOOT_HEIGHT,
+ DEFAULT_WALK_SWING_RIGHT_LEFT, WALK_SWING_RIGHT_LEFT,
+ DEFAULT_WALK_SWING_TOP_DOWN, WALK_SWING_TOP_DOWN,
+ DEFAULT_WALK_PELVIS_OFFSET, WALK_PELVIS_OFFSET,
+ DEFAULT_WALK_ARM_SWING_GAIN, WALK_ARM_SWING_GAIN,
+ DEFAULT_WALK_MAX_VEL, WALK_MAX_VEL,
+ DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL,
+ DEFAULT_HEAD_PAN_P&0xFF, HEAD_PAN_P,
+ DEFAULT_HEAD_PAN_P>>8, HEAD_PAN_P+1,
+ DEFAULT_HEAD_PAN_I&0xFF, HEAD_PAN_I,
+ DEFAULT_HEAD_PAN_I>>8, HEAD_PAN_I+1,
+ DEFAULT_HEAD_PAN_D&0xFF, HEAD_PAN_D,
+ DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1,
+ DEFAULT_HEAD_PAN_I_CLAMP&0xFF, HEAD_PAN_I_CLAMP,
+ DEFAULT_HEAD_PAN_I_CLAMP>>8, HEAD_PAN_I_CLAMP+1,
+ DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1,
+ DEFAULT_HEAD_TILT_P&0xFF, HEAD_TILT_P,
+ DEFAULT_HEAD_TILT_P>>8, HEAD_TILT_P+1,
+ DEFAULT_HEAD_TILT_I&0xFF, HEAD_TILT_I,
+ DEFAULT_HEAD_TILT_I>>8, HEAD_TILT_I+1,
+ DEFAULT_HEAD_TILT_D&0xFF, HEAD_TILT_D,
+ DEFAULT_HEAD_TILT_D>>8, HEAD_TILT_D+1,
+ DEFAULT_HEAD_TILT_I_CLAMP&0xFF, HEAD_TILT_I_CLAMP,
+ DEFAULT_HEAD_TILT_I_CLAMP>>8, HEAD_TILT_I_CLAMP+1,
+ DEFAULT_GRIPPER_LEFT_ID, GRIPPER_LEFT_ID,
+ DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF, GRIPPER_LEFT_MAX_ANGLE,
+ DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8, GRIPPER_LEFT_MAX_ANGLE+1,
+ DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF, GRIPPER_LEFT_MIN_ANGLE,
+ DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8, GRIPPER_LEFT_MIN_ANGLE+1,
+ DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF, GRIPPER_LEFT_MAX_FORCE,
+ DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8, GRIPPER_LEFT_MAX_FORCE+1,
+ DEFAULT_GRIPPER_RIGHT_ID, GRIPPER_RIGHT_ID,
+ DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF, GRIPPER_RIGHT_MAX_ANGLE,
+ DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8, GRIPPER_RIGHT_MAX_ANGLE+1,
+ DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF, GRIPPER_RIGHT_MIN_ANGLE,
+ DEFAULT_GRIPPER_RIGHT_MIN_ANGLE>>8, GRIPPER_RIGHT_MIN_ANGLE+1,
+ DEFAULT_GRIPPER_RIGHT_MAX_FORCE&0xFF, GRIPPER_RIGHT_MAX_FORCE,
+ DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8, GRIPPER_RIGHT_MAX_FORCE+1,
+ DEFAULT_SMART_CHARGER_PERIOD&0xFF, SMART_CHARGER_PERIOD,
+ DEFAULT_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1};
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
diff --git a/src/motion_manager.c b/src/motion_manager.c
index eeb4d17016b66ad5f27457943a895d87629599f8..6e7768ab975482d88278925b992ee9e3de973a64 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -11,6 +11,7 @@
#include "imu.h"
#include "smart_charger.h"
#include "dyn_battery.h"
+#include "gpio.h"
#define MANAGER_TIMER TIM5
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM5_CLK_ENABLE()
@@ -332,7 +333,7 @@ void manager_init(uint16_t period_us)
smart_charger_set_master(&darwin_dyn_master); //set bus Dynamixel master version
// Set smart charger's memory map
dyn_master_read_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]);
- dyn_master_read_byte(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
+ dyn_master_write_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,2,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
break;
default: break;
}
@@ -389,12 +390,13 @@ void manager_init(uint16_t period_us)
dyn_master_read_word(&darwin_dyn_master_v2,servo_ids[i],BATTERY_MODEL_NUMBER_L,&model);
switch(model)
{
+ // gpio_set_led(LED_RX);
case DYN_BATTERY_MODEL: ram_data[DARWIN_SMART_CHARGER_CNTRL]=SMART_CHARGER_DET; //smart charger detected
ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i]; //smart charger ID
smart_charger_set_master(&darwin_dyn_master_v2); //Set bus Dynamixel master version
// Set smart charger's memory map
dyn_master_read_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]);
- dyn_master_read_byte(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
+ //dyn_master_write_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,2,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
break;
default: break;
}
diff --git a/src/ram.c b/src/ram.c
index f2c505dfb8b5e82c15e1311d9a27dad51bd60416..e061c05b1e42cf5b5247a9c9b61043bc74410a6d 100755
--- a/src/ram.c
+++ b/src/ram.c
@@ -151,6 +151,7 @@ void ram_init(void)
ram_data[SMART_CHARGER_PERIOD]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(SMART_CHARGER_PERIOD+1,&eeprom_data)==0)
ram_data[SMART_CHARGER_PERIOD+1]=(uint8_t)(eeprom_data&0x00FF);
+
}
inline void ram_read_byte(uint16_t address,uint8_t *data)
diff --git a/src/smart_charger.c b/src/smart_charger.c
index 3812797f57a5e046e4286bb189a5eca7c04bbc27..03a9ed8f31a5736ee6fee473b1a37b8f5388101b 100644
--- a/src/smart_charger.c
+++ b/src/smart_charger.c
@@ -9,18 +9,10 @@ uint8_t smart_charger_id; //smart charger dynamixel ID
uint8_t smart_charger_enabled; //smart charger module enable signal
uint8_t smart_charger_detected; //smart charger detect signal
uint16_t smart_charger_period; //smart charger period value in ms
-//uint16_t counter; //for read access operations
uint16_t smart_charger_count; //counter = sc period / mm period --> Default: (1500ms*1000)/7800us
uint8_t smart_charger_write; //write smart charger signal
uint8_t init_regs[6]={0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
-#define FIFOSIZE 32
-unsigned char *infifo; //Pointer to fifo position where to store next input data
-unsigned char *outfifo; //Pointer to fifo position where is stored the data to be written
-unsigned char numdata; //Number of elements stored in the fifo
-unsigned char write_fifo[FIFOSIZE]; //Vector with data to write (data_write)
-unsigned char current[2];
-
//---------------------------------------
TDynamixelMaster *dyn_battery_master;
@@ -31,12 +23,9 @@ void smart_charger_init(uint16_t period_us)
smart_charger_id = ram_data[DARWIN_SMART_CHARGER_ID];
smart_charger_enabled = ram_data[DARWIN_SMART_CHARGER_CNTRL]&SMART_CHARGER_EN;
smart_charger_detected = ram_data[DARWIN_SMART_CHARGER_CNTRL]&SMART_CHARGER_DET;
- // counter = 0;
- smart_charger_period = (ram_data[DARWIN_SMART_CHARGER_PERIOD_L]) + ((ram_data[DARWIN_SMART_CHARGER_PERIOD_H])<<8);
+ smart_charger_period = ram_data[DARWIN_SMART_CHARGER_PERIOD_L] + (ram_data[DARWIN_SMART_CHARGER_PERIOD_H]<<8);
smart_charger_count = (smart_charger_period*1000)/period_us;
smart_charger_write = 0x00;
- infifo=outfifo=&write_fifo[0];
- numdata=0; //fifo empty
ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
}
@@ -93,7 +82,7 @@ void smart_charger_process_read_cmd(unsigned short int address,unsigned short in
void smart_charger_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
- uint16_t period;//,i;
+ uint16_t period;
//Enable/Disable smart charger
@@ -114,28 +103,8 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
//Write Battery limit current
if(ram_in_range(DARWIN_SMART_CHARGER_LIMIT_CURRENT_L,address,length) && ram_in_range(DARWIN_SMART_CHARGER_LIMIT_CURRENT_H,address,length))
{
- current[0]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address];
- current[1]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address];
-
-
- /* if(infifo==&write_fifo[FIFOSIZE]);
- infifo=&write_fifo[0]; //go to first position of fifo
- if(numdata<FIFOSIZE) //free space in fifo
- {
- for(i=DARWIN_SMART_CHARGER_LIMIT_CURRENT_L;i<=DARWIN_SMART_CHARGER_LIMIT_CURRENT_H;i++)
- {
- *infifo=data[i-address];
- infifo++;
- numdata++;
- }
- */
- /* *infifo=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address]; //copy first byte to fifo
- numdata++;
- infifo++;
- *infifo=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address]; //copy second byte to fifo
- numdata++;
- infifo++; */
- // }
+ ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address];
+ ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address];
smart_charger_write = 0x01;
}
}
@@ -144,38 +113,27 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
// motion manager interface function
void smart_charger_process(void)
{
+ //gpio_set_led(LED_3);
uint8_t error;
- static uint16_t counter;
+ static uint16_t counter=0;
counter++;
//Write smart_charger - Battery limit current (EEPROM)
if(smart_charger_detected && smart_charger_enabled && smart_charger_write && counter!=smart_charger_count)
{
- error=dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2, current);
- if(error==DYN_SUCCESS){
-/* dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2,outfifo);
- if(outfifo==&write_fifo[FIFOSIZE-1])
- outfifo=&write_fifo[0]; //go to first position of fifo
- else
- outfifo=outfifo+2;
- numdata=numdata-2;
- if(numdata==0) //empty fifo
- */
+ error=dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
+ if(error==DYN_SUCCESS)
smart_charger_write = 0x00;
- }
}
//Read smart charger - time to empty, time to full and battery_status
if(smart_charger_detected && smart_charger_enabled && counter==smart_charger_count)
{
-
error=dyn_master_read_table(dyn_battery_master,smart_charger_id,BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]);
if(error!=DYN_SUCCESS){
+ // gpio_set_led(LED_TX);
ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
- gpio_set_led(LED_RX); //naranja
- }else{
- gpio_set_led(LED_3);//verde
}
counter = 0;
}