diff --git a/Makefile b/Makefile
index c31048f2ab1f85686d2f3b5e12a5990b0b5a87cc..ff8179a385e59ef6f8a485934f7d50bdfb7d59a5 100755
--- a/Makefile
+++ b/Makefile
@@ -29,6 +29,7 @@ TARGET_FILES+=src/joint_motion.c
TARGET_FILES+=src/head_tracking.c
TARGET_FILES+=src/grippers.c
TARGET_FILES+=src/smart_charger.c
+TARGET_FILES+=src/stairs.c
TARGET_PROCESSOR=STM32F103RE
@@ -94,7 +95,7 @@ TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal.c
TARGET_FILES+=$(USART_PATH)/src/usart3.c
TARGET_FILES+=$(USART_PATH)/src/usart2.c
-TARGET_FILES+=$(USART_PATH)/src/usart1.c
+TARGET_FILES+=$(USART_PATH)/src/usart1_remap.c
DARWIN_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
DARWIN_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(DARWIN_OBJS_TMP))
diff --git a/include/adc_dma.h b/include/adc_dma.h
index 0fd1dc41c7b437f8177404d968c381193d708393..db29c8d33f3945d9ff928d1d50cc85c48f5db6fc 100755
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -10,7 +10,7 @@ extern "C" {
#define ADC_NUM_CHANNELS 12
typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH7=6,ADC_CH8=7,
- ADC_CH9=8,ADC_CH10=9,ADC_CH12=10,ADC_CH14=11} adc_ch_t;
+ ADC_CH9=8,ADC_CH10=9,ADC_CH12=10} adc_ch_t;
void adc_init(void);
void adc_start(void);
diff --git a/include/darwin_dyn_master_v2.h b/include/darwin_dyn_master_v2.h
index b4a1b34d01c20f6e411ad3902e9d86682697c99c..a7e5d190f4dc6a65dd022c5c8d2a59656915501f 100644
--- a/include/darwin_dyn_master_v2.h
+++ b/include/darwin_dyn_master_v2.h
@@ -12,6 +12,8 @@ extern "C" {
extern TDynamixelMaster darwin_dyn_master_v2;
void darwin_dyn_master_v2_init(void);
+inline void darwin_dyn_master_v2_enable_power(void);
+inline void darwin_dyn_master_v2_disable_power(void);
#ifdef __cplusplus
}
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 86ee43f0353cf4e4b92c807470bc6b3cee4becbc..b84afcc7e78db9bde69d3b2fa67e03b84805ef40 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -75,119 +75,184 @@ extern "C" {
#define HEAD_TILT_I ((unsigned short int)0x004E)
#define HEAD_TILT_D ((unsigned short int)0x0050)
#define HEAD_TILT_I_CLAMP ((unsigned short int)0x0052)
-#define GRIPPER_LEFT_ID ((unsigned short int)0x0054)
-#define GRIPPER_LEFT_MAX_ANGLE ((unsigned short int)0x0055)
-#define GRIPPER_LEFT_MIN_ANGLE ((unsigned short int)0x0057)
-#define GRIPPER_LEFT_MAX_FORCE ((unsigned short int)0x0059)
-#define GRIPPER_RIGHT_ID ((unsigned short int)0x005B)
-#define GRIPPER_RIGHT_MAX_ANGLE ((unsigned short int)0x005C)
-#define GRIPPER_RIGHT_MIN_ANGLE ((unsigned short int)0x005E)
-#define GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x0060)
-#define SMART_CHARGER_PERIOD ((unsigned short int)0x0062)
+#define GRIPPER_LEFT_TOP_ID ((unsigned short int)0x0054)
+#define GRIPPER_LEFT_BOT_ID ((unsigned short int)0x0055)
+#define GRIPPER_LEFT_MAX_ANGLE ((unsigned short int)0x0056)
+#define GRIPPER_LEFT_MIN_ANGLE ((unsigned short int)0x0058)
+#define GRIPPER_LEFT_MAX_FORCE ((unsigned short int)0x005A)
+#define GRIPPER_RIGHT_TOP_ID ((unsigned short int)0x005C)
+#define GRIPPER_RIGHT_BOT_ID ((unsigned short int)0x005D)
+#define GRIPPER_RIGHT_MAX_ANGLE ((unsigned short int)0x005E)
+#define GRIPPER_RIGHT_MIN_ANGLE ((unsigned short int)0x0060)
+#define GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x0062)
+#define SMART_CHARGER_PERIOD ((unsigned short int)0x0064)
+#define STAIRS_PHASE1_TIME ((unsigned short int)0x0066)
+#define STAIRS_PHASE2_TIME ((unsigned short int)0x0068)
+#define STAIRS_PHASE3_TIME ((unsigned short int)0x006A)
+#define STAIRS_PHASE4_TIME ((unsigned short int)0x006C)
+#define STAIRS_PHASE5_TIME ((unsigned short int)0x006E)
+#define STAIRS_PHASE6_TIME ((unsigned short int)0x0070)
+#define STAIRS_PHASE7_TIME ((unsigned short int)0x0072)
+#define STAIRS_PHASE8_TIME ((unsigned short int)0x0074)
+#define STAIRS_PHASE9_TIME ((unsigned short int)0x0076)
+#define STAIRS_X_OFFSET ((unsigned short int)0x0078)
+#define STAIRS_Y_OFFSET ((unsigned short int)0x0079)
+#define STAIRS_Z_OFFSET ((unsigned short int)0x007A)
+#define STAIRS_R_OFFSET ((unsigned short int)0x007B)
+#define STAIRS_P_OFFSET ((unsigned short int)0x007C)
+#define STAIRS_A_OFFSET ((unsigned short int)0x007D)
+#define STAIRS_Y_SHIFT ((unsigned short int)0x007E)
+#define STAIRS_X_SHIFT ((unsigned short int)0x007F)
+#define STAIRS_Z_OVERSHOOT ((unsigned short int)0x0080)
+#define STAIRS_Z_HEIGHT ((unsigned short int)0x0081)
+#define STAIRS_HIP_PITCH_OFF ((unsigned short int)0x0082)
+#define STAIRS_R_SHIFT ((unsigned short int)0x0084)
+#define STAIRS_P_SHIFT ((unsigned short int)0x0085)
+#define STAIRS_A_SHIFT ((unsigned short int)0x0086)
+#define STAIRS_Y_SPREAD ((unsigned short int)0x0087)
+#define STAIRS_X_SHIFT_BODY ((unsigned short int)0x0088)
#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_TOP_ID = GRIPPER_LEFT_TOP_ID,
+ DARWIN_GRIPPER_LEFT_BOT_ID = GRIPPER_LEFT_BOT_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_TOP_ID = GRIPPER_RIGHT_TOP_ID,
+ DARWIN_GRIPPER_RIGHT_BOT_ID = GRIPPER_RIGHT_BOT_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_STAIRS_PHASE1_TIME_L = STAIRS_PHASE1_TIME,
+ DARWIN_STAIRS_PHASE1_TIME_H = STAIRS_PHASE1_TIME+1,
+ DARWIN_STAIRS_PHASE2_TIME_L = STAIRS_PHASE2_TIME,
+ DARWIN_STAIRS_PHASE2_TIME_H = STAIRS_PHASE2_TIME+1,
+ DARWIN_STAIRS_PHASE3_TIME_L = STAIRS_PHASE3_TIME,
+ DARWIN_STAIRS_PHASE3_TIME_H = STAIRS_PHASE3_TIME+1,
+ DARWIN_STAIRS_PHASE4_TIME_L = STAIRS_PHASE4_TIME,
+ DARWIN_STAIRS_PHASE4_TIME_H = STAIRS_PHASE4_TIME+1,
+ DARWIN_STAIRS_PHASE5_TIME_L = STAIRS_PHASE5_TIME,
+ DARWIN_STAIRS_PHASE5_TIME_H = STAIRS_PHASE5_TIME+1,
+ DARWIN_STAIRS_PHASE6_TIME_L = STAIRS_PHASE6_TIME,
+ DARWIN_STAIRS_PHASE6_TIME_H = STAIRS_PHASE6_TIME+1,
+ DARWIN_STAIRS_PHASE7_TIME_L = STAIRS_PHASE7_TIME,
+ DARWIN_STAIRS_PHASE7_TIME_H = STAIRS_PHASE7_TIME+1,
+ DARWIN_STAIRS_PHASE8_TIME_L = STAIRS_PHASE8_TIME,
+ DARWIN_STAIRS_PHASE8_TIME_H = STAIRS_PHASE8_TIME+1,
+ DARWIN_STAIRS_PHASE9_TIME_L = STAIRS_PHASE9_TIME,
+ DARWIN_STAIRS_PHASE9_TIME_H = STAIRS_PHASE9_TIME+1,
+ DARWIN_STAIRS_X_OFFSET = STAIRS_X_OFFSET,
+ DARWIN_STAIRS_Y_OFFSET = STAIRS_Y_OFFSET,
+ DARWIN_STAIRS_Z_OFFSET = STAIRS_Z_OFFSET,
+ DARWIN_STAIRS_Y_SHIFT = STAIRS_Y_SHIFT,
+ DARWIN_STAIRS_R_OFFSET = STAIRS_R_OFFSET,
+ DARWIN_STAIRS_P_OFFSET = STAIRS_P_OFFSET,
+ DARWIN_STAIRS_A_OFFSET = STAIRS_A_OFFSET,
+ DARWIN_STAIRS_X_SHIFT = STAIRS_X_SHIFT,
+ DARWIN_STAIRS_Z_OVERSHOOT = STAIRS_Z_OVERSHOOT,
+ DARWIN_STAIRS_Z_HEIGHT = STAIRS_Z_HEIGHT,
+ DARWIN_STAIRS_HIP_PITCH_OFF_L = STAIRS_HIP_PITCH_OFF,
+ DARWIN_STAIRS_HIP_PITCH_OFF_H = STAIRS_HIP_PITCH_OFF+1,
+ DARWIN_STAIRS_R_SHIFT = STAIRS_R_SHIFT,
+ DARWIN_STAIRS_P_SHIFT = STAIRS_P_SHIFT,
+ DARWIN_STAIRS_A_SHIFT = STAIRS_A_SHIFT,
+ DARWIN_STAIRS_Y_SPREAD = STAIRS_Y_SPREAD,
+ DARWIN_STAIRS_X_SHIFT_BODY = STAIRS_X_SHIFT_BODY,
+
//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
@@ -250,10 +315,6 @@ typedef enum {
DARWIN_ADC_TEMP_H = 0x012C,
DARWIN_ADC_CH12_L = 0x012D,
DARWIN_ADC_CH12_H = 0x012E,
- DARWIN_ADC_VREF_L = 0x012F,
- DARWIN_ADC_VREF_H = 0x0130,
- DARWIN_ADC_CH14_L = 0x0131,
- DARWIN_ADC_CH14_H = 0x0132,
DARWIN_IMU_CNTRL = 0x013B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// accel_det | gyro_det | calibrating | running | | start_cal | stop | start
DARWIN_IMU_CAL_SAMPLES_L = 0x013C,
@@ -272,8 +333,8 @@ typedef enum {
DARWIN_IMU_ACCEL_Z_H = 0x0149,
DARWIN_MM_NUM_SERVOS = 0x014A,
- DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // scanning | | | | | Enable power | Enable balance | Enable manager
+ DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // scanning | fwd fall | bwd fall | | Enable power v2 | Enable power | Enable balance | Enable manager
DARWIN_MM_MODULE_EN0 = 0x014C, // 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 |
@@ -568,68 +629,22 @@ typedef enum {
DARWIN_HEAD_MIN_TILT_H = 0x0244,
DARWIN_HEAD_TILT_TARGET_L = 0x0245, // angle in fixed point format 9|7
DARWIN_HEAD_TILT_TARGET_H = 0x0246,
-
-// to do: include more registers os smart charger's memory map to read
-/* DARWIN_BATT_CHARGER_STATUS = 0x0247,
- DARWIN_BATT_INPUT_CURRENT_L = 0x0248,
- DARWIN_BATT_INPUT_CURRENT_H = 0x0249,
- DARWIN_BATT_CHARGE_CURRENT_L = 0x024A,
- DARWIN_BATT_CHARGE_CURRENT_H = 0x024B,
- DARWIN_BATT_CHARGE_VOLTAGE_L = 0x024C,
- DARWIN_BATT_CHARGE_VOLTAGE_H = 0x024D,
- DARWIN_BATT_LIMIT_CURRENT_L = 0x024E,
- DARWIN_BATT_LIMIT_CURRENT_H = 0x024F, //
- DARWIN_BATT_TEMPERATURE_L = 0x0250,
- DARWIN_BATT_TEMPERATURE_H = 0x0251,
- DARWIN_BATT_VOLTAGE_L = 0x0252,
- DARWIN_BATT_VOLTAGE_H = 0x0253,
- DARWIN_BATT_CURRENT_L = 0x0254,
- DARWIN_BATT_CURRENT_H = 0x0255,
- DARWIN_BATT_AVG_CURRENT_L = 0x0256,
- DARWIN_BATT_AVG_CURRENT_H = 0x0257,
- DARWIN_BATT_RELATIVE_SOC = 0x0258,//
- DARWIN_BATT_ABSOLUTE_SOC = 0x0259,
- DARWIN_BATT_REMAINING_CAP_L = 0x025A,
- DARWIN_BATT_REMAINING_CAP_H = 0x025B,
- DARWIN_BATT_FULL_CHARGE_CAP_L = 0x025C,
- DARWIN_BATT_FULL_CHARGE_CAP_H = 0x025D,
- DARWIN_BATT_RUN_TIME_EMPTY_L = 0x025E, //
- DARWIN_BATT_RUN_TIME_EMPTY_H = 0x025F,
- DARWIN_BATT_AVG_TIME_EMPTY_L = 0x0260,
- DARWIN_BATT_AVG_TIME_EMPTY_H = 0x0261,
- DARWIN_BATT_AVG_TIME_FULL_L = 0x0262,
- DARWIN_BATT_AVG_TIME_FULL_H = 0x0263,
- DARWIN_BATT_STATUS_L = 0x0264,
- DARWIN_BATT_STATUS_H = 0x0265,
- DARWIN_BATT_DESIGN_CAP_L = 0x0266,
- DARWIN_BATT_DESIGN_CAP_H = 0x0267,
- DARWIN_BATT_DESIGN_VOLTAGE_L = 0x0268,
- DARWIN_BATT_DESIGN_VOLTAGE_H = 0x0269,
- DARWIN_BATT_CELL1_VOLTAGE_L = 0x026A,
- DARWIN_BATT_CELL1_VOLTAGE_H = 0x026B,
- DARWIN_BATT_CELL2_VOLTAGE_L = 0x026C,
- DARWIN_BATT_CELL2_VOLTAGE_H = 0x026D,
- DARWIN_BATT_CELL3_VOLTAGE_L = 0x026E,
- DARWIN_BATT_CELL3_VOLTAGE_H = 0x026F,
- DARWIN_BATT_CELL4_VOLTAGE_L = 0x0270,
- DARWIN_BATT_CELL4_VOLTAGE_H = 0x0271,
-*/
-//añadidos
+
DARWIN_SMART_CHARGER_ID = 0x0247,
- DARWIN_SMART_CHARGER_STATUS = 0x0248,
- DARWIN_SMART_CHARGER_LIMIT_CURRENT_L = 0x0249,
- DARWIN_SMART_CHARGER_LIMIT_CURRENT_H = 0x024A,
- DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L = 0x024B,
- DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_H = 0x024C,
- DARWIN_SMART_CHARGER_AVG_TIME_FULL_L = 0x024D,
- DARWIN_SMART_CHARGER_AVG_TIME_FULL_H = 0x024E,
- DARWIN_SMART_CHARGER_BATT_STATUS_L = 0x024F,
- DARWIN_SMART_CHARGER_BATT_STATUS_H = 0x0250,
- DARWIN_SMART_CHARGER_CNTRL = 0x0251, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | detected | enable
-//Cambio registro
- DARWIN_GRIPPER_CNTRL = 0x0252 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // left opened | right opened | | | close left | open left | close right | open right
+ DARWIN_SMART_CHARGER_LIMIT_CURRENT_L = 0x0248, // Input max current in mA
+ DARWIN_SMART_CHARGER_LIMIT_CURRENT_H = 0x0249,
+ DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L = 0x024A, // Average time to empty batteries in min
+ DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_H = 0x024B,
+ DARWIN_SMART_CHARGER_AVG_TIME_FULL_L = 0x024C, // Average time to full batteries in min
+ DARWIN_SMART_CHARGER_AVG_TIME_FULL_H = 0x024D,
+ DARWIN_SMART_CHARGER_BATT_STATUS_L = 0x024E, // Battery status
+ DARWIN_SMART_CHARGER_BATT_STATUS_H = 0x024F,
+ DARWIN_SMART_CHARGER_CNTRL = 0x0250, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | detected | enable
+ DARWIN_GRIPPER_CNTRL = 0x0251, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // left opened | right opened | left moving | right moving | close left | open left | close right | open right
+ DARWIN_STAIRS_CNTRL = 0x0252 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // current phase | climbing | stop stairs | start stairs down | start stairs up
}darwin_registers;
#define GPIO_BASE_ADDRESS 0x0100
@@ -674,7 +689,10 @@ typedef enum {
#define MANAGER_ENABLE 0x01
#define MANAGER_EN_BAL 0x02
#define MANAGER_EN_PWR 0x04
+#define MANAGER_EN_PWR_V2 0x08
#define MANAGER_SCANNING 0x80
+#define MANAGER_FWD_FALL 0x40
+#define MANAGER_BWD_FALL 0x20
#define MANAGER_EVEN_SER_EN 0x80
#define MANAGER_EVEN_SER_MOD 0x70
#define MANAGER_ODD_SER_EN 0x08
@@ -711,23 +729,35 @@ typedef enum {
#define HEAD_STOP 0x02
#define HEAD_STATUS 0x10
-#define GRIPPER_BASE_ADDRESS 0x0252//0x0274
+#define GRIPPER_BASE_ADDRESS 0x0251
#define GRIPPER_MEM_LENGTH 1
#define GRIPPER_EEPROM_ADDRESS 0x0054
-#define GRIPPER_EEPROM_LENGTH 14
+#define GRIPPER_EEPROM_LENGTH 16
#define GRIPPER_OPEN_RIGHT 0x01
#define GRIPPER_CLOSE_RIGHT 0x02
#define GRIPPER_OPEN_LEFT 0x04
#define GRIPPER_CLOSE_LEFT 0x08
+#define GRIPPER_MOVING_LEFT 0x20
+#define GRIPPER_MOVING_RIGHT 0x10
+#define GRIPPER_OPENED_LEFT 0x80
+#define GRIPPER_OPENED_RIGHT 0x40
#define SMART_CHARGER_BASE_ADDRESS 0x0247
-#define SMART_CHARGER_MEM_LENGTH 10 //45
-#define SMART_CHARGER_EEPROM_BASE 0x0060
+#define SMART_CHARGER_MEM_LENGTH 11
+#define SMART_CHARGER_EEPROM_BASE 0x0062
#define SMART_CHARGER_EEPROM_LENGTH 2
#define SMART_CHARGER_DET 0x01
#define SMART_CHARGER_EN 0x02
-
+#define STAIRS_BASE_ADDRESS 0x0252
+#define STAIRS_MEM_LENGTH 1
+#define STAIRS_EEPROM_ADDRESS 0x0066
+#define STAIRS_EEPROM_LENGTH 35
+#define STAIRS_START_UP 0x01
+#define STAIRS_START_DOWN 0x02
+#define STAIRS_STOP 0x04
+#define STAIRS_STATUS 0x08
+#define STAIRS_PHASE 0xF0
#ifdef __cplusplus
}
diff --git a/include/dyn_battery.h b/include/dyn_battery.h
index ac02ce82b99b8996815da0b769d379eaa18ce3e6..70e47b622cbb9e935c4cacd2d708d15971979527 100644
--- a/include/dyn_battery.h
+++ b/include/dyn_battery.h
@@ -45,7 +45,7 @@ typedef enum {
BATTERY_OUTPUT_VOLTAGE_H = 0x1E,
/* RAM */
BATTERY_CHARGER_STATUS = 0x1F,
- BATTERY_INPUT_CURRENT_L = 0x20,//corriente consumida (mA)
+ BATTERY_INPUT_CURRENT_L = 0x20,
BATTERY_INPUT_CURRENT_H = 0x21,
BATTERY_CHARGE_CURRENT_L = 0x22,
BATTERY_CHARGE_CURRENT_H = 0x23,
@@ -61,25 +61,25 @@ typedef enum {
BATTERY_CURRENT_H = 0x2D,
BATTERY_AVG_CURRENT_L = 0x2E,
BATTERY_AVG_CURRENT_H = 0x2F,
- BATTERY_RELATIVE_SOC = 0x30,//State Of Charge (%)
+ BATTERY_RELATIVE_SOC = 0x30,
BATTERY_ABSOLUTE_SOC = 0x31,
- BATTERY_REMAINING_CAP_L = 0x32,//mAh (tiempo de vida)
+ BATTERY_REMAINING_CAP_L = 0x32,
BATTERY_REMAINING_CAP_H = 0x33,
- BATTERY_FULL_CHARGE_CAP_L = 0x34,//
+ BATTERY_FULL_CHARGE_CAP_L = 0x34,
BATTERY_FULL_CHARGE_CAP_H = 0x35,
- BATTERY_RUN_TIME_EMPTY_L = 0x36,//(min)
+ BATTERY_RUN_TIME_EMPTY_L = 0x36,
BATTERY_RUN_TIME_EMPTY_H = 0x37,
BATTERY_AVG_TIME_EMPTY_L = 0x38,
- BATTERY_AVG_TIME_EMPTY_H = 0x39,//tiempo hasta que este descargada (min)
- BATTERY_AVG_TIME_FULL_L = 0x3A,//tiempo hasta que este cargada (min)
+ BATTERY_AVG_TIME_EMPTY_H = 0x39,
+ BATTERY_AVG_TIME_FULL_L = 0x3A,
BATTERY_AVG_TIME_FULL_H = 0x3B,
- BATTERY_STATUS_L = 0x3C,//cargando o descargando, cargada, descargada,...
+ BATTERY_STATUS_L = 0x3C,
BATTERY_STATUS_H = 0x3D,
- BATTERY_DESIGN_CAP_L = 0x3E,//Capacidad de la bateria - numero constante
+ BATTERY_DESIGN_CAP_L = 0x3E,
BATTERY_DESIGN_CAP_H = 0x3F,
BATTERY_DESIGN_VOLTAGE_L = 0x40,
BATTERY_DESIGN_VOLTAGE_H = 0x41,
- BATTERY_CELL1_VOLTAGE_L = 0x42,//Voltage de cada celda
+ BATTERY_CELL1_VOLTAGE_L = 0x42,
BATTERY_CELL1_VOLTAGE_H = 0x43,
BATTERY_CELL2_VOLTAGE_L = 0x44,
BATTERY_CELL2_VOLTAGE_H = 0x45,
diff --git a/include/eeprom.h b/include/eeprom.h
index c20035882aa091380fd791694b2318e5029ac777..5d227885a7c436bf53c08f10f51c986163b46e31 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)0x60)
+#define NB_OF_VAR ((uint8_t)0x89)
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index 7ba89ed6261ee183c407170d0a0fff7e1a2587ab..09eca5c7ead93c546ad246a4cf536853dbc8d9af 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -5,86 +5,112 @@
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_TOP_ID 0x0015 // ID 21 for the left gripper
+#define DEFAULT_GRIPPER_LEFT_BOT_ID 0x0016 // ID 22 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 0x0080 // 1023 max force in binary format
+#define DEFAULT_GRIPPER_RIGHT_TOP_ID 0x0017 // ID 23 for the left gripper
+#define DEFAULT_GRIPPER_RIGHT_BOT_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 0x0080 // 1023 max force in binary format
+#define DEFAULT_SMART_CHARGER_PERIOD 0x05DC // 1500 ms
+#define DEFAULT_STAIRS_PHASE1_TIME 0x0640 // 1600 ms
+#define DEFAULT_STAIRS_PHASE2_TIME 0x0C80 // 3200 ms
+#define DEFAULT_STAIRS_PHASE3_TIME 0x12C0 // 4800 ms
+#define DEFAULT_STAIRS_PHASE4_TIME 0x1900 // 6400 ms
+#define DEFAULT_STAIRS_PHASE5_TIME 0x1F40 // 8000 ms
+#define DEFAULT_STAIRS_PHASE6_TIME 0x2580 // 9600 ms
+#define DEFAULT_STAIRS_PHASE7_TIME 0x2BC0 // 11200 ms
+#define DEFAULT_STAIRS_PHASE8_TIME 0x3200 // 12800 ms
+#define DEFAULT_STAIRS_PHASE9_TIME 0x3840 // 14400 ms
+#define DEFAULT_STAIRS_X_OFFSET 0xFFF6 // -10 mm
+#define DEFAULT_STAIRS_Y_OFFSET 0x0005 // 5mm
+#define DEFAULT_STAIRS_Z_OFFSET 0x0014 // 20 mm
+#define DEFAULT_STAIRS_R_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_STAIRS_P_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_STAIRS_A_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_STAIRS_Y_SHIFT 0x0028 // 40 mm
+#define DEFAULT_STAIRS_X_SHIFT 0x0050 // 80 mm
+#define DEFAULT_STAIRS_Z_OVERSHOOT 0x000F // 15 mm
+#define DEFAULT_STAIRS_Z_HEIGHT 0x001E // 30 mm
+#define DEFAULT_STAIRS_HIP_PITCH_OFF 0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10
+#define DEFAULT_STAIRS_R_SHIFT 0x000B // 2.875 degrees in fixed point format 5 (1+5) | 2
+#define DEFAULT_STAIRS_P_SHIFT 0x0017 // 5.73 degrees in fixed point format 5 (1+5) | 2
+#define DEFAULT_STAIRS_A_SHIFT 0x0045 // 17.19 degrees in fixed point format 6 (1+5) | 2
+#define DEFAULT_STAIRS_Y_SPREAD 0x0014 // 20 mm
+#define DEFAULT_STAIRS_X_SHIFT_BODY 0x0023 // 35 mm
-
+#define EEPROM_SIZE 137
#ifdef __cplusplus
}
diff --git a/include/grippers.h b/include/grippers.h
index 9df31bcba3b0a176d7c83c76f7a89e40b98b23b2..9a1aa5fd90e5c64d1cc75a9bac5fe51f73109634 100644
--- a/include/grippers.h
+++ b/include/grippers.h
@@ -16,13 +16,15 @@ inline void grippers_set_period(uint16_t period_us);
inline uint16_t grippers_get_period(void);
void grippers_open(grippers_t gripper);
void grippers_close(grippers_t gripper);
+void grippers_stop(grippers_t gripper);
uint8_t gripper_is_open(grippers_t gripper);
+uint8_t gripper_is_close(grippers_t gripper);
+uint8_t gripper_is_moving(grippers_t gripper);
// operation functions
uint8_t grippers_in_range(unsigned short int address, unsigned short int length);
void grippers_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
void grippers_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-// motion manager interface functions
void grippers_process(void);
#ifdef __cplusplus
diff --git a/include/imu.h b/include/imu.h
index 079304bf1b235660acb081ab1186a926366633c0..f3e1c3d203ca0ebe27e7a82ba3c569d4d5cddb11 100755
--- a/include/imu.h
+++ b/include/imu.h
@@ -14,6 +14,7 @@ void imu_stop(void);
void imu_set_calibration_samples(uint16_t num_samples);
void imu_start_gyro_cal(void);
void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z);
+void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z);
uint8_t imu_is_gyro_calibrated(void);
// operation functions
uint8_t imu_in_range(unsigned short int address,unsigned short int length);
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 10f053f73afb1aeef86c1b705e4e2f68b0feb327..d0f234ce1e61bd2eefffab36a056d07729157523 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -29,7 +29,11 @@ typedef enum {
R_ANKLE_ROLL = 17,
L_ANKLE_ROLL = 18,
L_PAN = 19,
- L_TILT = 20} servo_id_t;
+ L_TILT = 20,
+ L_GRIPPER_TOP = 21,
+ L_GRIPPER_BOT = 22,
+ R_GRIPPER_TOP = 23,
+ R_GRIPPER_BOT = 24} servo_id_t;
typedef enum {MM_NONE = 0,
@@ -38,6 +42,10 @@ typedef enum {MM_NONE = 0,
MM_JOINTS = 3,
MM_HEAD = 4} TModules;
+typedef enum {MM_FWD_FALL = 0,
+ MM_BWD_FALL = 1,
+ MM_STANDING = 2} TFall;
+
typedef struct
{
void (*process_fnct)(void);
@@ -86,6 +94,8 @@ inline void manager_disable(void);
inline uint8_t manager_is_enabled(void);
void manager_enable_balance(void);
void manager_disable_balance(void);
+inline uint8_t manager_has_fallen(void);
+TFall manager_get_fallen_position(void);
inline uint8_t manager_get_num_servos(void);
void manager_set_module(uint8_t servo_id,TModules module);
inline TModules manager_get_module(uint8_t servo_id);
diff --git a/include/smart_charger.h b/include/smart_charger.h
index d6f8659ee1635a6f3a4446b1ddb1cc3aedbf0e76..42c3217353f678c2f0f4d82fc63bbc35732c5478 100644
--- a/include/smart_charger.h
+++ b/include/smart_charger.h
@@ -37,13 +37,13 @@ void smart_charger_init(uint16_t period_us);
*
* \param master pointer to a TDynamixelMaster structure ((darwin_dyn_master or darwin_dyn_master_v2).
*/
-inline void smart_charger_set_version(TDynamixelMaster *master);
+inline void smart_charger_set_master(TDynamixelMaster *master);
/**
* \brief Function to get the master version of the Dynamixel protocol set to the module
* \return the Dynamixel master structure associated to the smart charger module (darwin_dyn_master or darwin_dyn_master_v2).
*/
-inline TDynamixelMaster* smart_charger_get_version(void);
+inline TDynamixelMaster* smart_charger_get_master(void);
/**
* \brief Function to set smart charger's period
*
@@ -118,4 +118,4 @@ void smart_charger_process(void);
}
#endif
-#endif
\ No newline at end of file
+#endif
diff --git a/include/stairs.h b/include/stairs.h
new file mode 100755
index 0000000000000000000000000000000000000000..4cb0bacaa035ca6ccbb4e75ed88ecafacc92c4a8
--- /dev/null
+++ b/include/stairs.h
@@ -0,0 +1,31 @@
+#ifndef _STAIRS_H
+#define _STAIRS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
+
+// public functions
+void stairs_init(uint16_t period_us);
+inline void stairs_set_period(uint16_t period_us);
+inline uint16_t stairs_get_period(void);
+void stairs_start(uint8_t up);
+void stairs_stop(void);
+uint8_t is_climbing_stairs(void);
+uint8_t stairs_get_phase(void);
+
+// operation functions
+uint8_t stairs_in_range(unsigned short int address, unsigned short int length);
+void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+// motion manager interface functions
+void stairs_process(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/adc_dma.c b/src/adc_dma.c
index d24f466d8bdf495add858283b077871a3f177eb5..eb0f821a5e19c6faea28b2ff64462cbdfb3b4547 100755
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -7,15 +7,13 @@
#define ADC1_CH4 ADC_CHANNEL_7
#define ADC1_CH5 ADC_CHANNEL_8
#define ADC1_CH6 ADC_CHANNEL_TEMPSENSOR
-#define ADC1_CH7 ADC_CHANNEL_VREFINT
#define ADC2_CH1 ADC_CHANNEL_9
#define ADC2_CH2 ADC_CHANNEL_10
#define ADC2_CH3 ADC_CHANNEL_11
#define ADC2_CH4 ADC_CHANNEL_12
-#define ADC2_CH5 ADC_CHANNEL_13
-#define ADC2_CH6 ADC_CHANNEL_14
-#define ADC2_CH7 ADC_CHANNEL_15
+#define ADC2_CH5 ADC_CHANNEL_14
+#define ADC2_CH6 ADC_CHANNEL_15
#define ADC1_CH1_PIN GPIO_PIN_0
#define ADC1_CH1_PORT GPIOA
@@ -47,15 +45,12 @@
#define ADC2_CH4_PIN GPIO_PIN_2
#define ADC2_CH4_PORT GPIOC
#define ADC2_CH4_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH5_PIN GPIO_PIN_3
+#define ADC2_CH5_PIN GPIO_PIN_4
#define ADC2_CH5_PORT GPIOC
#define ADC2_CH5_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH6_PIN GPIO_PIN_4
+#define ADC2_CH6_PIN GPIO_PIN_5
#define ADC2_CH6_PORT GPIOC
#define ADC2_CH6_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH7_PIN GPIO_PIN_5
-#define ADC2_CH7_PORT GPIOC
-#define ADC2_CH7_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
#define ADC2_ENABLE_CLK __ADC2_CLK_ENABLE()
@@ -82,7 +77,7 @@ ADC_HandleTypeDef hadc2;
DMA_HandleTypeDef hdma_adc1;
TIM_HandleTypeDef ADC_TIM_Handle;
-uint32_t adc_data[9];// temporal buffer to store ADC data before conversion
+uint32_t adc_data[6];// temporal buffer to store ADC data before conversion
uint16_t adc_period_ms;
// private functions
@@ -153,7 +148,7 @@ void ADC_DMA_IRQHandler(void)
if(__HAL_DMA_GET_IT_SOURCE(&hdma_adc1, DMA_IT_TC) != RESET)
{
__HAL_DMA_CLEAR_FLAG(&hdma_adc1, __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_adc1));
- for(i=0;i<7;i++)
+ for(i=0;i<6;i++)
{
if(i==5)
{
@@ -202,7 +197,6 @@ void adc_init(void)
ADC2_CH4_ENABLE_PORT_CLK;
ADC2_CH5_ENABLE_PORT_CLK;
ADC2_CH6_ENABLE_PORT_CLK;
- ADC2_CH7_ENABLE_PORT_CLK;
ADC_ENABLE_DMA_CLK;
@@ -214,7 +208,7 @@ void adc_init(void)
hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
- hadc1.Init.NbrOfConversion = 7;
+ hadc1.Init.NbrOfConversion = 6;
HAL_ADC_Init(&hadc1);
multimode.Mode = ADC_DUALMODE_REGSIMULT;
@@ -246,10 +240,6 @@ void adc_init(void)
sConfig.Rank = 6;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
- sConfig.Channel = ADC1_CH7;
- sConfig.Rank = 7;
- HAL_ADC_ConfigChannel(&hadc1, &sConfig);
-
// configure GPIO
GPIO_InitStruct.Pin = ADC1_CH1_PIN|ADC1_CH2_PIN|ADC1_CH3_PIN|ADC1_CH4_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
@@ -266,7 +256,7 @@ void adc_init(void)
hadc2.Init.DiscontinuousConvMode = DISABLE;
hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;//ADC_EXTERNALTRIGCONV_T1_CC1;
hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
- hadc2.Init.NbrOfConversion = 7;
+ hadc2.Init.NbrOfConversion = 6;
HAL_ADC_Init(&hadc2);
// configure ADC2 channels
@@ -295,13 +285,9 @@ void adc_init(void)
sConfig.Rank = 6;
HAL_ADC_ConfigChannel(&hadc2, &sConfig);
- sConfig.Channel = ADC2_CH7;
- sConfig.Rank = 7;
- HAL_ADC_ConfigChannel(&hadc2, &sConfig);
-
// configure GPIO
GPIO_InitStruct.Pin = ADC2_CH2_PIN|ADC2_CH3_PIN|ADC2_CH4_PIN|ADC2_CH5_PIN|
- ADC2_CH6_PIN|ADC2_CH7_PIN;
+ ADC2_CH6_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
@@ -366,7 +352,7 @@ void adc_init(void)
HAL_ADCEx_Calibration_Start(&hadc2);
HAL_ADC_Start(&hadc2);
- HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,7);
+ HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,6);
}
void adc_start(void)
@@ -426,8 +412,6 @@ uint16_t adc_get_channel(adc_ch_t channel)
break;
case ADC_CH12: value=ram_data[DARWIN_ADC_CH12_L]+ram_data[DARWIN_ADC_CH12_H]*256;
break;
- case ADC_CH14: value=ram_data[DARWIN_ADC_CH14_L]+ram_data[DARWIN_ADC_CH14_H]*256;
- break;
default: value=0x0000;
}
@@ -462,8 +446,6 @@ uint16_t adc_get_channel_raw(adc_ch_t channel)
break;
case ADC_CH12: value=(adc_data[5]&0xFFFF0000)>>16;
break;
- case ADC_CH14: value=(adc_data[6]&0xFFFF0000)>>16;
- break;
default: value=0x0000;
}
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index d074c765ddd1f91e06636c2809f33fa476b6acb2..7693bace89751e5e77b06d534dc8b9787c1fb4bb 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -7,6 +7,7 @@
#include "darwin_time.h"
#include "darwin_dyn_slave.h"
#include "darwin_dyn_master.h"
+#include "darwin_dyn_master_v2.h"
#include "motion_manager.h"
#include "action.h"
#include "action_id.h"
@@ -14,7 +15,6 @@
int main(void)
{
uint16_t eeprom_data,period;
- uint8_t i;
/* initialize the HAL module */
HAL_Init();
diff --git a/src/darwin_dyn_master_v2.c b/src/darwin_dyn_master_v2.c
index 5dc5e5daf5ded2ca691a234b8c2dc9e1d6f9aae4..6152125c7700e3e8a4139a8086b974cfaf2df635 100755
--- a/src/darwin_dyn_master_v2.c
+++ b/src/darwin_dyn_master_v2.c
@@ -11,6 +11,10 @@
#define TX_EN_PIN GPIO_PIN_1
#define TX_EN_GPIO_PORT GPIOA
+#define POWER_GPIO_CLK __GPIOC_CLK_ENABLE()
+#define POWER_PIN GPIO_PIN_3
+#define POWER_GPIO_PORT GPIOC
+
/* private variables */
TDynamixelMaster darwin_dyn_master_v2;
TTime darwin_dyn_master_v2_timer;
@@ -79,5 +83,18 @@ void darwin_dyn_master_v2_init(void)
/* configure dynamixel master module */
dyn_master_set_rx_timeout(&darwin_dyn_master_v2,50);
dyn_master_set_return_level(&darwin_dyn_master_v2,return_all);
+ darwin_dyn_master_v2_disable_power();
+}
+
+inline void darwin_dyn_master_v2_enable_power(void)
+{
+ HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET);
+ ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR_V2;
+}
+
+inline void darwin_dyn_master_v2_disable_power(void)
+{
+ HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET);
+ ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR_V2);
}
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index dd7f12fac2b7730652638b6bf9e393ab0b2f1155..b287d062003f5a8011ae145d60137559255b6382 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -13,6 +13,7 @@
#include "head_tracking.h"
#include "grippers.h"
#include "smart_charger.h"
+#include "stairs.h"
/* timer for the execution of the dynamixel slave loop */
#define DYN_SLAVE_TIMER TIM7
@@ -96,6 +97,8 @@ unsigned char darwin_on_write(unsigned short int address,unsigned short int leng
// smart charger commands
if(smart_charger_in_range(address,length))
smart_charger_process_write_cmd(address,length,data);
+ if(stairs_in_range(address,length))
+ stairs_process_write_cmd(address,length,data);
// write eeprom
for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
EE_WriteVariable(i,data[j]);
diff --git a/src/eeprom.c b/src/eeprom.c
index d511af23b9cf8ebb04bf8cd314b10cfea15fc435..fd7d1f016d4c07e2bef8678d444a430c3e54311f 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -53,108 +53,145 @@
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_TOP_ID, GRIPPER_LEFT_TOP_ID,
+ DEFAULT_GRIPPER_LEFT_BOT_ID, GRIPPER_LEFT_BOT_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_TOP_ID, GRIPPER_RIGHT_TOP_ID,
+ DEFAULT_GRIPPER_RIGHT_BOT_ID, GRIPPER_RIGHT_BOT_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,
+ DEFAULT_STAIRS_PHASE1_TIME&0xFF, STAIRS_PHASE1_TIME,
+ DEFAULT_STAIRS_PHASE1_TIME>>8, STAIRS_PHASE1_TIME+1,
+ DEFAULT_STAIRS_PHASE2_TIME&0xFF, STAIRS_PHASE2_TIME,
+ DEFAULT_STAIRS_PHASE2_TIME>>8, STAIRS_PHASE2_TIME+1,
+ DEFAULT_STAIRS_PHASE3_TIME&0xFF, STAIRS_PHASE3_TIME,
+ DEFAULT_STAIRS_PHASE3_TIME>>8, STAIRS_PHASE3_TIME+1,
+ DEFAULT_STAIRS_PHASE4_TIME&0xFF, STAIRS_PHASE4_TIME,
+ DEFAULT_STAIRS_PHASE4_TIME>>8, STAIRS_PHASE4_TIME+1,
+ DEFAULT_STAIRS_PHASE5_TIME&0xFF, STAIRS_PHASE5_TIME,
+ DEFAULT_STAIRS_PHASE5_TIME>>8, STAIRS_PHASE5_TIME+1,
+ DEFAULT_STAIRS_PHASE6_TIME&0xFF, STAIRS_PHASE6_TIME,
+ DEFAULT_STAIRS_PHASE6_TIME>>8, STAIRS_PHASE6_TIME+1,
+ DEFAULT_STAIRS_PHASE7_TIME&0xFF, STAIRS_PHASE7_TIME,
+ DEFAULT_STAIRS_PHASE7_TIME>>8, STAIRS_PHASE7_TIME+1,
+ DEFAULT_STAIRS_PHASE8_TIME&0xFF, STAIRS_PHASE8_TIME,
+ DEFAULT_STAIRS_PHASE8_TIME>>8, STAIRS_PHASE8_TIME+1,
+ DEFAULT_STAIRS_PHASE9_TIME&0xFF, STAIRS_PHASE9_TIME,
+ DEFAULT_STAIRS_PHASE9_TIME>>8, STAIRS_PHASE9_TIME+1,
+ DEFAULT_STAIRS_X_OFFSET, STAIRS_X_OFFSET,
+ DEFAULT_STAIRS_Y_OFFSET, STAIRS_Y_OFFSET,
+ DEFAULT_STAIRS_Z_OFFSET, STAIRS_Z_OFFSET,
+ DEFAULT_STAIRS_R_OFFSET, STAIRS_R_OFFSET,
+ DEFAULT_STAIRS_P_OFFSET, STAIRS_P_OFFSET,
+ DEFAULT_STAIRS_A_OFFSET, STAIRS_A_OFFSET,
+ DEFAULT_STAIRS_Y_SHIFT, STAIRS_Y_SHIFT,
+ DEFAULT_STAIRS_X_SHIFT, STAIRS_X_SHIFT,
+ DEFAULT_STAIRS_Z_OVERSHOOT, STAIRS_Z_OVERSHOOT,
+ DEFAULT_STAIRS_Z_HEIGHT, STAIRS_Z_HEIGHT,
+ DEFAULT_STAIRS_HIP_PITCH_OFF&0xFF, STAIRS_HIP_PITCH_OFF,
+ DEFAULT_STAIRS_HIP_PITCH_OFF>>8, STAIRS_HIP_PITCH_OFF+1,
+ DEFAULT_STAIRS_R_SHIFT, STAIRS_R_SHIFT,
+ DEFAULT_STAIRS_P_SHIFT, STAIRS_P_SHIFT,
+ DEFAULT_STAIRS_A_SHIFT, STAIRS_A_SHIFT,
+ DEFAULT_STAIRS_Y_SPREAD, STAIRS_Y_SPREAD,
+ DEFAULT_STAIRS_X_SHIFT_BODY, STAIRS_X_SHIFT_BODY};
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
@@ -736,7 +773,7 @@ static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data)
HAL_StatusTypeDef flashstatus = HAL_OK;
uint32_t newpageaddress = EEPROM_START_ADDRESS;
uint32_t oldpageid = 0;
- uint16_t validpage = PAGE0, varidx = 0;
+ uint16_t validpage = PAGE0, varidx = 0, address=0;
uint16_t eepromstatus = 0, readstatus = 0;
uint32_t page_error = 0;
FLASH_EraseInitTypeDef s_eraseinit;
@@ -784,15 +821,16 @@ static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data)
/* Transfer process: transfer variables from old to the new active page */
for (varidx = 0; varidx < NB_OF_VAR; varidx++)
{
- if (eeprom_data[varidx] != VirtAddress) /* Check each variable except the one passed as parameter */
+ address=((__IO uint16_t*)oldpageid)[1+varidx*2];
+ if (address != VirtAddress) /* Check each variable except the one passed as parameter */
{
/* Read the other last variable updates */
- readstatus = EE_ReadVariable(eeprom_data[varidx], &DataVar);
+ readstatus = EE_ReadVariable(address, &DataVar);
/* In case variable corresponding to the virtual address was found */
if (readstatus != 0x1)
{
/* Transfer the variable to the new active page */
- eepromstatus = EE_VerifyPageFullWriteVariable(eeprom_data[varidx], DataVar);
+ eepromstatus = EE_VerifyPageFullWriteVariable(address, DataVar);
/* If program operation was failed, a Flash error code is returned */
if (eepromstatus != HAL_OK)
{
diff --git a/src/grippers.c b/src/grippers.c
index c5fae087946fe969081ef44b37459cbbd41ee25c..5544346b9a44459ee306563a05523dfe20bb66d9 100644
--- a/src/grippers.c
+++ b/src/grippers.c
@@ -2,42 +2,48 @@
#include "dyn_servos.h"
#include "grippers.h"
#include "ram.h"
+#include "joint_motion.h"
// private variables
int64_t grippers_period;
int16_t grippers_period_us;
-uint8_t grippers_left_opened;
-uint8_t grippers_left_open;
-uint8_t grippers_left_close;
-uint8_t grippers_right_opened;
-uint8_t grippers_right_open;
-uint8_t grippers_right_close;
+uint8_t gripper_left_target;
+uint8_t gripper_right_target;
// public functions
void grippers_init(uint16_t period_us)
{
uint16_t force;
+ uint32_t servo_mask=0x00000000;
/* initialize private variables */
- grippers_left_opened=0x00;
- grippers_left_open=0x00;
- grippers_left_close=0x00;
- grippers_right_opened=0x00;
- grippers_right_open=0x00;
- grippers_right_close=0x00;
+ gripper_left_target=0x00;//close;
+ gripper_right_target=0x00;//close;
grippers_period=(period_us<<16)/1000000;
grippers_period_us=period_us;
/* configure the maximum force of the servos */
- if(ram_data[DARWIN_GRIPPER_LEFT_ID]!=0xFF)
+ servo_mask=0x00000000;
+ if(ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]!=0xFF)
{
force=ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_H]<<8);
- dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_ID],XL_TORQUE_LIMIT_L,force);
+ dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_TOP_ID],XL_TORQUE_LIMIT_L,force);
+ dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_BOT_ID],XL_TORQUE_LIMIT_L,force);
+ servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_TOP_ID];
+ servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_BOT_ID];
+ joint_motion_set_group_servos(1,servo_mask);
}
- if(ram_data[DARWIN_GRIPPER_RIGHT_ID]!=0xFF)
+ servo_mask=0x00000000;
+ if(ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]!=0xFF)
{
force=ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_H]<<8);
- dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_ID],XL_TORQUE_LIMIT_L,force);
+ dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID],XL_TORQUE_LIMIT_L,force);
+ dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID],XL_TORQUE_LIMIT_L,force);
+ servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID];
+ servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID];
+ joint_motion_set_group_servos(2,servo_mask);
}
+ grippers_close(GRIPPER_LEFT);
+ grippers_close(GRIPPER_RIGHT);
}
inline void grippers_set_period(uint16_t period_us)
@@ -54,27 +60,101 @@ inline uint16_t grippers_get_period(void)
void grippers_open(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
- grippers_left_open=0x01;
+ {
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60;
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60;
+ joint_motion_start(1);
+ }
else if(gripper==GRIPPER_RIGHT)
- grippers_right_open=0x01;
+ {
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60;
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60;
+ joint_motion_start(2);
+ }
}
void grippers_close(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
- grippers_left_close=0x01;
+ {
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60;
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60;
+ joint_motion_start(1);
+ }
else if(gripper==GRIPPER_RIGHT)
- grippers_right_close=0x01;
+ {
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60;
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60;
+ joint_motion_start(2);
+ }
+}
+
+void grippers_stop(grippers_t gripper)
+{
+ if(gripper==GRIPPER_LEFT)
+ joint_motion_stop(1);
+ else
+ joint_motion_stop(2);
}
uint8_t gripper_is_open(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
- return grippers_left_opened;
- else if(gripper==GRIPPER_RIGHT)
- return grippers_right_opened;
+ {
+ if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
+ return 0x01;
+ else
+ return 0x00;
+ }
else
- return 0x00;
+ {
+ if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT)
+ return 0x01;
+ else
+ return 0x00;
+ }
+}
+
+uint8_t gripper_is_close(grippers_t gripper)
+{
+ if(gripper==GRIPPER_LEFT)
+ {
+ if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
+ return 0x00;
+ else
+ return 0x01;
+ }
+ else
+ {
+ if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT)
+ return 0x00;
+ else
+ return 0x01;
+ }
+}
+
+uint8_t gripper_is_moving(grippers_t gripper)
+{
+ if(gripper==GRIPPER_LEFT)
+ return are_joints_moving(1);
+ else
+ return are_joints_moving(2);
}
// operation functions
@@ -107,45 +187,32 @@ void grippers_process_write_cmd(unsigned short int address,unsigned short int le
}
}
-// motion manager interface functions
void grippers_process(void)
{
- uint16_t angle;
-
- if(ram_data[DARWIN_GRIPPER_LEFT_ID]!=0xFF)
+ if(!are_joints_moving(1))
+ {
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_LEFT;
+ if(gripper_left_target==0x01)
+ ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_LEFT;
+ else
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
+ }
+ else
{
- if(grippers_left_opened==0x01)
- {
- if(grippers_left_close==0x01)
- {
- grippers_left_close=0x00;
- angle=ram_data[DARWIN_GRIPPER_LEFT_MIN_ANGLE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MIN_ANGLE_H]<<8);
- manager_current_angles[ram_data[DARWIN_GRIPPER_LEFT_ID]]=angle<<9;
- }
- else if(grippers_left_open==0x01)
- {
- grippers_left_open=0x00;
- angle=ram_data[DARWIN_GRIPPER_LEFT_MAX_ANGLE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_ANGLE_H]<<8);
- manager_current_angles[ram_data[DARWIN_GRIPPER_LEFT_ID]]=angle<<9;
- }
- }
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
+ ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_LEFT;
}
- if(ram_data[DARWIN_GRIPPER_RIGHT_ID]!=0xFF)
+ if(!are_joints_moving(2))
+ {
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_RIGHT;
+ if(gripper_right_target==0x01)
+ ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_RIGHT;
+ else
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
+ }
+ else
{
- if(grippers_right_opened==0x01)
- {
- if(grippers_right_close==0x01)
- {
- grippers_right_close=0x00;
- angle=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]<<8);
- manager_current_angles[ram_data[DARWIN_GRIPPER_RIGHT_ID]]=angle<<9;
- }
- else if(grippers_right_open==0x01)
- {
- grippers_right_open=0x00;
- angle=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]<<8);
- manager_current_angles[ram_data[DARWIN_GRIPPER_RIGHT_ID]]=angle<<9;
- }
- }
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
+ ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_RIGHT;
}
}
diff --git a/src/imu.c b/src/imu.c
index 3d3f0478946591baac10d9cfa0017ce762f82be9..bbaed728d98168aec6e571a9473b1edc91b65dbc 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -31,11 +31,11 @@
// accelerometer registers
#define ACCEL_ID 0x32
#define ACCEL_WHO_AM_I 0x0F
-#define ACCEL_CNTRL_REG1 0x20
-#define ACCEL_CNTRL_REG2 0x21
-#define ACCEL_CNTRL_REG3 0x22
-#define ACCEL_CNTRL_REG4 0x23
-#define ACCEL_CNTRL_REG5 0x24
+#define ACCEL_CNTRL_REG1 0x20
+#define ACCEL_CNTRL_REG2 0x21
+#define ACCEL_CNTRL_REG3 0x22
+#define ACCEL_CNTRL_REG4 0x23
+#define ACCEL_CNTRL_REG5 0x24
#define ACCEL_HP_FILTER_RESET 0x25
#define ACCEL_REFERENCE 0x26
#define ACCEL_STATUS_REG 0x27
@@ -58,11 +58,11 @@
// gyroscope registers
#define GYRO_ID 0xD3
#define GYRO_WHO_AM_I 0x0F
-#define GYRO_CNTRL_REG1 0x20
-#define GYRO_CNTRL_REG2 0x21
-#define GYRO_CNTRL_REG3 0x22
-#define GYRO_CNTRL_REG4 0x23
-#define GYRO_CNTRL_REG5 0x24
+#define GYRO_CNTRL_REG1 0x20
+#define GYRO_CNTRL_REG2 0x21
+#define GYRO_CNTRL_REG3 0x22
+#define GYRO_CNTRL_REG4 0x23
+#define GYRO_CNTRL_REG5 0x24
#define GYRO_REFERENCE 0x25
#define GYRO_OUT_TEMP 0x26
#define GYRO_STATUS_REG 0x27
@@ -72,7 +72,7 @@
#define GYRO_OUT_Y_H 0x2B
#define GYRO_OUT_Z_L 0x2C
#define GYRO_OUT_Z_H 0x2D
-#define GYRO_FIFO_CNTRL_REG 0x2E
+#define GYRO_FIFO_CNTRL_REG 0x2E
#define GYRO_FIFO_SRC_REG 0x2F
#define GYRO_INT1_CFG 0x30
#define GYRO_INT1_SRC 0x31
@@ -110,6 +110,7 @@ uint16_t gyro_calibration_num_samples;
const uint8_t accel_conf_reg=ACCEL_CNTRL_REG1;
const uint8_t accel_conf_len=5;
uint8_t accel_conf_data[5];
+int32_t accel_data[3];
const uint8_t accel_data_reg=ACCEL_OUT_X_L;
const uint8_t accel_data_len=6;
@@ -254,13 +255,12 @@ void imu_accel_get_data(void)
void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out)
{
uint8_t i;
- int16_t value;
for(i=0;i<3;i++)
{
- value=(data_in[i*2]+(data_in[i*2+1]<<8));
- data_out[i*2]=value%256;
- data_out[i*2+1]=value/256;
+ accel_data[i]=(int16_t)(data_in[i*2]+(data_in[i*2+1]<<8));
+ data_out[i*2]=accel_data[i]%256;
+ data_out[i*2+1]=accel_data[i]/256;
}
}
@@ -603,6 +603,13 @@ void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z)
*z=gyro_data[2];
}
+void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z)
+{
+ *x=accel_data[0];
+ *y=accel_data[1];
+ *z=accel_data[2];
+}
+
// operation functions
uint8_t imu_in_range(unsigned short int address,unsigned short int length)
{
diff --git a/src/joint_motion.c b/src/joint_motion.c
index 8e8147aa5f2568e9fbb920ffdccddc71433295b6..395bfc0989f7d682875c94cf5972e15b4bcf4956 100644
--- a/src/joint_motion.c
+++ b/src/joint_motion.c
@@ -6,6 +6,7 @@
int64_t joint_target_angles[MANAGER_MAX_NUM_SERVOS];//48|16
int64_t joint_target_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
int64_t joint_target_accels[MANAGER_MAX_NUM_SERVOS];//48|16
+int64_t joint_target_stop_angles[MANAGER_MAX_NUM_SERVOS];//48|16
int64_t joint_current_angles[MANAGER_MAX_NUM_SERVOS];//48|16
int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
@@ -120,6 +121,8 @@ void joint_motion_start(uint8_t group_id)
ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=joint_target_accels[i]>>16;
}
}
+ // stop angles
+ joint_target_stop_angles[i]=joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i]);
// the current angles, speeds and accelerations are in RAM
if(joint_target_angles[i]>=joint_current_angles[i])
joint_dir[i]=1;
@@ -129,6 +132,7 @@ void joint_motion_start(uint8_t group_id)
}
}
joint_moving[group_id]=0x01;
+ joint_stop[group_id]=0x00;
ram_data[DARWIN_JOINT_GRP0_CNTRL+group_id*5]|=JOINT_STATUS;
}
@@ -236,7 +240,7 @@ void joint_motion_process(void)
}
else
{
- if(abs(joint_target_angles[i]-joint_current_angles[i])>(joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i])))// constant speed phase
+ if(abs(joint_target_angles[i]-joint_current_angles[i])>joint_target_stop_angles[i])// constant speed phase
{
joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16);
if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
@@ -256,6 +260,8 @@ void joint_motion_process(void)
}
}
}
+ else
+ joint_current_speeds[i]=0;
manager_current_angles[i]=joint_current_angles[i];
manager_current_slopes[i]=5;
}
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 4672002a904cf6b4af702a78645883c7176ac278..805fc182ad691cd24b4bc3baa1e7a6b315335d22 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -11,6 +11,8 @@
#include "imu.h"
#include "smart_charger.h"
#include "dyn_battery.h"
+#include "stairs.h"
+#include <stdlib.h>
#define MANAGER_TIMER TIM5
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM5_CLK_ENABLE()
@@ -131,6 +133,7 @@ void manager_balance(void)
{
uint32_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain;
int32_t gyro_x,gyro_y,gyro_z;
+ int32_t accel_x,accel_y,accel_z;
if(manager_balance_enabled==0x01)// balance is enabled
{
@@ -146,10 +149,30 @@ void manager_balance(void)
manager_balance_offset[R_ANKLE_PITCH]=((((int64_t)gyro_y*(int64_t)ankle_pitch_gain)/12000)>>9);
manager_balance_offset[L_KNEE]=((((int64_t)gyro_y*(int64_t)knee_gain)/12000)>>9);
manager_balance_offset[L_ANKLE_PITCH]=-((((int64_t)gyro_y*(int64_t)ankle_pitch_gain)/12000)>>9);
- manager_balance_offset[R_HIP_ROLL]=-((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9);
- manager_balance_offset[L_HIP_ROLL]=-((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9);
- manager_balance_offset[R_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
- manager_balance_offset[L_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
+ manager_balance_offset[R_HIP_ROLL]=((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9);
+ manager_balance_offset[L_HIP_ROLL]=((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9);
+ manager_balance_offset[R_ANKLE_ROLL]=-((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
+ manager_balance_offset[L_ANKLE_ROLL]=-((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
+ }
+ // fall detection (using accelerometer)
+ imu_get_accel_data(&accel_x,&accel_y,&accel_z);
+ if(abs(accel_y)>abs(accel_z))
+ {
+ if(accel_y>0)
+ {
+ ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_FWD_FALL);
+ ram_data[DARWIN_MM_CNTRL]|=MANAGER_BWD_FALL;
+ }
+ else
+ {
+ ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_BWD_FALL);
+ ram_data[DARWIN_MM_CNTRL]|=MANAGER_FWD_FALL;
+ }
+ }
+ else
+ {
+ ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_FWD_FALL);
+ ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_BWD_FALL);
}
}
@@ -173,8 +196,10 @@ void MANAGER_TIMER_IRQHandler(void)
head_tracking_process();
// call the walking process
walking_process();
- // call the grippers process
+ // call the gripper process
grippers_process();
+ // call the stairs process
+ stairs_process();
// balance the robot
manager_balance();
// access to smart charger
@@ -322,24 +347,25 @@ void manager_init(uint16_t period_us)
}
//Other devices detected - smart charger
- for(i=current; i<num; i++){
+ for(i=current; i<num; i++)
+ {
dyn_master_read_word(&darwin_dyn_master,servo_ids[i],BATTERY_MODEL_NUMBER_L,&model);
switch(model)
{
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_version(&darwin_dyn_master); //set bus Dynamixel master version
- // Read smart charger's memory map
- //dyn_master_read_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,43,&ram_data[DARWIN_BATT_CHARGER_STATUS]);
- //Read charger status
- dyn_master_read_byte(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,&ram_data[DARWIN_SMART_CHARGER_STATUS]);
- break;
+ ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i]; //smart charger ID
+ 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_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;
- }
+ }
}
darwin_dyn_master_disable_power();
+ darwin_dyn_master_v2_enable_power();
// detect the servos connected on the v2 bus
dyn_master_scan(&darwin_dyn_master_v2,&num,servo_ids);
ram_data[DARWIN_MM_NUM_SERVOS]+=num;
@@ -384,22 +410,24 @@ void manager_init(uint16_t period_us)
}
//Other devices detected - smart charger
- for(i=current; i<num; i++){
+ for(i=current; i<num; i++)
+ {
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_version(&darwin_dyn_master_v2); //Set bus Dynamixel master version
- // Read smart charger's memory map
- //dyn_master_read_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,43,&ram_data[DARWIN_BATT_CHARGER_STATUS]);
- //Read charger status
- dyn_master_read_byte(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,&ram_data[DARWIN_SMART_CHARGER_STATUS]);
+ 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_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;
- }
+ }
}
-
+
+ darwin_dyn_master_v2_disable_power();
ram_data[DARWIN_MM_PRESENT_SERVOS1]=(present_servos&0x000000FF);
ram_data[DARWIN_MM_PRESENT_SERVOS2]=((present_servos&0x0000FF00)>>8);
@@ -445,6 +473,7 @@ void manager_init(uint16_t period_us)
head_tracking_init(period_us);
grippers_init(period_us);
smart_charger_init(period_us);
+ stairs_init(period_us);
}
uint16_t manager_get_period(void)
@@ -507,6 +536,24 @@ void manager_disable_balance(void)
ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_BAL);
}
+uint8_t manager_has_fallen(void)
+{
+ if(ram_data[DARWIN_MM_CNTRL]&(MANAGER_FWD_FALL|MANAGER_BWD_FALL))
+ return 0x01;
+ else
+ return 0x00;
+}
+
+TFall manager_get_fallen_position(void)
+{
+ if(ram_data[DARWIN_MM_CNTRL]&MANAGER_FWD_FALL)
+ return MM_FWD_FALL;
+ else if(ram_data[DARWIN_MM_CNTRL]&MANAGER_BWD_FALL)
+ return MM_BWD_FALL;
+ else
+ return MM_STANDING;
+}
+
inline uint8_t manager_get_num_servos(void)
{
return manager_num_servos;
@@ -650,6 +697,10 @@ void manager_process_write_cmd(unsigned short int address,unsigned short int len
darwin_dyn_master_enable_power();
else
darwin_dyn_master_disable_power();
+ if(data[DARWIN_MM_CNTRL-address]&MANAGER_EN_PWR_V2)
+ darwin_dyn_master_v2_enable_power();
+ else
+ darwin_dyn_master_v2_disable_power();
}
// balance gains
for(i=MM_BAL_KNEE_GAIN_OFFSET;i<=MM_BAL_HIP_ROLL_GAIN_OFFSET+1;i++)
diff --git a/src/ram.c b/src/ram.c
index f2c505dfb8b5e82c15e1311d9a27dad51bd60416..be8aa9dd975afec953d101fafbc9576ea391ecd4 100755
--- a/src/ram.c
+++ b/src/ram.c
@@ -10,147 +10,11 @@ void ram_init(void)
for(i=0;i<RAM_SIZE;i++)
ram_data[i]=0x00;
// read contents from EEPROM to RAM
- if(EE_ReadVariable(DEVICE_MODEL_OFFSET,&eeprom_data)==0)
- ram_data[DEVICE_MODEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- 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&0x00FF);
- if(EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data)==0)
- 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&0x00FF);
- if(EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data)==0)
- 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)
- ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET,&eeprom_data)==0)
- ram_data[MM_BAL_KNEE_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET+1,&eeprom_data)==0)
- ram_data[MM_BAL_KNEE_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET,&eeprom_data)==0)
- ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,&eeprom_data)==0)
- ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET,&eeprom_data)==0)
- ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,&eeprom_data)==0)
- ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET,&eeprom_data)==0)
- ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET+1,&eeprom_data)==0)
- ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data)==0)
- ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- for(i=0;i<32;i++)
+ for(i=0;i<NB_OF_VAR;i++)
{
- if(EE_ReadVariable(MM_SERVO0_OFFSET+i,&eeprom_data)==0)
- ram_data[DARWIN_MM_SERVO0_OFFSET+i]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(i,&eeprom_data)==0)
+ ram_data[i]=(uint8_t)(eeprom_data&0x00FF);
}
- if(EE_ReadVariable(WALK_X_OFFSET,&eeprom_data)==0)
- ram_data[WALK_X_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_Y_OFFSET,&eeprom_data)==0)
- ram_data[WALK_Y_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_Z_OFFSET,&eeprom_data)==0)
- ram_data[WALK_Z_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_ROLL_OFFSET,&eeprom_data)==0)
- ram_data[WALK_ROLL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_PITCH_OFFSET,&eeprom_data)==0)
- ram_data[WALK_PITCH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_YAW_OFFSET,&eeprom_data)==0)
- ram_data[WALK_YAW_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_HIP_PITCH_OFF,&eeprom_data)==0)
- ram_data[WALK_HIP_PITCH_OFF]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_HIP_PITCH_OFF+1,&eeprom_data)==0)
- ram_data[WALK_HIP_PITCH_OFF+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_PERIOD_TIME,&eeprom_data)==0)
- ram_data[WALK_PERIOD_TIME]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_PERIOD_TIME+1,&eeprom_data)==0)
- ram_data[WALK_PERIOD_TIME+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_DSP_RATIO,&eeprom_data)==0)
- ram_data[WALK_DSP_RATIO]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_STEP_FW_BW_RATIO,&eeprom_data)==0)
- ram_data[WALK_STEP_FW_BW_RATIO]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_FOOT_HEIGHT,&eeprom_data)==0)
- ram_data[WALK_FOOT_HEIGHT]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_SWING_RIGHT_LEFT,&eeprom_data)==0)
- ram_data[WALK_SWING_RIGHT_LEFT]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_SWING_TOP_DOWN,&eeprom_data)==0)
- ram_data[WALK_SWING_TOP_DOWN]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_PELVIS_OFFSET,&eeprom_data)==0)
- ram_data[WALK_PELVIS_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_ARM_SWING_GAIN,&eeprom_data)==0)
- ram_data[WALK_ARM_SWING_GAIN]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_MAX_VEL,&eeprom_data)==0)
- ram_data[WALK_MAX_VEL]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_MAX_ROT_VEL,&eeprom_data)==0)
- ram_data[WALK_MAX_ROT_VEL]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_P,&eeprom_data)==0)
- ram_data[HEAD_PAN_P]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_P+1,&eeprom_data)==0)
- ram_data[HEAD_PAN_P+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_I,&eeprom_data)==0)
- ram_data[HEAD_PAN_I]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_I+1,&eeprom_data)==0)
- ram_data[HEAD_PAN_I+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_D,&eeprom_data)==0)
- ram_data[HEAD_PAN_D]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_D+1,&eeprom_data)==0)
- ram_data[HEAD_PAN_D+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_I_CLAMP,&eeprom_data)==0)
- ram_data[HEAD_PAN_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_I_CLAMP+1,&eeprom_data)==0)
- ram_data[HEAD_PAN_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_P,&eeprom_data)==0)
- ram_data[HEAD_TILT_P]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_P+1,&eeprom_data)==0)
- ram_data[HEAD_TILT_P+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_I,&eeprom_data)==0)
- ram_data[HEAD_TILT_I]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_I+1,&eeprom_data)==0)
- ram_data[HEAD_TILT_I+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_D,&eeprom_data)==0)
- ram_data[HEAD_TILT_D]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_D+1,&eeprom_data)==0)
- ram_data[HEAD_TILT_D+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_I_CLAMP,&eeprom_data)==0)
- ram_data[HEAD_TILT_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_I_CLAMP+1,&eeprom_data)==0)
- ram_data[HEAD_TILT_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_ID,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_ID]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE+1,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MAX_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MIN_ANGLE,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MIN_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MIN_ANGLE+1,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MIN_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE+1,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_ID,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_ID]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE+1,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MAX_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MIN_ANGLE,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MIN_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MIN_ANGLE+1,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MIN_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MAX_FORCE,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MAX_FORCE+1,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(SMART_CHARGER_PERIOD,&eeprom_data)==0)
- 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 70de2e7d9e8861e467e41ffc048098053149e7f2..03a9ed8f31a5736ee6fee473b1a37b8f5388101b 100644
--- a/src/smart_charger.c
+++ b/src/smart_charger.c
@@ -8,19 +8,11 @@
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
-uint16_t counter; //for read access operations
+uint16_t smart_charger_period; //smart charger period value in ms
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;
@@ -29,32 +21,28 @@ TDynamixelMaster *dyn_battery_master;
void smart_charger_init(uint16_t period_us)
{
smart_charger_id = ram_data[DARWIN_SMART_CHARGER_ID];
- smart_charger_enabled = 0x00;
+ 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];
- smart_charger_period = (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);
}
-inline void smart_charger_set_version(TDynamixelMaster *master)
+inline void smart_charger_set_master(TDynamixelMaster *master)
{
dyn_battery_master=master;
}
-inline TDynamixelMaster* smart_charger_get_version(void)
+inline TDynamixelMaster* smart_charger_get_master(void)
{
return dyn_battery_master;
}
-void smart_charger_set_period(uint16_t period) //en ms
+void smart_charger_set_period(uint16_t period)
{
- uint16_t mm_period=manager_get_period_us(); //default: 7800us
- smart_charger_count = (period*1000)/mm_period; //(ms*1000)/us
+ uint16_t mm_period=manager_get_period_us();
+ smart_charger_count = (period*1000)/mm_period;
smart_charger_period = period;
ram_data[DARWIN_SMART_CHARGER_PERIOD_L]=period&0x00FF;
ram_data[DARWIN_SMART_CHARGER_PERIOD_H]=(period&0xFF00)>>8;
@@ -94,8 +82,8 @@ 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)
{
- //uint8_t i;
- uint16_t period,i;
+ uint16_t period;
+
//Enable/Disable smart charger
if(ram_in_range(DARWIN_SMART_CHARGER_CNTRL,address,length))
@@ -115,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,37 +112,28 @@ 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_TX);//amarillo
- counter++;
+{
+ //gpio_set_led(LED_3);
uint8_t error;
+ 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;
}
diff --git a/src/stairs.c b/src/stairs.c
new file mode 100755
index 0000000000000000000000000000000000000000..68795d993ac479c7ea8754f35b6f40b2b1a4ccb0
--- /dev/null
+++ b/src/stairs.c
@@ -0,0 +1,580 @@
+#include "stairs.h"
+#include "darwin_kinematics.h"
+#include "darwin_math.h"
+#include "ram.h"
+#include <math.h>
+
+typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90} stairs_phase_t;
+
+// internal motion variables
+float stairs_Z_stair_height;
+float stairs_Z_overshoot;
+float stairs_Y_shift_amplitude;
+float stairs_X_shift_amplitude;
+float stairs_R_shift_amplitude;
+float stairs_P_shift_amplitude;
+float stairs_A_shift_amplitude;
+float stairs_Y_spread_amplitude;
+float stairs_X_shift_body;
+
+// internal offset variables
+float stairs_Hip_Pitch_Offset;
+float stairs_X_Offset;
+float stairs_Y_Offset;
+float stairs_Z_Offset;
+float stairs_R_Offset;
+float stairs_P_Offset;
+float stairs_A_Offset;
+
+// internal time variables
+float stairs_Time;
+float stairs_period;
+float stairs_shift_weight_left_time;
+float stairs_rise_right_foot_time;
+float stairs_advance_right_foot_time;
+float stairs_contact_right_foot_time;
+float stairs_shift_weight_right_time;
+float stairs_rise_left_foot_time;
+float stairs_advance_left_foot_time;
+float stairs_contact_left_foot_time;
+float stairs_center_weight_time;
+
+// control variables
+uint8_t stairs_Ctrl_Running;
+uint8_t stairs_up;
+
+// private functions
+
+// public functions
+void stairs_init(uint16_t period_us)
+{
+ // initialize the internal motion variables
+
+
+ // initialize internal control variables
+ stairs_Time=0;
+ stairs_period=((float)period_us)/1000.0;
+ stairs_Ctrl_Running=0x00;
+ stairs_up=0x00;
+}
+
+inline void stairs_set_period(uint16_t period_us)
+{
+ stairs_period=((float)period_us)/1000.0;
+}
+
+inline uint16_t stairs_get_period(void)
+{
+ return (uint16_t)(stairs_period*1000);
+}
+
+void stairs_start(uint8_t up)
+{
+ // load all parameters
+ stairs_shift_weight_left_time=((float)ram_data[DARWIN_STAIRS_PHASE1_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE1_TIME_H]<<8));
+ stairs_rise_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE2_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE2_TIME_H]<<8));
+ stairs_advance_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE3_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE3_TIME_H]<<8));
+ stairs_contact_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE4_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE4_TIME_H]<<8));
+ stairs_shift_weight_right_time=((float)ram_data[DARWIN_STAIRS_PHASE5_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE5_TIME_H]<<8));
+ stairs_rise_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE6_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE6_TIME_H]<<8));
+ stairs_advance_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE7_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE7_TIME_H]<<8));
+ stairs_contact_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE8_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE8_TIME_H]<<8));
+ stairs_center_weight_time=((float)ram_data[DARWIN_STAIRS_PHASE9_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE9_TIME_H]<<8));
+ stairs_Z_stair_height=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_HEIGHT]));
+ stairs_Z_overshoot=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OVERSHOOT]));
+ stairs_Y_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SHIFT]));
+ stairs_X_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT]));
+ stairs_Hip_Pitch_Offset=((float)((int16_t)(ram_data[DARWIN_WALK_HIP_PITCH_OFF_L]+(ram_data[DARWIN_WALK_HIP_PITCH_OFF_H]<<8))))/1024.0;
+ stairs_R_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_R_SHIFT]))*PI/720.0;// (r_shift/4)*(pi/180)
+ stairs_P_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_P_SHIFT]))*PI/720.0;// (p_shift/4)*(pi/180)
+ stairs_A_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_A_SHIFT]))*PI/720.0;// (a_shift/4)*(pi/180)
+ stairs_Y_spread_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SPREAD]));
+ stairs_X_shift_body=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT_BODY]));
+ stairs_X_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]));
+ stairs_Y_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]));
+ stairs_Z_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]));
+ stairs_R_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]))*PI/1440.0;
+ stairs_P_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]))*PI/1440.0;
+ stairs_A_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]))*PI/1440.0;
+ // start operation
+ ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS;
+ stairs_Ctrl_Running=0x01;
+ stairs_up=up;
+}
+
+void stairs_stop(void)
+{
+ stairs_Ctrl_Running=0x00;
+}
+
+uint8_t is_climbing_stairs(void)
+{
+ if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
+ return 0x01;
+ else
+ return 0x00;
+}
+
+uint8_t stairs_get_phase(void)
+{
+ return (int8_t)ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_PHASE;
+}
+
+// motion manager interface functions
+void stairs_process(void)
+{
+ float angle[14]={0},ep[12]={0};
+ float delta;
+
+ if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
+ {
+ ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE);
+ if(stairs_up)
+ {
+ if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
+ {
+ //1
+ delta=stairs_Time/stairs_shift_weight_left_time;
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
+ ep[8]=stairs_Z_Offset;
+ ep[9]=stairs_R_Offset;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT;
+ }
+ else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time)
+ {
+ //2
+ delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time)
+ {
+ //3
+ delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time)
+ {
+ //4
+ delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time)
+ {
+ //5
+ delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body*delta;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-3.0*stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-3.0*stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
+ }
+ else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time)
+ {
+ //6
+ delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body;
+ ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
+ ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time)
+ {
+ //7
+ delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta;
+ ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time)
+ {
+ //8
+ delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time)
+ {
+ //9
+ delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
+ ep[3]=stairs_R_Offset;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
+ ep[9]=stairs_R_Offset;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT;
+ }
+ else
+ {
+ ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
+ stairs_Ctrl_Running=0x00;
+ }
+ }
+ else
+ {
+ if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
+ {
+ //1
+ delta=stairs_Time/stairs_shift_weight_left_time;
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height*delta;
+ ep[3]=stairs_R_Offset;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height*delta;
+ ep[9]=stairs_R_Offset;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT;
+ }
+ else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time)
+ {
+ //2
+ delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time)
+ {
+ //3
+ delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time)
+ {
+ //4
+ delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot*delta;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_stair_height*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time)
+ {
+ //5
+ delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body*delta;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
+ }
+ else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time)
+ {
+ //6
+ delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset-1.0*stairs_P_shift_amplitude;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot-stairs_Z_overshoot*delta;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset-1.0*stairs_P_shift_amplitude;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time)
+ {
+ //7
+ delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;//+stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset-1.0*stairs_P_shift_amplitude+1.0*stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[8]=stairs_Z_Offset;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;//+stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset-1.0*stairs_P_shift_amplitude+1.0*stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time)
+ {
+ //8
+ delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-(stairs_Z_stair_height+stairs_Z_overshoot)*delta;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time)
+ {
+ //9
+ delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
+ ep[8]=stairs_Z_Offset;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT;
+ }
+ else
+ {
+ ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
+ stairs_Ctrl_Running=0x00;
+ }
+ }
+
+ if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
+ stairs_Time += stairs_period;
+ else
+ stairs_Time=0;
+
+ // Compute angles with the inverse kinematics
+ if((darwin_leg_ik(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) ||
+ (darwin_leg_ik(&angle[6],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11])!=1))
+ return;// Do not use angles
+
+ // Compute motor value
+ if(manager_get_module(R_HIP_YAW)==MM_WALKING)
+ {
+ manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0;
+ manager_current_slopes[R_HIP_YAW]=5;
+ }
+ if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
+ {
+ manager_current_angles[R_HIP_ROLL]=((-180.0*angle[1])/PI)*65536.0;
+ manager_current_slopes[R_HIP_ROLL]=5;
+ }
+ if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
+ {
+ manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-stairs_Hip_Pitch_Offset)*65536.0;
+ manager_current_slopes[R_HIP_PITCH]=5;
+ }
+ if(manager_get_module(R_KNEE)==MM_WALKING)
+ {
+ manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0;
+ manager_current_slopes[R_KNEE]=5;
+ }
+ if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
+ {
+ manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
+ manager_current_slopes[R_ANKLE_PITCH]=5;
+ }
+ if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
+ {
+ manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0;
+ manager_current_slopes[R_ANKLE_ROLL]=5;
+ }
+ if(manager_get_module(L_HIP_YAW)==MM_WALKING)
+ {
+ manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
+ manager_current_slopes[L_HIP_YAW]=5;
+ }
+ if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
+ {
+ manager_current_angles[L_HIP_ROLL]=((-180.0*angle[7])/PI)*65536.0;
+ manager_current_slopes[L_HIP_ROLL]=5;
+ }
+ if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
+ {
+ manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+stairs_Hip_Pitch_Offset)*65536.0;
+ manager_current_slopes[L_HIP_PITCH]=5;
+ }
+ if(manager_get_module(L_KNEE)==MM_WALKING)
+ {
+ manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0;
+ manager_current_slopes[L_KNEE]=5;
+ }
+ if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
+ {
+ manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
+ manager_current_slopes[L_ANKLE_PITCH]=5;
+ }
+ if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING)
+ {
+ manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0;
+ manager_current_slopes[L_ANKLE_ROLL]=5;
+ }
+ }
+}
+
+// operation functions
+uint8_t stairs_in_range(unsigned short int address, unsigned short int length)
+{
+ if(ram_in_window(STAIRS_BASE_ADDRESS,STAIRS_MEM_LENGTH,address,length) ||
+ ram_in_window(STAIRS_EEPROM_ADDRESS,STAIRS_EEPROM_LENGTH,address,length))
+ return 0x01;
+ else
+ return 0x00;
+}
+
+void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+
+}
+
+void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ uint16_t i;
+
+ // walk control
+ if(ram_in_range(DARWIN_STAIRS_CNTRL,address,length))
+ {
+ if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_UP)
+ stairs_start(0x01);
+ if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_DOWN)
+ stairs_start(0x00);
+ if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_STOP)
+ stairs_stop();
+ }
+ for(i=STAIRS_EEPROM_ADDRESS;i<=STAIRS_EEPROM_ADDRESS+STAIRS_EEPROM_LENGTH;i++)
+ if(ram_in_range(i,address,length))
+ ram_data[i]=data[i-address];
+}
+
diff --git a/src/test_charger.c b/src/test_charger.c
index 5a547fceff13e6ed90be00b86f97142b1f391d35..23cb9b869118c00872faf71c98f56b5019823381 100644
--- a/src/test_charger.c
+++ b/src/test_charger.c
@@ -74,7 +74,7 @@ int main(void)
gpio_set_led(LED_3);//Si existen dispositivos conectados -> luz verde
}
-
+/*
////READ CHARGER STATUS
unsigned short int address=DARWIN_SMART_CHARGER_STATUS;
unsigned short int length=1;
@@ -84,9 +84,9 @@ int main(void)
error=darwin_on_read(address,length,&data_read);
if(data_read&0x01)
gpio_set_led(LED_2); //verde
-
+*/
-/* ////READ PERIOD SMART CHARGER
+ ////READ PERIOD SMART CHARGER
unsigned short int address=DARWIN_SMART_CHARGER_PERIOD_L;
unsigned short int length=2;
unsigned char data_read[2];
@@ -98,7 +98,7 @@ int main(void)
gpio_set_led(LED_3); //verde
else if(dread==0)
gpio_set_led(LED_4); //azul
-*/
+
/* ////READ ID SMART CHARGER
unsigned char data_read2;
@@ -110,7 +110,7 @@ int main(void)
//else if(data_read==0)
*/
-/* ////WRITE PERIOD SMART CHARGER
+ ////WRITE PERIOD SMART CHARGER
//HAL_Delay(500);
unsigned short int address=DARWIN_SMART_CHARGER_PERIOD_L;
unsigned short int length=2;
@@ -126,9 +126,9 @@ int main(void)
gpio_set_led(LED_3); //verde
else if(dread==1500)
gpio_set_led(LED_4); //azul
-*/
-/* ////ENABLE SMART CHARGER MODULE
+
+ ////ENABLE SMART CHARGER MODULE
address=DARWIN_SMART_CHARGER_CNTRL;
length=1;
uint8_t data_read;
@@ -145,14 +145,21 @@ int main(void)
darwin_on_read(address,length,&data_read);
if(data_read==3) //Smart charger control = detected + enabled
gpio_set_led(LED_3); //verde
-*/
+
-/* ////ENABLE MOTION MANAGER TIMER
+ ////ENABLE MOTION MANAGER TIMER
address=DARWIN_MM_CNTRL;
data_write=MANAGER_ENABLE;
darwin_on_write(address,length,&data_write);
HAL_Delay(500);
-*/
+
+ ////WRITE LIMIT CURRENT
+/* address=DARWIN_SMART_CHARGER_LIMIT_CURRENT_L;
+ length=2;
+ data_write=0.512;
+ darwin_on_write(address,length,&data_write);
+ HAL_Delay(500);
+*/
/* //Read battery status
address=DARWIN_BATT_AVG_TIME_EMPTY_L;