diff --git a/include/action.h b/include/action.h index 29e4eff35f432037a2857d8a118833f3e1420acb..afaa86108ce32495aee2d520c819f092ff71221e 100755 --- a/include/action.h +++ b/include/action.h @@ -2,9 +2,6 @@ #define _ACTION_H #include "stm32f10x.h" -#include "motion_manager.h" - -extern int64_t action_current_angles[MANAGER_MAX_NUM_SERVOS]; // public functions void action_init(uint16_t period); diff --git a/include/comm.h b/include/comm.h new file mode 100644 index 0000000000000000000000000000000000000000..c5e404e9005fd2c665384daab6bcfb129c862a87 --- /dev/null +++ b/include/comm.h @@ -0,0 +1,10 @@ +#ifndef _COMM_H +#define _COMM_H + +#include "stm32f10x.h" + +// public functions +void comm_init(void); +void comm_start(void); + +#endif diff --git a/include/dyn_servos.h b/include/dyn_servos.h index 775f26d6ccbede393c8efa80a9da52f2c6c558e9..eb6619d703503564800a70cc96362e49913d21bd 100755 --- a/include/dyn_servos.h +++ b/include/dyn_servos.h @@ -12,6 +12,7 @@ #define SERVO_MX64 0x0136 #define SERVO_EX106 0x006B #define SERVO_MX106 0x0140 +#define SERVO_XL320 0x015E // Servo register map typedef enum{ @@ -88,4 +89,50 @@ typedef enum{ P_D_ERROR_OUT_L = 66, P_D_ERROR_OUT_H = 67}TDynServo; +typedef enum{ + XL_MODEL_NUMBER_L = 0, + XL_MODEL_NUMBER_H = 1, + XL_VERSION = 2, + XL_ID = 3, + XL_BAUD_RATE = 4, + XL_RETURN_DELAY_TIME = 5, + XL_CW_ANGLE_LIMIT_L = 6, + XL_CW_ANGLE_LIMIT_H = 7, + XL_CCW_ANGLE_LIMIT_L = 8, + XL_CCW_ANGLE_LIMIT_H = 9, + XL_SYSTEM_DATA2 = 10, + XL_CONTROL_MODE = 11, + XL_HIGH_LIMIT_TEMPERATURE = 12, + XL_LOW_LIMIT_VOLTAGE = 13, + XL_HIGH_LIMIT_VOLTAGE = 14, + XL_MAX_TORQUE_L = 15, + XL_MAX_TORQUE_H = 16, + XL_RETURN_LEVEL = 17, + XL_ALARM_SHUTDOWN = 18, + XL_TORQUE_ENABLE = 24, + XL_LED = 25, + XL_D_GAIN = 27, + XL_I_GAIN = 28, + XL_P_GAIN = 29, + XL_GOAL_POSITION_L = 30, + XL_GOAL_POSITION_H = 31, + XL_MOVING_SPEED_L = 32, + XL_MOVING_SPEED_H = 33, + XL_TORQUE_LIMIT_L = 35, + XL_TORQUE_LIMIT_H = 36, + XL_PRESENT_POSITION_L = 37, + XL_PRESENT_POSITION_H = 38, + XL_PRESENT_SPEED_L = 39, + XL_PRESENT_SPEED_H = 40, + XL_PRESENT_LOAD_L = 41, + XL_PRESENT_LOAD_H = 42, + XL_PRESENT_VOLTAGE = 45, + XL_PRESENT_TEMPERATURE = 46, + XL_REGISTERED_INSTRUCTION = 47, + XL_MOVING = 49, + XL_HARDWARE_ERROR_STATUS = 50, + XL_PUNCH_L = 51, + XL_PUNCH_H = 51}TXLServo; + + #endif diff --git a/include/dynamixel_master_uart_dma.h b/include/dynamixel_master_uart_dma.h index 1a2916a3057bbbce52ced79291e9c644ee969a7e..cf0e788b23dd0109244c420b1060130aa72876be 100755 --- a/include/dynamixel_master_uart_dma.h +++ b/include/dynamixel_master_uart_dma.h @@ -3,25 +3,43 @@ #include "stm32f10x.h" #include "dynamixel.h" +#include "dynamixel2.h" + +typedef struct{ + uint8_t id; + uint8_t address; + uint8_t length; + uint8_t *data; +}TBulkData; // dynamixel master functions void dyn_master_init(void); -void dyn_master_flush(void); +void dyn_master_reset(void); void dyn_master_set_timeout(uint16_t timeout_ms); void dyn_master_scan(uint8_t *num,uint8_t *ids); +void dyn2_master_scan(uint8_t *num,uint8_t *ids); inline void dyn_master_enable_power(void); inline void dyn_master_disable_power(void); uint8_t dyn_master_is_power_enabled(void); uint8_t dyn_master_ping(uint8_t id); +uint8_t dyn2_master_ping(uint8_t id); uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data); +uint8_t dyn2_master_read_byte(uint8_t id,uint16_t address,uint8_t *data); uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data); +uint8_t dyn2_master_read_word(uint8_t id,uint16_t address,uint16_t *data); uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data); +uint8_t dyn2_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data); uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data); +uint8_t dyn2_master_write_byte(uint8_t id, uint16_t address, uint8_t data); uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data); +uint8_t dyn2_master_write_word(uint8_t id, uint16_t address, uint16_t data); uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data); +uint8_t dyn2_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data); uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data); void dyn_master_action(void); void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data); +void dyn2_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, uint8_t *data); +uint8_t dyn_master_bulk_read(uint8_t num,TBulkData *info); // repeater functions uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet); diff --git a/include/grippers.h b/include/grippers.h new file mode 100644 index 0000000000000000000000000000000000000000..af3f37f90de33173e70d983b715acc2e2f51aaf8 --- /dev/null +++ b/include/grippers.h @@ -0,0 +1,14 @@ +#ifndef _GRIPPERS_H +#define _GRIPPERS_H + +#include "stm32f10x.h" + +typedef enum {LEFT_GRIPPER=0,RIGHT_GRIPPER=1} grippers_t; + +/* public functions */ +void grippers_init(void); +void grippers_set_force(grippers_t gripper,uint8_t force); +void grippers_open(grippers_t gripper); +void grippers_close(grippers_t gripper); + +#endif diff --git a/include/head_tracking.h b/include/head_tracking.h new file mode 100644 index 0000000000000000000000000000000000000000..ddba99bcf5aed0eb8862c827514aa58039072480 --- /dev/null +++ b/include/head_tracking.h @@ -0,0 +1,23 @@ +#ifndef _HEAD_TRACKING_H +#define _HEAD_TRACKINH_H + +#include "stm32f10x.h" + +// public functions +void head_tracking_init(uint16_t period); +void head_tracking_start(void); +void head_tracking_stop(void); +void head_tracking_set_period(int16_t period); +void head_tracking_set_pan_p(uint8_t value); +void head_tracking_set_pan_i(uint8_t value); +void head_tracking_set_pan_d(uint8_t value); +void head_tracking_set_tilt_p(uint8_t value); +void head_tracking_set_tilt_i(uint8_t value); +void head_tracking_set_tilt_d(uint8_t value); +void head_tracking_set_target_pan(int16_t target); +void head_tracking_set_target_tilt(int16_t target); + +// motion manager process function +void head_tracking_process(void); + +#endif diff --git a/include/imu.h b/include/imu.h index ef91bc9a17d61977a7815d79e9e19a64893f876b..65909fa11cd71face5c6b9ec144ae790fcf3e433 100755 --- a/include/imu.h +++ b/include/imu.h @@ -7,6 +7,8 @@ void imu_init(void); void imu_start(void); void imu_stop(void); -void imu_accel_get_data(void); + +void imu_start_gyro_cal(void); +void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z); #endif diff --git a/include/joint_motion.h b/include/joint_motion.h new file mode 100644 index 0000000000000000000000000000000000000000..654f9d9df99b2592984781b34de134550fa21828 --- /dev/null +++ b/include/joint_motion.h @@ -0,0 +1,20 @@ +#ifndef _JOINT_MOTION_H +#define _JOINT_MOTION_H + +#include "stm32f10x.h" + +// public functions +void joint_motion_init(uint16_t period); +void joint_motion_set_period(uint16_t period); +uint16_t joint_motion_get_period(void); +inline void joint_motion_load_target_angle(uint8_t servo_id, int16_t angle); +inline void joint_motion_load_target_speed(uint8_t servo_id, uint8_t speed); +inline void joint_motion_load_target_accel(uint8_t servo_id, uint8_t accel); +void joint_motion_start(void); +void joint_motion_stop(void); +uint8_t are_joints_moving(void); + +// motion manager interface functions +void joint_motion_process(void); + +#endif diff --git a/include/motion_manager.h b/include/motion_manager.h index 050812e51c95429eaba92c2a653af63cc7f5ecc9..0d6c86fee6bc0b32ac3de4635d26007d11063a61 100755 --- a/include/motion_manager.h +++ b/include/motion_manager.h @@ -8,7 +8,9 @@ typedef enum{MM_NONE = 0, MM_ACTION = 1, MM_WALKING = 2, - MM_JOINTS = 3} TModules; + MM_JOINTS = 3, + MM_HEAD = 4, + MM_GRIPPER = 5} TModules; // default joint identifiers enum{ @@ -31,7 +33,11 @@ enum{ R_ANKLE_ROLL = 17, L_ANKLE_ROLL = 18, HEAD_PAN = 19, - HEAD_TILT = 20 + HEAD_TILT = 20, + R_WRIST = 23, + L_WRIST = 22, + R_GRIPPER = 21, + L_GRIPPER = 24 }; // servo information structure @@ -47,8 +53,14 @@ typedef struct{ uint16_t current_value; TModules module; uint8_t enabled; + int16_t cw_angle_limit; + int16_t ccw_angle_limit; + uint8_t dyn_version; }TServoInfo; +// public variables +extern int64_t current_angles[MANAGER_MAX_NUM_SERVOS]; + // public functions void manager_init(uint16_t period_us); inline uint16_t manager_get_period(void); @@ -56,6 +68,8 @@ inline void manager_set_period(uint16_t period_us); inline void manager_enable(void); 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_get_num_servos(void); inline void manager_set_module(uint8_t servo_id,TModules module); inline TModules manager_get_module(uint8_t servo_id); @@ -63,5 +77,12 @@ inline void manager_enable_servo(uint8_t servo_id); inline void manager_disable_servo(uint8_t servo_id); void manager_disable_servos(void); inline uint8_t manager_is_servo_enabled(uint8_t servo_id); +inline int16_t manager_get_cw_angle_limit(uint8_t servo_id); +inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id); +void manager_set_knee_gain(uint8_t gain); +void manager_set_ankle_pitch_gain(uint8_t gain); +void manager_set_hip_roll_gain(uint8_t gain); +void manager_set_ankle_roll_gain(uint8_t gain); +uint8_t manager_get_dyn_version(uint8_t servo_id); #endif diff --git a/include/ram.h b/include/ram.h index 335bdaed2b87735e3936547351030b8cf3e1eef8..737a71b94a99266b57b9156f03cc16cfebf2f419 100755 --- a/include/ram.h +++ b/include/ram.h @@ -17,7 +17,8 @@ typedef enum { DARWIN_ID = DEVICE_ID_OFFSET, DARWIN_BAUD_RATE = BAUDRATE_OFFSET, DARWIN_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET, - DARWIN_MM_PERIOD = MM_PERIOD_OFFSET, + DARWIN_MM_PERIOD_L = MM_PERIOD_OFFSET, + DARWIN_MM_PERIOD_H = MM_PERIOD_OFFSET+1, DARWIN_RETURN_LEVEL = RETURN_LEVEL_OFFSET, DARWIN_DXL_POWER = 0x18, DARWIN_LED_PANNEL = 0x19, @@ -146,47 +147,114 @@ typedef enum { DARWIN_MM_SERVO25_CUR_POS_H = 0x94, DARWIN_MM_SERVO26_CUR_POS_L = 0x95, DARWIN_MM_SERVO26_CUR_POS_H = 0x96, - DARWIN_MM_SERVO27_CUR_POS_L = 0x97, - DARWIN_MM_SERVO27_CUR_POS_H = 0x98, - DARWIN_MM_SERVO28_CUR_POS_L = 0x99, - DARWIN_MM_SERVO28_CUR_POS_H = 0x9A, - DARWIN_MM_SERVO29_CUR_POS_L = 0x9B, - DARWIN_MM_SERVO29_CUR_POS_H = 0x9C, - DARWIN_MM_SERVO30_CUR_POS_L = 0x9D, - DARWIN_MM_SERVO30_CUR_POS_H = 0x9E, - DARWIN_ACTION_PAGE = 0x9F, - DARWIN_ACTION_CONTROL = 0xA0, - DARWIN_ACTION_STATUS = 0xA1, - DARWIN_X_OFFSET = 0xA2, - DARWIN_Y_OFFSET = 0xA3, - DARWIN_Z_OFFSET = 0xA4, - DARWIN_ROLL = 0xA5, - DARWIN_PITCH = 0xA6, - DARWIN_YAW = 0xA7, - DARWIN_HIP_PITCH_OFF_L = 0xA8, - DARWIN_HIP_PITCH_OFF_H = 0xA9, - DARWIN_PERIOD_TIME_L = 0xAA, - DARWIN_PERIOD_TIME_H = 0xAB, - DARWIN_DSP_RATIO = 0xAC, - DARWIN_STEP_FW_BW_RATIO = 0xAD, - DARWIN_STEP_FW_BW = 0xAE, - DARWIN_STEP_LEFT_RIGHT = 0xAF, - DARWIN_STEP_DIRECTION = 0xB1, - DARWIN_FOOT_HEIGHT = 0xB2, - DARWIN_SWING_RIGHT_LEFT = 0xB3, - DARWIN_SWING_TOP_DOWN = 0xB4, - DARWIN_PELVIS_OFFSET = 0xB5, - DARWIN_ARM_SWING_GAIN = 0xB6, - DARWIN_BAL_KNEE_GAIN = 0xB7, - DARWIN_BAL_ANKLE_PITCH_GAIN = 0xB8, - DARWIN_BAL_HIP_ROLL_GAIN = 0xB9, - DARWIN_BAL_ANKLE_ROLL_GAIN = 0xBA, - DARWIN_P_GAIN = 0xBB, - DARWIN_D_GAIN = 0xBC, - DARWIN_I_GAIN = 0xBD + DARWIN_ACTION_PAGE = 0x97, + DARWIN_ACTION_CONTROL = 0x98, + DARWIN_ACTION_STATUS = 0x99, + DARWIN_X_OFFSET = 0x9A, + DARWIN_Y_OFFSET = 0x9B, + DARWIN_Z_OFFSET = 0x9C, + DARWIN_ROLL = 0x9D, + DARWIN_PITCH = 0x9E, + DARWIN_YAW = 0x9F, + DARWIN_HIP_PITCH_OFF_L = 0xA0, + DARWIN_HIP_PITCH_OFF_H = 0xA1, + DARWIN_PERIOD_TIME_L = 0xA2, + DARWIN_PERIOD_TIME_H = 0xA3, + DARWIN_DSP_RATIO = 0xA4, + DARWIN_STEP_FW_BW_RATIO = 0xA5, + DARWIN_STEP_FW_BW = 0xA6, + DARWIN_STEP_LEFT_RIGHT = 0xA7, + DARWIN_STEP_DIRECTION = 0xA8, + DARWIN_FOOT_HEIGHT = 0xA9, + DARWIN_SWING_RIGHT_LEFT = 0xAA, + DARWIN_SWING_TOP_DOWN = 0xAB, + DARWIN_PELVIS_OFFSET = 0xAC, + DARWIN_ARM_SWING_GAIN = 0xAD, + DARWIN_P_GAIN = 0xAE, + DARWIN_D_GAIN = 0xAF, + DARWIN_I_GAIN = 0xB0, + DARWIN_MAX_ACC = 0xB1, + DARWIN_MAX_ROT_ACC = 0xB2, + DARWIN_MM_KNEE_GAIN = 0xB3, + DARWIN_MM_ANKLE_PITCH_GAIN = 0xB4, + DARWIN_MM_HIP_ROLL_GAIN = 0xB5, + DARWIN_MM_ANKLE_ROLL_GAIN = 0xB6, + DARWIN_SERVO_0_SPEED = 0xB7, + DARWIN_SERVO_1_SPEED = 0xB8, + DARWIN_SERVO_2_SPEED = 0xB9, + DARWIN_SERVO_3_SPEED = 0xBA, + DARWIN_SERVO_4_SPEED = 0xBB, + DARWIN_SERVO_5_SPEED = 0xBC, + DARWIN_SERVO_6_SPEED = 0xBD, + DARWIN_SERVO_7_SPEED = 0xBE, + DARWIN_SERVO_8_SPEED = 0xBF, + DARWIN_SERVO_9_SPEED = 0xC0, + DARWIN_SERVO_10_SPEED = 0xC1, + DARWIN_SERVO_11_SPEED = 0xC2, + DARWIN_SERVO_12_SPEED = 0xC3, + DARWIN_SERVO_13_SPEED = 0xC4, + DARWIN_SERVO_14_SPEED = 0xC5, + DARWIN_SERVO_15_SPEED = 0xC6, + DARWIN_SERVO_16_SPEED = 0xC7, + DARWIN_SERVO_17_SPEED = 0xC8, + DARWIN_SERVO_18_SPEED = 0xC9, + DARWIN_SERVO_19_SPEED = 0xCA, + DARWIN_SERVO_20_SPEED = 0xCB, + DARWIN_SERVO_21_SPEED = 0xCC, + DARWIN_SERVO_22_SPEED = 0xCD, + DARWIN_SERVO_23_SPEED = 0xCE, + DARWIN_SERVO_24_SPEED = 0xCF, + DARWIN_SERVO_25_SPEED = 0xD0, + DARWIN_SERVO_26_SPEED = 0xD1, + DARWIN_SERVO_0_ACCEL = 0xD2, + DARWIN_SERVO_1_ACCEL = 0xD3, + DARWIN_SERVO_2_ACCEL = 0xD4, + DARWIN_SERVO_3_ACCEL = 0xD5, + DARWIN_SERVO_4_ACCEL = 0xD6, + DARWIN_SERVO_5_ACCEL = 0xD7, + DARWIN_SERVO_6_ACCEL = 0xD8, + DARWIN_SERVO_7_ACCEL = 0xD9, + DARWIN_SERVO_8_ACCEL = 0xDA, + DARWIN_SERVO_9_ACCEL = 0xDB, + DARWIN_SERVO_10_ACCEL = 0xDC, + DARWIN_SERVO_11_ACCEL = 0xDD, + DARWIN_SERVO_12_ACCEL = 0xDE, + DARWIN_SERVO_13_ACCEL = 0xDF, + DARWIN_SERVO_14_ACCEL = 0xE0, + DARWIN_SERVO_15_ACCEL = 0xE1, + DARWIN_SERVO_16_ACCEL = 0xE2, + DARWIN_SERVO_17_ACCEL = 0xE3, + DARWIN_SERVO_18_ACCEL = 0xE4, + DARWIN_SERVO_19_ACCEL = 0xE5, + DARWIN_SERVO_20_ACCEL = 0xE6, + DARWIN_SERVO_21_ACCEL = 0xE7, + DARWIN_SERVO_22_ACCEL = 0xE8, + DARWIN_SERVO_23_ACCEL = 0xE9, + DARWIN_SERVO_24_ACCEL = 0xEA, + DARWIN_SERVO_25_ACCEL = 0xEB, + DARWIN_SERVO_26_ACCEL = 0xEC, + DARWIN_JOINTS_CONTROL = 0xED, + DARWIN_JOINTS_STATUS = 0xEE, + DARWIN_HEAD_PAN_L = 0xEF, + DARWIN_HEAD_PAN_H = 0xF0, + DARWIN_HEAD_TILT_L = 0xF1, + DARWIN_HEAD_TILT_H = 0xF2, + DARWIN_HEAD_PAN_P = 0xF3, + DARWIN_HEAD_PAN_I = 0xF4, + DARWIN_HEAD_PAN_D = 0xF5, + DARWIN_HEAD_TILT_P = 0xF6, + DARWIN_HEAD_TILT_I = 0xF7, + DARWIN_HEAD_TILT_D = 0xF8, + DARWIN_HEAD_CONTROL = 0xF9, + DARWIN_HEAD_STATUS = 0xFA, + DARWIN_MM_CUR_PERIOD = 0xFB, + DARWIN_GRIPPER_CONTROL = 0xFC, + DARWIN_GRIPPER_STATUS = 0xFD, + DARWIN_GRIPPER_FORCE_LEFT = 0xFE, + DARWIN_GRIPPER_FORCE_RIGHT = 0xFF } darwin_registers; -#define RAM_SIZE 256 +#define RAM_SIZE ((uint16_t)0x100) extern uint8_t ram_data[RAM_SIZE]; diff --git a/include/walking.h b/include/walking.h index 0b7e4773c19c089450704b25897cc6a1fe9bd8d7..da933a5efbd30306b01a35ca7e868b4c01d75b6d 100755 --- a/include/walking.h +++ b/include/walking.h @@ -2,19 +2,12 @@ #define _WALKING_H #include "stm32f10x.h" -#include "motion_manager.h" - -extern int64_t walking_current_angles[MANAGER_MAX_NUM_SERVOS]; // public functions -void walking_init(void); -void walking_set_param(uint8_t param_id,uint16_t param_value); -void walking_enable_autobalance(void); -void walking_disable_autobalance(void); -uint8_t walking_is_autobalance_enabled(void); -void walking_enable_aim(void); -void walking_disable_aim(void); -uint8_t walking_is_aim_enabled(void); +void walking_init(uint16_t period); +inline void walking_set_period(uint16_t period); +inline uint16_t walking_get_period(void); +void walking_set_param(uint8_t param_id,int8_t param_value); void walking_start(void); void walking_stop(void); uint8_t is_walking(void); diff --git a/src/action.c b/src/action.c index cc0429d99bb803ffc144cbd5a5e4ae280e5ed584..67ff5ba94832bf05191203d3cb5495eec66b18f2 100755 --- a/src/action.c +++ b/src/action.c @@ -1,5 +1,6 @@ #include "action.h" #include "motion_pages.h" +#include "motion_manager.h" #include "ram.h" #define SPEED_BASE_SCHEDULE 0x00 @@ -13,7 +14,6 @@ TPage action_current_page; uint8_t action_current_step_index; // angle variables int64_t action_moving_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format -int64_t action_current_angles[PAGE_MAX_NUM_SERVOS]; int64_t action_start_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format // speed variables int64_t action_start_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format @@ -126,46 +126,49 @@ void action_load_next_step(void) max_angle=0; for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) { - angle=action_current_page.steps[action_current_step_index].position[i]; - if(angle==0x5A00)// bigger than 180 - target_angles=action_current_angles[i]; - else - target_angles=angle<<9; - action_moving_angles[i]=target_angles-action_current_angles[i]; - if(action_end) - next_angles=target_angles; - else + if(manager_get_module(i)==MM_ACTION) { - if(action_current_step_index==action_current_page.header.stepnum-1) - next_angle=action_next_page.steps[0].position[i]; + angle=action_current_page.steps[action_current_step_index].position[i]; + if(angle==0x5A00)// bigger than 180 + target_angles=current_angles[i]; else - next_angle=action_current_page.steps[action_current_step_index+1].position[i]; - if(next_angle==0x5A00) + target_angles=angle<<9; + action_moving_angles[i]=target_angles-current_angles[i]; + if(action_end) next_angles=target_angles; else - next_angles=next_angle<<9; - } - // check changes in direction - if(((action_current_angles[i] < target_angles) && (target_angles < next_angles)) || - ((action_current_angles[i] > target_angles) && (target_angles > next_angles))) - dir_change=0x00; - else - dir_change=0x01; - // check the type of ending - if(dir_change || action_pause_time>0 || action_end) - action_zero_speed_finish[i]=0x01; - else - action_zero_speed_finish[i]=0x00; - // find the maximum angle of motion in speed base schedule - if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule - { - // fer el valor absolut - if(action_moving_angles[i]<0) - tmp_angle=-action_moving_angles[i]; + { + if(action_current_step_index==action_current_page.header.stepnum-1) + next_angle=action_next_page.steps[0].position[i]; + else + next_angle=action_current_page.steps[action_current_step_index+1].position[i]; + if(next_angle==0x5A00) + next_angles=target_angles; + else + next_angles=next_angle<<9; + } + // check changes in direction + if(((current_angles[i] < target_angles) && (target_angles < next_angles)) || + ((current_angles[i] > target_angles) && (target_angles > next_angles))) + dir_change=0x00; + else + dir_change=0x01; + // check the type of ending + if(dir_change || action_pause_time>0 || action_end) + action_zero_speed_finish[i]=0x01; else - tmp_angle=action_moving_angles[i]; - if(tmp_angle>max_angle) - max_angle=tmp_angle; + action_zero_speed_finish[i]=0x00; + // find the maximum angle of motion in speed base schedule + if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule + { + // fer el valor absolut + if(action_moving_angles[i]<0) + tmp_angle=-action_moving_angles[i]; + else + tmp_angle=action_moving_angles[i]; + if(tmp_angle>max_angle) + max_angle=tmp_angle; + } } } if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE) @@ -194,16 +197,19 @@ void action_load_next_step(void) zero_speed_divider=action_period; for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) { - action_pre_time_initial_speed_angle=(action_start_speed[i]*action_pre_time)>>1; - moving_angle=action_moving_angles[i]<<16; - if(action_zero_speed_finish[i]) - action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider; - else - action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider; -// if((action_main_speed[i]>>16)>info.max_speed) -// action_main_speed[i]=(info.max_speed*65536); -// else if((action_main_speed[i]>>16)<-info.max_speed) -// action_main_speed[i]=-(info.max_speed*65536); + if(manager_get_module(i)==MM_ACTION) + { + action_pre_time_initial_speed_angle=(action_start_speed[i]*action_pre_time)>>1; + moving_angle=action_moving_angles[i]<<16; + if(action_zero_speed_finish[i]) + action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider; + else + action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider; +// if((action_main_speed[i]>>16)>info.max_speed) +// action_main_speed[i]=(info.max_speed*65536); +// else if((action_main_speed[i]>>16)<-info.max_speed) +// action_main_speed[i]=-(info.max_speed*65536); + } } } @@ -217,7 +223,7 @@ void action_init(uint16_t period) { // angle variables action_moving_angles[i]=0;// fixed point 48|16 format - action_start_angles[i]=action_current_angles[i]; + action_start_angles[i]=current_angles[i]; // speed variables action_start_speed[i]=0;// fixed point 48|16 format action_main_speed[i]=0;// fixed point 48|16 format @@ -265,6 +271,7 @@ uint8_t action_load_page(uint8_t page_id) return 0x00; if(!pages_check_checksum(&action_next_page)) return 0x00; + ram_write_byte(DARWIN_ACTION_PAGE,page_id); return 0x01; } @@ -274,18 +281,20 @@ void action_start_page(void) uint8_t i; for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - action_start_angles[i]=action_current_angles[i]; + action_start_angles[i]=current_angles[i]; action_stop=0x00; action_current_time=0; action_section_time=0; action_current_step_index=-1; ram_set_bit(DARWIN_ACTION_STATUS,0); action_running=0x01; + ram_set_bit(DARWIN_ACTION_CONTROL,0); } void action_stop_page(void) { action_stop=0x01; + ram_set_bit(DARWIN_ACTION_CONTROL,1); } uint8_t action_is_running(void) @@ -315,28 +324,31 @@ void action_process(void) { for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) { - /* the last segment of the pre section */ - delta_speed=(action_main_speed[i]-action_start_speed[i]); - current_speed[i]=action_start_speed[i]+delta_speed; - accel_angles[i]=(action_start_speed[i]*action_section_time+((delta_speed*action_section_time)>>1))>>16; - action_current_angles[i]=action_start_angles[i]+accel_angles[i]; - /* update of the state */ - if(!action_zero_speed_finish[i]) + if(manager_get_module(i)==MM_ACTION) { - if((action_step_time-action_pre_time)==0) - main_angles[i]=0; + /* the last segment of the pre section */ + delta_speed=(action_main_speed[i]-action_start_speed[i]); + current_speed[i]=action_start_speed[i]+delta_speed; + accel_angles[i]=(action_start_speed[i]*action_section_time+((delta_speed*action_section_time)>>1))>>16; + current_angles[i]=action_start_angles[i]+accel_angles[i]; + /* update of the state */ + if(!action_zero_speed_finish[i]) + { + if((action_step_time-action_pre_time)==0) + main_angles[i]=0; + else + main_angles[i]=((action_moving_angles[i]-accel_angles[i])*(action_step_time-(action_pre_time<<1)))/(action_step_time-action_pre_time); + action_start_angles[i]=current_angles[i]; + } else - main_angles[i]=((action_moving_angles[i]-accel_angles[i])*(action_step_time-(action_pre_time<<1)))/(action_step_time-action_pre_time); - action_start_angles[i]=action_current_angles[i]; - } - else - { - main_angles[i]=action_moving_angles[i]-accel_angles[i]-(((action_main_speed[i]*action_pre_time)>>1)>>16); - action_start_angles[i]=action_current_angles[i]; + { + main_angles[i]=action_moving_angles[i]-accel_angles[i]-(((action_main_speed[i]*action_pre_time)>>1)>>16); + action_start_angles[i]=current_angles[i]; + } + /* the first step of the main section */ + current_angles[i]=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1)); + current_speed[i]=action_main_speed[i]; } - /* the first step of the main section */ - action_current_angles[i]=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1)); - current_speed[i]=action_main_speed[i]; } action_current_time=action_current_time-action_section_time; action_section_time=action_step_time-(action_pre_time<<1); @@ -346,10 +358,13 @@ void action_process(void) { for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) { - delta_speed=((action_main_speed[i]-action_start_speed[i])*action_current_time)/action_section_time; - current_speed[i]=action_start_speed[i]+delta_speed; - accel_angles[i]=((action_start_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16; - action_current_angles[i]=action_start_angles[i]+accel_angles[i]; + if(manager_get_module(i)==MM_ACTION) + { + delta_speed=((action_main_speed[i]-action_start_speed[i])*action_current_time)/action_section_time; + current_speed[i]=action_start_speed[i]+delta_speed; + accel_angles[i]=((action_start_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16; + current_angles[i]=action_start_angles[i]+accel_angles[i]; + } } state=ACTION_PRE; } @@ -358,23 +373,26 @@ void action_process(void) { for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) { - /* last segment of the main section */ - action_current_angles[i]=action_start_angles[i]+main_angles[i]; - current_speed[i]=action_main_speed[i]; - /* update state */ - action_start_angles[i]=action_current_angles[i]; - main_angles[i]=action_moving_angles[i]-main_angles[i]-accel_angles[i]; - /* first segment of the post section */ - if(action_zero_speed_finish[i]) + if(manager_get_module(i)==MM_ACTION) { - delta_speed=((0.0-action_main_speed[i])*(action_current_time-action_section_time))/action_pre_time; - current_speed[i]=action_main_speed[i]+delta_speed; - action_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16); - } - else - { - action_current_angles[i]=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time); + /* last segment of the main section */ + current_angles[i]=action_start_angles[i]+main_angles[i]; current_speed[i]=action_main_speed[i]; + /* update state */ + action_start_angles[i]=current_angles[i]; + main_angles[i]=action_moving_angles[i]-main_angles[i]-accel_angles[i]; + /* first segment of the post section */ + if(action_zero_speed_finish[i]) + { + delta_speed=((0.0-action_main_speed[i])*(action_current_time-action_section_time))/action_pre_time; + current_speed[i]=action_main_speed[i]+delta_speed; + current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16); + } + else + { + current_angles[i]=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time); + current_speed[i]=action_main_speed[i]; + } } } action_current_time=action_current_time-action_section_time; @@ -385,8 +403,11 @@ void action_process(void) { for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) { - action_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time; - current_speed[i]=action_main_speed[i]; + if(manager_get_module(i)==MM_ACTION) + { + current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time; + current_speed[i]=action_main_speed[i]; + } } state=ACTION_MAIN; } @@ -395,21 +416,24 @@ void action_process(void) { for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) { - /* last segment of the post section */ - if(action_zero_speed_finish[i]) - { - delta_speed=-action_main_speed[i]; - current_speed[i]=action_main_speed[i]+delta_speed; - action_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16); - } - else + if(manager_get_module(i)==MM_ACTION) { - action_current_angles[i]=action_start_angles[i]+main_angles[i]; - current_speed[i]=action_main_speed[i]; + /* last segment of the post section */ + if(action_zero_speed_finish[i]) + { + delta_speed=-action_main_speed[i]; + current_speed[i]=action_main_speed[i]+delta_speed; + current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16); + } + else + { + current_angles[i]=action_start_angles[i]+main_angles[i]; + current_speed[i]=action_main_speed[i]; + } + /* update state */ + action_start_angles[i]=current_angles[i]; + action_start_speed[i]=current_speed[i]; } - /* update state */ - action_start_angles[i]=action_current_angles[i]; - action_start_speed[i]=current_speed[i]; } /* load the next step */ if(action_pause_time==0) @@ -428,10 +452,13 @@ void action_process(void) /* first step of the next section */ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) { - delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time; - current_speed[i]=action_start_speed[i]+delta_speed; - accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16); - action_current_angles[i]=action_start_angles[i]+accel_angles[i]; + if(manager_get_module(i)==MM_ACTION) + { + delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time; + current_speed[i]=action_start_speed[i]+delta_speed; + accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16); + current_angles[i]=action_start_angles[i]+accel_angles[i]; + } } action_current_time=action_current_time-action_section_time; action_section_time=action_pre_time; @@ -449,16 +476,19 @@ void action_process(void) { for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) { - if(action_zero_speed_finish[i]) + if(manager_get_module(i)==MM_ACTION) { - delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time; - current_speed[i]=action_main_speed[i]+delta_speed; - action_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16); - } - else - { - action_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time; - current_speed[i]=action_main_speed[i]; + if(action_zero_speed_finish[i]) + { + delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time; + current_speed[i]=action_main_speed[i]+delta_speed; + current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16); + } + else + { + current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time; + current_speed[i]=action_main_speed[i]; + } } } state=ACTION_POST; @@ -485,7 +515,7 @@ void action_process(void) delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time; current_speed[i]=action_start_speed[i]+delta_speed; accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16); - action_current_angles[i]=action_start_angles[i]+accel_angles[i]; + current_angles[i]=action_start_angles[i]+accel_angles[i]; } action_current_time=action_current_time-action_section_time; action_section_time=action_pre_time; diff --git a/src/adc_dma.c b/src/adc_dma.c index 0076e5f32ace44c880835fac0258df7719291350..f4549cfedcbb9c8f68e2cd02d65a3ad950471522 100755 --- a/src/adc_dma.c +++ b/src/adc_dma.c @@ -99,16 +99,16 @@ void adc_init(void) RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC2, ENABLE); - RCC_APB2PeriphClockCmd(ADC1_CH1_PORT_CLK | ADC1_CH2_PORT_CLK | ADC1_CH3_PORT_CLK | ADC1_CH4_PORT_CLK | + RCC_APB2PeriphClockCmd(ADC1_CH2_PORT_CLK | ADC1_CH3_PORT_CLK | ADC1_CH4_PORT_CLK | ADC1_CH5_PORT_CLK | ADC1_CH6_PORT_CLK | ADC1_CH7_PORT_CLK | ADC1_CH8_PORT_CLK, ENABLE); RCC_APB2PeriphClockCmd(ADC2_CH1_PORT_CLK | ADC2_CH2_PORT_CLK | ADC2_CH3_PORT_CLK | ADC2_CH4_PORT_CLK | - ADC2_CH1_PORT_CLK | ADC2_CH2_PORT_CLK | ADC2_CH3_PORT_CLK | ADC2_CH4_PORT_CLK, ENABLE); + ADC2_CH5_PORT_CLK | ADC2_CH6_PORT_CLK | ADC2_CH7_PORT_CLK | ADC2_CH8_PORT_CLK, ENABLE); /* DMA Config */ DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&ram_data[DARWIN_MIC1_L]; DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)ADC_CCR_ADDRESS; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; - DMA_InitStructure.DMA_BufferSize = 32; + DMA_InitStructure.DMA_BufferSize = 30; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; diff --git a/src/cm730_fw.c b/src/cm730_fw.c index e233c0dbf3c590b6cd628360c05dcebcf98db931..3e3aed3627e23bb28ed5ab02680ffb96bc699a9c 100755 --- a/src/cm730_fw.c +++ b/src/cm730_fw.c @@ -2,139 +2,24 @@ #include "gpio.h" #include "time.h" #include "eeprom.h" -#include "motion_pages.h" #include "motion_manager.h" #include "action.h" #include "walking.h" +#include "joint_motion.h" +#include "head_tracking.h" +#include "grippers.h" #include "dynamixel_slave_uart_dma.h" #include "dynamixel_master_uart_dma.h" #include "ram.h" #include "imu.h" #include "adc_dma.h" - -uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data) -{ - uint8_t error; - - // pre-read operation - // read operation - error=ram_read_table(address,length,data); - // post-read operation - if(ram_in_range(DARWIN_PUSHBUTTON,DARWIN_PUSHBUTTON,address,length)) - ram_data[DARWIN_PUSHBUTTON]=0x00; - - return error; -} - -uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data) -{ - uint8_t error,byte_value,i,j,do_op=0x00; - uint16_t word_value; - - // pre-write operation - if(ram_in_range(DARWIN_DXL_POWER,DARWIN_DXL_POWER,address,length)) - { - if(data[DARWIN_DXL_POWER-address]&0x01) dyn_master_enable_power(); - else dyn_master_disable_power(); - } - if(ram_in_range(DARWIN_LED_PANNEL,DARWIN_LED_PANNEL,address,length)) - { - byte_value=data[DARWIN_LED_PANNEL-address]; - if(byte_value&0x01) - gpio_set_led(LED_2); - else - gpio_clear_led(LED_2); - if(byte_value&0x02) - gpio_set_led(LED_3); - else - gpio_clear_led(LED_3); - if(byte_value&0x04) - gpio_set_led(LED_4); - else - gpio_clear_led(LED_4); - if(byte_value&0x08) - gpio_set_led(LED_TX); - else - gpio_clear_led(LED_TX); - if(byte_value&0x10) - gpio_set_led(LED_RX); - else - gpio_clear_led(LED_RX); - } - if(ram_in_range(DARWIN_MM_PERIOD,DARWIN_MM_PERIOD+1,address,length)) - { - word_value=data[DARWIN_MM_PERIOD-address]+(data[DARWIN_MM_PERIOD+1-address]<<8); - action_set_period(word_value); - word_value=(word_value*1000000)>>16; - manager_set_period(word_value); - } - for(i=DARWIN_MM_MODULE_EN0,j=0;i<=DARWIN_MM_MODULE_EN15;i++,j+=2) - { - do_op=0x01; - if(ram_in_range(i,i,address,length)) - { - byte_value=data[i-address]; - if(byte_value&0x80) manager_enable_servo(j); - else manager_disable_servo(j); - manager_set_module(j,(byte_value&0x70)>>4); - if(byte_value&0x08) manager_enable_servo(j+1); - else manager_disable_servo(j+1); - manager_set_module(j+1,byte_value&0x07); - } - } - if(do_op) - { - do_op=0x00; - manager_disable_servos(); - } - if(ram_in_range(DARWIN_MM_CONTROL,DARWIN_MM_CONTROL,address,length)) - { - if(data[DARWIN_MM_CONTROL-address]&0x01) - manager_enable(); - else - manager_disable(); - } - if(ram_in_range(DARWIN_ACTION_CONTROL,DARWIN_ACTION_CONTROL,address,length)) - { - if(data[DARWIN_ACTION_CONTROL-address]&0x01) - action_start_page(); - if(data[DARWIN_ACTION_CONTROL-address]&0x02) - action_stop_page(); - } - if(ram_in_range(DARWIN_ACTION_PAGE,DARWIN_ACTION_PAGE,address,length))// load the page identifier - action_load_page(data[DARWIN_ACTION_PAGE-address]); - if(ram_in_range(DARWIN_WALK_CONTROL,DARWIN_WALK_CONTROL,address,length)) - { - if(data[DARWIN_WALK_CONTROL-address]&0x01) - walking_start(); - if(data[DARWIN_WALK_CONTROL-address]&0x02) - walking_stop(); - } - if(ram_in_range(DARWIN_IMU_CONTROL,DARWIN_IMU_CONTROL,address,length))// load the page identifier - { - if(data[DARWIN_IMU_CONTROL-address]&0x01) - imu_start(); - else - imu_stop(); - } - // write eeprom - for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++) - EE_WriteVariable(i,data[j]); - // write operation - error=ram_write_table(address,length,data); - // post-write operation - - return error; -} +#include "comm.h" +#include <math.h> int main(void) { - uint8_t inst_packet[1024]; - uint8_t status_packet[1024]; - uint8_t data[1024],error,length; uint16_t eeprom_data; - uint16_t action_period,mm_period; - uint8_t i; + uint16_t period; /* initialize EEPROM */ EE_Init(); @@ -157,88 +42,22 @@ int main(void) dyn_slave_set_return_level((uint8_t)eeprom_data); // initialize motion manager EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data); - action_period=eeprom_data&0x00FF; + period=eeprom_data&0x00FF; EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data); - action_period+=((eeprom_data&0x00FF)<<8); - mm_period=(action_period*1000000)>>16; - manager_init(mm_period); - // initialize the action module - action_init(action_period); - walking_init(); + period+=((eeprom_data&0x00FF)<<8); + manager_init(period); + grippers_init(); // initialize imu imu_init(); // initialize adc - adc_init(); - - gpio_blink_led(LED_2,1000); - - for(i=0;i<20;i++) - { - manager_enable_servo(i); - manager_set_module(i,MM_WALKING); - } -// manager_enable(); + //adc_init(); + gpio_blink_led(LED_5_R,1000); + comm_init(); - walking_start(); + comm_start(); while(1) /* main function does not return */ { - if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received - { - dyn_slave_get_inst_packet(inst_packet);// get the received packet - // check the packet checksum - if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay - { - // check address - if(dyn_get_id(inst_packet)==dyn_slave_get_address())// the packet is addressed to this device - { - // process the packet - switch(dyn_get_instruction(inst_packet)) - { - case DYN_PING: dyn_slave_send_status_packet(DYN_NO_ERROR,0,data); - break; - case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data); - if(dyn_slave_get_return_level()!=0) - { - if(error==RAM_SUCCESS) - dyn_slave_send_status_packet(DYN_NO_ERROR,dyn_get_read_length(inst_packet),data); - else - dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); - } - break; - case DYN_WRITE: length=dyn_get_write_data(inst_packet,data); - if(length!=DYN_BAD_FORMAT) - error=write_operation(dyn_get_write_address(inst_packet),length,data); - if(dyn_slave_get_return_level()==2) - { - if(error==RAM_SUCCESS || length==DYN_BAD_FORMAT) - dyn_slave_send_status_packet(DYN_NO_ERROR,0,data); - else - dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); - } - break; - default: - break; - } - } - else// the packet is addressed to another device - { - // send the incomming packet to the dynamixel bus - if(dyn_master_resend_inst_packet(inst_packet,status_packet)==DYN_NO_ERROR) - { - // send the answer back to the computer - dyn_slave_resend_status_packet(status_packet); - } - else - dyn_slave_reset();// prepare the dynamixel master to accept data - } - } - else - { - // send a checksum error answer - dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00); - } - } } } diff --git a/src/comm.c b/src/comm.c new file mode 100644 index 0000000000000000000000000000000000000000..cb15dacaa11ca638d571f91323305ae3d6c4edbe --- /dev/null +++ b/src/comm.c @@ -0,0 +1,303 @@ +#include "comm.h" +#include "gpio.h" +#include "ram.h" +#include "motion_manager.h" +#include "action.h" +#include "walking.h" +#include "joint_motion.h" +#include "head_tracking.h" +#include "grippers.h" +#include "dynamixel_slave_uart_dma.h" +#include "dynamixel_master_uart_dma.h" +#include "imu.h" +#include "adc_dma.h" + +#define COMM_TIMER TIM7 +#define COMM_TIMER_IRQn TIM7_IRQn +#define COMM_TIMER_IRQHandler TIM7_IRQHandler +#define COMM_TIMER_CLK RCC_APB1Periph_TIM7 + +uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data) +{ + uint8_t error; + + // pre-read operation + // read operation + error=ram_read_table(address,length,data); + // post-read operation + if(ram_in_range(DARWIN_PUSHBUTTON,DARWIN_PUSHBUTTON,address,length)) + ram_data[DARWIN_PUSHBUTTON]=0x00; + + return error; +} + +uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data) +{ + uint8_t error=RAM_SUCCESS,byte_value,i,j,do_op=0x00,module; + uint16_t word_value; + + // pre-write operation + if(ram_in_range(DARWIN_DXL_POWER,DARWIN_DXL_POWER,address,length)) + { + if(data[DARWIN_DXL_POWER-address]&0x01) dyn_master_enable_power(); + else dyn_master_disable_power(); + } + if(ram_in_range(DARWIN_LED_PANNEL,DARWIN_LED_PANNEL,address,length)) + { + byte_value=data[DARWIN_LED_PANNEL-address]; + if(byte_value&0x01) + gpio_set_led(LED_2); + else + gpio_clear_led(LED_2); + if(byte_value&0x02) + gpio_set_led(LED_3); + else + gpio_clear_led(LED_3); + if(byte_value&0x04) + gpio_set_led(LED_4); + else + gpio_clear_led(LED_4); + if(byte_value&0x08) + gpio_set_led(LED_TX); + else + gpio_clear_led(LED_TX); + if(byte_value&0x10) + gpio_set_led(LED_RX); + else + gpio_clear_led(LED_RX); + } + if(ram_in_range(DARWIN_MM_PERIOD_L,DARWIN_MM_PERIOD_H,address,length)) + { + word_value=data[DARWIN_MM_PERIOD_L-address]+(data[DARWIN_MM_PERIOD_H-address]<<8); + manager_set_period(word_value); + } + for(i=DARWIN_MM_MODULE_EN0,j=0;i<=DARWIN_MM_MODULE_EN15;i++,j+=2) + { + do_op=0x01; + if(ram_in_range(i,i,address,length)) + { + byte_value=data[i-address]; + if(byte_value&0x80) manager_enable_servo(j); + else manager_disable_servo(j); + module=(byte_value&0x70)>>4; + manager_set_module(j,module); + if(module==MM_JOINTS) + joint_motion_load_target_angle(j,current_angles[j]>>9); + if(byte_value&0x08) manager_enable_servo(j+1); + else manager_disable_servo(j+1); + module=byte_value&0x07; + manager_set_module(j+1,module); + if(module==MM_JOINTS) + joint_motion_load_target_angle(j+1,current_angles[j+1]>>9); + } + } + if(do_op) + { + do_op=0x00; + manager_disable_servos(); + } + if(ram_in_range(DARWIN_MM_CONTROL,DARWIN_MM_CONTROL,address,length)) + { + if(data[DARWIN_MM_CONTROL-address]&0x01) + manager_enable(); + else + manager_disable(); + if(data[DARWIN_MM_CONTROL-address]&0x02) + manager_enable_balance(); + else + manager_disable_balance(); + } + if(ram_in_range(DARWIN_MM_KNEE_GAIN,DARWIN_MM_KNEE_GAIN,address,length)) + manager_set_knee_gain(data[DARWIN_MM_CONTROL-address]); + if(ram_in_range(DARWIN_MM_ANKLE_PITCH_GAIN,DARWIN_MM_ANKLE_PITCH_GAIN,address,length)) + manager_set_ankle_pitch_gain(data[DARWIN_MM_CONTROL-address]); + if(ram_in_range(DARWIN_MM_HIP_ROLL_GAIN,DARWIN_MM_HIP_ROLL_GAIN,address,length)) + manager_set_hip_roll_gain(data[DARWIN_MM_CONTROL-address]); + if(ram_in_range(DARWIN_MM_ANKLE_ROLL_GAIN,DARWIN_MM_ANKLE_ROLL_GAIN,address,length)) + manager_set_ankle_roll_gain(data[DARWIN_MM_CONTROL-address]); + if(ram_in_range(DARWIN_ACTION_CONTROL,DARWIN_ACTION_CONTROL,address,length)) + { + if(data[DARWIN_ACTION_CONTROL-address]&0x01) + action_start_page(); + if(data[DARWIN_ACTION_CONTROL-address]&0x02) + action_stop_page(); + } + if(ram_in_range(DARWIN_ACTION_PAGE,DARWIN_ACTION_PAGE,address,length))// load the page identifier + action_load_page(data[DARWIN_ACTION_PAGE-address]); + if(ram_in_range(DARWIN_WALK_CONTROL,DARWIN_WALK_CONTROL,address,length)) + { + if(data[DARWIN_WALK_CONTROL-address]&0x01) + walking_start(); + if(data[DARWIN_WALK_CONTROL-address]&0x02) + walking_stop(); + } + for(i=DARWIN_X_OFFSET;i<=DARWIN_MAX_ROT_ACC;i++) + if(ram_in_range(i,i,address,length)) + walking_set_param(i,data[i-address]); + if(ram_in_range(DARWIN_IMU_CONTROL,DARWIN_IMU_CONTROL,address,length)) + { + if(data[DARWIN_IMU_CONTROL-address]&0x01) + imu_start(); + else + imu_stop(); + if(data[DARWIN_IMU_CONTROL-address]&0x02) + imu_start_gyro_cal(); + } + for(i=DARWIN_MM_SERVO0_CUR_POS_L,j=0;i<=DARWIN_MM_SERVO26_CUR_POS_L;i+=2,j++) + if(ram_in_range(i,i+1,address,length)) + joint_motion_load_target_angle(j,data[i-address]+(data[i+1-address]<<8)); + for(i=DARWIN_SERVO_0_SPEED,j=0;i<=DARWIN_SERVO_26_SPEED;i++,j++) + if(ram_in_range(i,i,address,length)) + joint_motion_load_target_speed(j,data[i-address]); + for(i=DARWIN_SERVO_0_ACCEL,j=0;i<=DARWIN_SERVO_26_ACCEL;i++,j++) + if(ram_in_range(i,i,address,length)) + joint_motion_load_target_accel(j,data[i-address]); + if(ram_in_range(DARWIN_JOINTS_CONTROL,DARWIN_JOINTS_CONTROL,address,length)) + { + if(data[DARWIN_JOINTS_CONTROL-address]&0x01) + joint_motion_start(); + if(data[DARWIN_JOINTS_CONTROL-address]&0x02) + joint_motion_stop(); + } + if(ram_in_range(DARWIN_HEAD_PAN_L,DARWIN_HEAD_PAN_H,address,length)) + { + word_value=data[DARWIN_HEAD_PAN_L-address]+(data[DARWIN_HEAD_PAN_H-address]<<8); + head_tracking_set_target_pan(word_value); + } + if(ram_in_range(DARWIN_HEAD_TILT_L,DARWIN_HEAD_TILT_H,address,length)) + { + word_value=data[DARWIN_HEAD_TILT_L-address]+(data[DARWIN_HEAD_TILT_H-address]<<8); + head_tracking_set_target_tilt(word_value); + } + if(ram_in_range(DARWIN_HEAD_PAN_P,DARWIN_HEAD_PAN_P,address,length)) + head_tracking_set_pan_p(data[DARWIN_HEAD_PAN_P-address]); + if(ram_in_range(DARWIN_HEAD_PAN_I,DARWIN_HEAD_PAN_I,address,length)) + head_tracking_set_pan_i(data[DARWIN_HEAD_PAN_I-address]); + if(ram_in_range(DARWIN_HEAD_PAN_D,DARWIN_HEAD_PAN_D,address,length)) + head_tracking_set_pan_d(data[DARWIN_HEAD_PAN_D-address]); + if(ram_in_range(DARWIN_HEAD_TILT_P,DARWIN_HEAD_TILT_P,address,length)) + head_tracking_set_tilt_p(data[DARWIN_HEAD_TILT_P-address]); + if(ram_in_range(DARWIN_HEAD_TILT_I,DARWIN_HEAD_TILT_I,address,length)) + head_tracking_set_tilt_i(data[DARWIN_HEAD_TILT_I-address]); + if(ram_in_range(DARWIN_HEAD_TILT_D,DARWIN_HEAD_TILT_D,address,length)) + head_tracking_set_tilt_d(data[DARWIN_HEAD_TILT_D-address]); + if(ram_in_range(DARWIN_HEAD_CONTROL,DARWIN_HEAD_CONTROL,address,length)) + { + if(data[DARWIN_HEAD_CONTROL-address]&0x01) + head_tracking_start(); + else + head_tracking_stop(); + } + if(ram_in_range(DARWIN_GRIPPER_CONTROL,DARWIN_GRIPPER_CONTROL,address,length)) + { + if(data[DARWIN_GRIPPER_CONTROL-address]&0x01) + grippers_open(LEFT_GRIPPER); + else if(data[DARWIN_GRIPPER_CONTROL-address]&0x02) + grippers_close(LEFT_GRIPPER); + else if(data[DARWIN_GRIPPER_CONTROL-address]&0x04) + grippers_open(RIGHT_GRIPPER); + else if(data[DARWIN_GRIPPER_CONTROL-address]&0x08) + grippers_close(RIGHT_GRIPPER); + } + // write eeprom + for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++) + EE_WriteVariable(i,data[j]); + // write operation + // error=ram_write_table(address,length,data); + // post-write operation + + return error; +} + +void COMM_TIMER_IRQHandler(void) +{ + uint8_t inst_packet[2*MAX_DATA_LENGTH]; + uint8_t status_packet[2*MAX_DATA_LENGTH]; + uint8_t data[2*MAX_DATA_LENGTH],error,length; + + TIM_ClearITPendingBit(COMM_TIMER,TIM_IT_Update); + + if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received + { + dyn_slave_get_inst_packet(inst_packet);// get the received packet + // check the packet checksum + if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay + { + // check address + if(dyn_get_id(inst_packet)==dyn_slave_get_address())// the packet is addressed to this device + { + // process the packet + switch(dyn_get_instruction(inst_packet)) + { + case DYN_PING: dyn_slave_send_status_packet(DYN_NO_ERROR,0,data); + break; + case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data); + if(dyn_slave_get_return_level()!=0) + { + if(error==RAM_SUCCESS) + dyn_slave_send_status_packet(DYN_NO_ERROR,dyn_get_read_length(inst_packet),data); + else + dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); + } + break; + case DYN_WRITE: length=dyn_get_write_data(inst_packet,data); + if(length!=DYN_BAD_FORMAT) + error=write_operation(dyn_get_write_address(inst_packet),length,data); + if(dyn_slave_get_return_level()==2) + { + if(error==RAM_SUCCESS || length==DYN_BAD_FORMAT) + dyn_slave_send_status_packet(DYN_NO_ERROR,0,data); + else + dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); + } + break; + default: + break; + } + } + else// the packet is addressed to another device + { + // send the incomming packet to the dynamixel bus + if(dyn_master_resend_inst_packet(inst_packet,status_packet)==DYN_NO_ERROR) + { + // send the answer back to the computer + dyn_slave_resend_status_packet(status_packet); + } + else + dyn_slave_reset();// prepare the dynamixel master to accept data + } + } + else + { + // send a checksum error answer + dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00); + } + } +} + +void comm_init(void) +{ + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + NVIC_InitTypeDef NVIC_InitStructure; + + RCC_APB1PeriphClockCmd(COMM_TIMER_CLK,ENABLE); + + TIM_TimeBaseStructure.TIM_Period = 1000; + TIM_TimeBaseStructure.TIM_Prescaler = 72; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(COMM_TIMER,&TIM_TimeBaseStructure); + + NVIC_InitStructure.NVIC_IRQChannel = COMM_TIMER_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); +} + +void comm_start(void) +{ + TIM_Cmd(COMM_TIMER, ENABLE); + TIM_ITConfig(COMM_TIMER, TIM_IT_Update, ENABLE); +} + diff --git a/src/dynamixel_master_uart_dma.c b/src/dynamixel_master_uart_dma.c index d384c7fe76ed5a0d14b2a8c8230e645110840f3d..936781e745d7bdda06b85cae10e1e8cdf68816a0 100755 --- a/src/dynamixel_master_uart_dma.c +++ b/src/dynamixel_master_uart_dma.c @@ -1,6 +1,7 @@ #include "dynamixel_master_uart_dma.h" +#include "motion_manager.h" #include "time.h" -#include "gpio.h" +#include "ram.h" #define USART USART1 #define USART_CLK RCC_APB2Periph_USART1 @@ -59,6 +60,7 @@ #define MAX_BUFFER_LEN 1024 // private variables +uint8_t dyn_master_version; uint16_t dyn_master_timeout;// answer reception timeout // input buffer uint8_t dyn_master_rx_buffer[MAX_BUFFER_LEN]; @@ -73,6 +75,9 @@ volatile uint8_t dyn_master_sending_packet; uint8_t dyn_master_no_answer; // power enabled variable uint8_t dyn_master_power_enabled; +// receiving a bulk read backet +uint8_t dyn_bulk_read_packet; +uint8_t dyn_bulk_read_length; // DMA initialization data structures DMA_InitTypeDef DMA_TX_InitStructure; DMA_InitTypeDef DMA_RX_InitStructure; @@ -86,12 +91,15 @@ void dyn_master_reset(void) /* wait until the transaction ends */ DMA_ClearFlag(USART_RX_DMA_FLAG_GLIF); DMA_ClearITPendingBit(USART_RX_DMA_FLAG_GLIF); + /* clear any pending data */ + USART_ReceiveData(USART); dyn_master_receiving_header=0x01; dyn_master_packet_ready=0x00; } void dyn_master_send(void) { + dyn_master_version=1; // wait until any previous transmission ends while(dyn_master_sending_packet); // set the DMA transfer @@ -102,6 +110,19 @@ void dyn_master_send(void) dyn_master_sending_packet=0x01; } +void dyn2_master_send(void) +{ + dyn_master_version=2; + // wait until any previous transmission ends + while(dyn_master_sending_packet); + // set the DMA transfer + DMA_SetCurrDataCounter(USART_TX_DMA_CHANNEL,dyn2_get_length(dyn_master_tx_buffer)+7); + DMA_Cmd(USART_TX_DMA_CHANNEL,ENABLE); + USART_ClearFlag(USART,USART_FLAG_TC); + USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE); + dyn_master_sending_packet=0x01; +} + uint8_t dyn_master_receive(void) { int16_t timeout_left=dyn_master_timeout; @@ -122,7 +143,36 @@ uint8_t dyn_master_receive(void) if(dyn_check_checksum(dyn_master_rx_buffer)==0xFF) return dyn_get_status_error(dyn_master_rx_buffer); else + { + dyn_master_reset(); return DYN_CHECKSUM_ERROR; + } +} + +uint8_t dyn2_master_receive(void) +{ + int16_t timeout_left=dyn_master_timeout; + + // wait for the status packet + while(!dyn_master_packet_ready) + { + delay_ms(1); + timeout_left-=1; + if(timeout_left<=0) + { + dyn_master_reset(); + return DYN_TIMEOUT; + } + } + dyn_master_packet_ready=0x00; + // check the input packet checksum + if(dyn2_check_status_crc(dyn_master_rx_buffer)==0x01) + return dyn2_get_status_error(dyn_master_rx_buffer); + else + { + dyn_master_reset(); + return DYN_CHECKSUM_ERROR; + } } void dyn_master_enable_tx(void) @@ -175,6 +225,44 @@ uint8_t dyn_master_read(uint8_t id,uint8_t address,uint8_t length,uint8_t *data) return error; } +uint8_t dyn2_master_read(uint8_t id,uint16_t address,uint16_t length,uint8_t *data) +{ + uint8_t error; + + // wait for the transmission to end + while(dyn_master_sending_packet); + // generate the read packet for the desired device + dyn2_init_instruction_packet(dyn_master_tx_buffer); + // set the ping instruction + dyn2_set_instruction(dyn_master_tx_buffer,DYN_READ); + // set the device id + if((error=dyn2_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS) + { + // set the length to read + dyn2_set_read_length(dyn_master_tx_buffer,length); + // sert the start address to start reading + dyn2_set_read_address(dyn_master_tx_buffer,address); + // set the checksum + dyn2_set_inst_crc(dyn_master_tx_buffer); + // enable transmission + dyn_master_enable_tx(); + // send the data + dyn2_master_send(); + // wait for the transmission to end + while(dyn_master_sending_packet); + dyn_master_enable_rx(); + // wait for the replay within the given timeout + error=dyn2_master_receive(); + if(error==DYN_NO_ERROR) + dyn2_get_status_data(dyn_master_rx_buffer,data);// read the data from the status packet + else + dyn_master_reset(); + return error; + } + else + return error; +} + uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data) { uint8_t error; @@ -211,6 +299,42 @@ uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *d return error; } +uint8_t dyn2_master_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data) +{ + uint8_t error; + + // wait for the transmission to end + while(dyn_master_sending_packet); + // generate the read packet for the desired device + dyn2_init_instruction_packet(dyn_master_tx_buffer); + // set the ping instruction + dyn2_set_instruction(dyn_master_tx_buffer,DYN_WRITE); + // set the device id + if((error=dyn2_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS) + { + // sert the start address to start reading + dyn2_set_read_address(dyn_master_tx_buffer,address); + // set the data to write and its length + dyn2_set_write_data(dyn_master_tx_buffer,length,data); + // set the checksum + dyn2_set_inst_crc(dyn_master_tx_buffer); + // enable transmission + dyn_master_enable_tx(); + // send the data + dyn2_master_send(); + // wait for the transmission to end + while(dyn_master_sending_packet); + dyn_master_enable_rx(); + // wait for the replay within the given timeout + error=dyn2_master_receive(); + if(error!=DYN_NO_ERROR) + dyn_master_reset(); + return error; + } + else + return error; +} + // interrupt handlers void USART_IRQHandler(void) { @@ -223,14 +347,32 @@ void USART_IRQHandler(void) dyn_master_no_answer=0x00; else { - // set up the DMA RX transaction - DMA_RX_InitStructure.DMA_BufferSize = 4; - DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_rx_buffer; - DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure); - DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE); - DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE); - USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE); - dyn_master_receiving_header=0x01; + if(dyn_bulk_read_packet==0x00) + { + // set up the DMA RX transaction + if(dyn_master_version==1) + DMA_RX_InitStructure.DMA_BufferSize = 4; + else + DMA_RX_InitStructure.DMA_BufferSize = 7; + DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_rx_buffer; + DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure); + DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE); + DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE); + USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE); + dyn_master_receiving_header=0x01; + } + else + { + // set up the DMA RX transaction + DMA_RX_InitStructure.DMA_BufferSize = dyn_bulk_read_length; + DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_rx_buffer; + DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure); + DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE); + DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE); + USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE); + dyn_master_receiving_header=0x00; + dyn_bulk_read_packet=0x00; + } } } } @@ -248,8 +390,16 @@ void USART_DMA_RX_IRQHandler(void) if(dyn_master_receiving_header==0x01) { DMA_Cmd(USART_RX_DMA_CHANNEL,DISABLE); - DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_master_rx_buffer); - DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_master_rx_buffer[4]; + if(dyn_master_version==1) + { + DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_master_rx_buffer); + DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_master_rx_buffer[4]; + } + else + { + DMA_RX_InitStructure.DMA_BufferSize = dyn2_get_length(dyn_master_rx_buffer); + DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_master_rx_buffer[7]; + } DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure); DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE); USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE); @@ -318,6 +468,9 @@ void dyn_master_init(void) dyn_master_sending_packet=0x00; dyn_master_receiving_header=0x01; dyn_master_no_answer=0x00; + dyn_bulk_read_packet=0x00; + dyn_bulk_read_length=0x00; + dyn_master_version=1; USART_DeInit(USART); USART_StructInit(&USART_InitStructure); @@ -330,7 +483,7 @@ void dyn_master_init(void) USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; USART_Init(USART, &USART_InitStructure); NVIC_InitStructure.NVIC_IRQChannel = USART_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -353,7 +506,7 @@ void dyn_master_init(void) DMA_Init(USART_TX_DMA_CHANNEL,&DMA_TX_InitStructure); /* initialize DMA interrupts */ NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_TX_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -375,7 +528,7 @@ void dyn_master_init(void) DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure); /* initialize DMA interrupts */ NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_RX_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -393,11 +546,6 @@ void dyn_master_init(void) dyn_master_disable_power(); } -void dyn_master_flush(void) -{ - // flush only the reception buffer to avoid interrupting a sync_write command -} - void dyn_master_set_timeout(uint16_t timeout_ms) { // save the desired timeout value @@ -408,6 +556,8 @@ void dyn_master_scan(uint8_t *num,uint8_t *ids) { uint8_t i; + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) + ids[0]=0; *num=0; for(i=0;i<254;i++) { @@ -419,6 +569,23 @@ void dyn_master_scan(uint8_t *num,uint8_t *ids) } } +void dyn2_master_scan(uint8_t *num,uint8_t *ids) +{ + uint8_t i; + + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) + ids[0]=0; + *num=0; + for(i=0;i<254;i++) + { + if(dyn2_master_ping(i)==DYN_NO_ERROR)// the device exists + { + ids[*num]=i; + (*num)++; + } + } +} + inline void dyn_master_enable_power(void) { GPIO_SetBits(POWER_GPIO_PORT,POWER_PIN); @@ -464,11 +631,44 @@ uint8_t dyn_master_ping(uint8_t id) return error; } +uint8_t dyn2_master_ping(uint8_t id) +{ + uint8_t error; + + // generate the ping packet for the desired device + dyn2_init_instruction_packet(dyn_master_tx_buffer); + // set the ping instruction + dyn2_set_instruction(dyn_master_tx_buffer,DYN_PING); + // set the device id + if((error=dyn2_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS) + { + // set the checksum + dyn2_set_inst_crc(dyn_master_tx_buffer); + // enable transmission + dyn_master_enable_tx(); + // send the data + dyn2_master_send(); + // wait for the transmission to end + while (dyn_master_sending_packet==0x01); + dyn_master_enable_rx(); + // wait for the replay within the given timeout + error=dyn2_master_receive(); + return error; + } + else + return error; +} + uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data) { return dyn_master_read(id,address,1,data); } +uint8_t dyn2_master_read_byte(uint8_t id,uint16_t address,uint8_t *data) +{ + return dyn2_master_read(id,address,1,data); +} + uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data) { uint8_t value[2]; @@ -481,16 +681,38 @@ uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data) return error; } +uint8_t dyn2_master_read_word(uint8_t id,uint16_t address,uint16_t *data) +{ + uint8_t value[2]; + uint8_t error; + + error=dyn2_master_read(id,address,2,value); + if(error==DYN_NO_ERROR) + (*data)=value[0]+value[1]*256; + + return error; +} + uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data) { return dyn_master_read(id,address,length,data); } +uint8_t dyn2_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data) +{ + return dyn2_master_read(id,address,length,data); +} + uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data) { return dyn_master_write(id,address,1,&data); } +uint8_t dyn2_master_write_byte(uint8_t id, uint16_t address, uint8_t data) +{ + return dyn2_master_write(id,address,1,&data); +} + uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data) { uint8_t value[2]; @@ -500,11 +722,25 @@ uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data) return dyn_master_write(id,address,2,value); } +uint8_t dyn2_master_write_word(uint8_t id, uint16_t address, uint16_t data) +{ + uint8_t value[2]; + + value[0]=data%256; + value[1]=data/256; + return dyn2_master_write(id,address,2,value); +} + uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data) { return dyn_master_write(id,address,length,data); } +uint8_t dyn2_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data) +{ + return dyn2_master_write(id,address,length,data); +} + uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data) { return DYN_SUCCESS; @@ -542,6 +778,36 @@ void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t len // wait for the transmission to end } +void dyn2_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, uint8_t *data) +{ + uint8_t i; + + // wait for the transmission to end + while(dyn_master_sending_packet); + dyn_master_no_answer=0x01; + // generate the sync write packet + dyn2_init_instruction_packet(dyn_master_tx_buffer); + // set the ping instruction + dyn2_set_instruction(dyn_master_tx_buffer,DYN_SYNC_WRITE); + // set the start address + dyn2_set_write_address(dyn_master_tx_buffer,address); + // set the data length + dyn2_set_sync_write_length(dyn_master_tx_buffer,length); + // load the data for each device + for(i=0;i<num;i++) + dyn2_set_sync_write_data(dyn_master_tx_buffer,ids[i],&data[i*length]); + // set the checksum + dyn2_set_inst_crc(dyn_master_tx_buffer); + // enable transmission + dyn_master_enable_tx(); + // send the data + for(i=0;i<27;i++) + ram_data[DARWIN_SERVO_0_SPEED+i]=dyn_master_tx_buffer[i]; + + dyn2_master_send(); + // wait for the transmission to end +} + // repeater functions uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet) { @@ -569,3 +835,48 @@ uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_pack return error; } +uint8_t dyn_master_bulk_read(uint8_t num,TBulkData *info) +{ + uint8_t i,error; + + // wait for the transmission to end + while(dyn_master_sending_packet); + dyn_bulk_read_packet=0x01; + dyn_bulk_read_length=0x00; + // generate the sync write packet + dyn_init_instruction_packet(dyn_master_tx_buffer); + // set the ping instruction + dyn_set_instruction(dyn_master_tx_buffer,DYN_BULK_READ); + // set the start address + dyn_set_write_address(dyn_master_tx_buffer,0x00); + // load the data for each device + for(i=0;i<num;i++) + { + dyn_set_bulk_read_data(dyn_master_tx_buffer,info[i].id,info[i].length,info[i].address); + dyn_bulk_read_length+=info[i].length+6; + } + // set the checksum + dyn_set_checksum(dyn_master_tx_buffer); + // enable transmission + dyn_master_enable_tx(); + // send the data + dyn_master_send(); + // wait for the transmission to end + while(dyn_master_sending_packet); + dyn_master_enable_rx(); + // wait for the replay within the given timeout + error=dyn_master_receive(); + if(error!=DYN_NO_ERROR) + { + dyn_master_reset(); + return error; + } + for(i=0;i<num;i++) + { + error=dyn_get_bulk_read_data(dyn_master_rx_buffer,dyn_bulk_read_length,info[i].id,info[i].data); + if(error!=DYN_SUCCESS) + return error; + } + + return DYN_SUCCESS; +} diff --git a/src/dynamixel_slave_uart_dma.c b/src/dynamixel_slave_uart_dma.c index 268b331fe102a8c55c1ff38f3f6212a5b9637412..6363f7e3e9e0f29a2b6e6b7cdd0aed07b0b4bfd0 100755 --- a/src/dynamixel_slave_uart_dma.c +++ b/src/dynamixel_slave_uart_dma.c @@ -91,14 +91,25 @@ void DYN_SLAVE_DMA_RX_IRQHandler(void) if(dyn_slave_receiving_header==0x01) { DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,DISABLE); - DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_slave_rx_buffer); - DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_slave_rx_buffer[4]; - DMA_Init(DYN_SLAVE_RX_DMA_CHANNEL,&DYN_SLAVE_DMA_RX_InitStructure); - DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,ENABLE); - USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE); - dyn_slave_receiving_header=0x00; - DMA_ClearFlag(DYN_SLAVE_RX_DMA_FLAG_GLIF); - DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_FLAG_GLIF); + if(dyn_get_length(dyn_slave_rx_buffer)==0x00) + { + DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,DISABLE); + DMA_ClearFlag(DYN_SLAVE_RX_DMA_FLAG_GLIF); + DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_FLAG_GLIF); + DMA_ITConfig(DYN_SLAVE_RX_DMA_CHANNEL,DMA_IT_TC,DISABLE); + dyn_slave_packet_ready=0x01; + } + else + { + DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_slave_rx_buffer); + DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_slave_rx_buffer[4]; + DMA_Init(DYN_SLAVE_RX_DMA_CHANNEL,&DYN_SLAVE_DMA_RX_InitStructure); + DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,ENABLE); + USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE); + dyn_slave_receiving_header=0x00; + DMA_ClearFlag(DYN_SLAVE_RX_DMA_FLAG_GLIF); + DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_FLAG_GLIF); + } } else { diff --git a/src/eeprom.c b/src/eeprom.c index 96dff4b16d429620d008653df5e6c0f946ffb615..0477a437945e2a91869fe6f08d94c81c5c4ec9c9 100755 --- a/src/eeprom.c +++ b/src/eeprom.c @@ -30,12 +30,12 @@ /* Private define ------------------------------------------------------------*/ #define EEPROM_SIZE 0x09 #define DEFAULT_DEVICE_MODEL 0x7300 -#define DEFAULT_FIRMWARE_VERSION 0x13 -#define DEFAULT_DEVICE_ID 0xC0 -#define DEFAULT_BAUDRATE 0x01 -#define DEFAULT_RETURN_DELAY 0x00 +#define DEFAULT_FIRMWARE_VERSION 0x0013 +#define DEFAULT_DEVICE_ID 0x00C0 +#define DEFAULT_BAUDRATE 0x0001 +#define DEFAULT_RETURN_DELAY 0x0000 #define DEFAULT_MM_PERIOD 0x01FF -#define DEFAULT_RETURN_LEVEL 0x02 +#define DEFAULT_RETURN_LEVEL 0x0002 /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ diff --git a/src/gpio.c b/src/gpio.c index 0c1c27795bbc24df2d6c4fa6106a1b93d2a3396a..aaa06b20bd9a96155152ac3b5f79fc854e52c6c8 100755 --- a/src/gpio.c +++ b/src/gpio.c @@ -1,4 +1,5 @@ #include "gpio.h" +#include "ram.h" #define LED_TX_GPIO_CLK RCC_APB2Periph_GPIOC #define LED_TX_PIN GPIO_Pin_13 @@ -332,8 +333,8 @@ void gpio_init(void) // initialize the timer interrupts NVIC_InitStructure.NVIC_IRQChannel = GPO_TIMER1_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -349,8 +350,8 @@ void gpio_init(void) // initialize the timer interrupts NVIC_InitStructure.NVIC_IRQChannel = GPO_TIMER2_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -366,8 +367,8 @@ void gpio_init(void) // initialize the timer interrupts NVIC_InitStructure.NVIC_IRQChannel = GPO_TIMER3_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -389,7 +390,7 @@ void gpio_init(void) EXTI_Init(&EXTI_InitStructure); NVIC_InitStructure.NVIC_IRQChannel = GPI_EXTI1_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -414,18 +415,23 @@ void gpio_set_led(led_t led_id) { case LED_TX: GPIO_ResetBits(LED_TX_GPIO_PORT,LED_TX_PIN); + ram_set_bit(DARWIN_LED_PANNEL,3); break; case LED_RX: GPIO_ResetBits(LED_RX_GPIO_PORT,LED_RX_PIN); + ram_set_bit(DARWIN_LED_PANNEL,4); break; case LED_2: GPIO_ResetBits(LED_2_GPIO_PORT,LED_2_PIN); + ram_set_bit(DARWIN_LED_PANNEL,0); break; case LED_3: GPIO_ResetBits(LED_3_GPIO_PORT,LED_3_PIN); + ram_set_bit(DARWIN_LED_PANNEL,1); break; case LED_4: GPIO_ResetBits(LED_4_GPIO_PORT,LED_4_PIN); + ram_set_bit(DARWIN_LED_PANNEL,2); break; case LED_5_R: GPIO_ResetBits(LED_5_R_GPIO_PORT,LED_5_R_PIN); @@ -454,18 +460,23 @@ void gpio_clear_led(led_t led_id) { case LED_TX: GPIO_SetBits(LED_TX_GPIO_PORT,LED_TX_PIN); + ram_clear_bit(DARWIN_LED_PANNEL,3); break; case LED_RX: GPIO_SetBits(LED_RX_GPIO_PORT,LED_RX_PIN); + ram_clear_bit(DARWIN_LED_PANNEL,4); break; case LED_2: GPIO_SetBits(LED_2_GPIO_PORT,LED_2_PIN); + ram_clear_bit(DARWIN_LED_PANNEL,0); break; case LED_3: GPIO_SetBits(LED_3_GPIO_PORT,LED_3_PIN); + ram_clear_bit(DARWIN_LED_PANNEL,1); break; case LED_4: GPIO_SetBits(LED_4_GPIO_PORT,LED_4_PIN); + ram_clear_bit(DARWIN_LED_PANNEL,2); break; case LED_5_R: GPIO_SetBits(LED_5_R_GPIO_PORT,LED_5_R_PIN); diff --git a/src/grippers.c b/src/grippers.c new file mode 100644 index 0000000000000000000000000000000000000000..f7fb14b8c593370d12393ddc6851f11680bc44c2 --- /dev/null +++ b/src/grippers.c @@ -0,0 +1,48 @@ +#include "grippers.h" +#include "motion_manager.h" +#include "ram.h" + +const uint16_t left_close_value=-120; +const uint16_t left_open_value=-20; +const uint16_t right_close_value=-140; +const uint16_t right_open_value=-40; + +/* public functions */ +void grippers_init(void) +{ + ram_data[DARWIN_GRIPPER_CONTROL]=0x00; +} + +void grippers_set_force(grippers_t gripper,uint8_t force) +{ + +} + +void grippers_open(grippers_t gripper) +{ + if(gripper==LEFT_GRIPPER) + { + current_angles[L_GRIPPER]=left_open_value<<16; + ram_set_bit(DARWIN_GRIPPER_CONTROL,0); + } + else + { + current_angles[R_GRIPPER]=right_open_value<<16; + ram_set_bit(DARWIN_GRIPPER_CONTROL,2); + } +} + +void grippers_close(grippers_t gripper) +{ + if(gripper==LEFT_GRIPPER) + { + current_angles[L_GRIPPER]=left_close_value<<16; + ram_set_bit(DARWIN_GRIPPER_CONTROL,1); + } + else + { + current_angles[R_GRIPPER]=right_close_value<<16; + ram_set_bit(DARWIN_GRIPPER_CONTROL,3); + } +} + diff --git a/src/head_tracking.c b/src/head_tracking.c new file mode 100644 index 0000000000000000000000000000000000000000..8acf565fc1e0bb66b9485d1fc3a7116e0228f105 --- /dev/null +++ b/src/head_tracking.c @@ -0,0 +1,147 @@ +#include "head_tracking.h" +#include "ram.h" +#include "motion_manager.h" + +#define DEFAULT_PAN_P 40// +#define DEFAULT_PAN_I 0// +#define DEFAULT_PAN_D 0// + +#define DEFAULT_TILT_P 40// +#define DEFAULT_TILT_I 0// +#define DEFAULT_TILT_D 0// + +// private variables +int64_t prev_error_pan;// 48|16 +int64_t prev_error_tilt;// 48|16 +int64_t integral_pan; +int64_t integral_tilt; +uint8_t pan_p; +uint8_t pan_i; +uint8_t pan_d; +uint8_t tilt_p; +uint8_t tilt_i; +uint8_t tilt_d; +int64_t pan_target;// 48|16 +int64_t tilt_target;// 48|16 +int64_t head_tracking_period; +uint8_t head_tracking_enabled; + +// public functions +void head_tracking_init(uint16_t period) +{ + /* initialize private variables */ + prev_error_pan=0; + prev_error_tilt=0; + integral_pan=0; + integral_tilt=0; + pan_p=DEFAULT_PAN_P; + ram_write_byte(DARWIN_HEAD_PAN_P,DEFAULT_PAN_P); + pan_i=DEFAULT_PAN_I; + ram_write_byte(DARWIN_HEAD_PAN_I,DEFAULT_PAN_I); + pan_d=DEFAULT_PAN_D; + ram_write_byte(DARWIN_HEAD_PAN_D,DEFAULT_PAN_D); + tilt_p=DEFAULT_TILT_P; + ram_write_byte(DARWIN_HEAD_TILT_P,DEFAULT_TILT_P); + tilt_i=DEFAULT_TILT_I; + ram_write_byte(DARWIN_HEAD_TILT_I,DEFAULT_TILT_I); + tilt_d=DEFAULT_TILT_D; + ram_write_byte(DARWIN_HEAD_TILT_D,DEFAULT_TILT_D); + pan_target=0; + tilt_target=0; + head_tracking_period=period; + head_tracking_enabled=0x00; +} + +void head_tracking_start(void) +{ + head_tracking_enabled=0x01; +} + +void head_tracking_stop(void) +{ + head_tracking_enabled=0x00; +} + +void head_tracking_set_period(int16_t period) +{ + head_tracking_period=period; +} + +void head_tracking_set_pan_p(uint8_t value) +{ + pan_p=value; + ram_write_byte(DARWIN_HEAD_PAN_P,value); +} + +void head_tracking_set_pan_i(uint8_t value) +{ + pan_i=value; + ram_write_byte(DARWIN_HEAD_PAN_I,value); +} + +void head_tracking_set_pan_d(uint8_t value) +{ + pan_d=value; + ram_write_byte(DARWIN_HEAD_PAN_D,value); +} + +void head_tracking_set_tilt_p(uint8_t value) +{ + tilt_p=value; + ram_write_byte(DARWIN_HEAD_TILT_P,value); +} + +void head_tracking_set_tilt_i(uint8_t value) +{ + tilt_i=value; + ram_write_byte(DARWIN_HEAD_TILT_I,value); +} + +void head_tracking_set_tilt_d(uint8_t value) +{ + tilt_d=value; + ram_write_byte(DARWIN_HEAD_TILT_D,value); +} + +void head_tracking_set_target_pan(int16_t target) +{ + pan_target=target<<9; + ram_write_word(DARWIN_HEAD_PAN_L,target); +} + +void head_tracking_set_target_tilt(int16_t target) +{ + tilt_target=target<<9; + ram_write_word(DARWIN_HEAD_TILT_L,target); +} + +// motion manager process function +void head_tracking_process(void) +{ + int64_t pan_error, tilt_error; + int64_t derivative_pan=0, derivative_tilt=0; + + if(head_tracking_enabled) + { + ram_set_bit(DARWIN_HEAD_STATUS,0x01); + if(manager_get_module(HEAD_PAN)==MM_HEAD) + { + pan_error = pan_target-current_angles[HEAD_PAN]; + integral_pan += (pan_error*head_tracking_period)>>16; + derivative_pan = ((pan_error - prev_error_pan)<<16)/head_tracking_period; + current_angles[HEAD_PAN]+=(pan_p*pan_error+pan_i*integral_pan+pan_d*derivative_pan)>>10; + prev_error_pan = pan_error; + } + if(manager_get_module(HEAD_TILT)==MM_HEAD) + { + tilt_error = tilt_target-current_angles[HEAD_TILT]; + integral_tilt += (tilt_error*head_tracking_period)>>16; + derivative_tilt = ((tilt_error - prev_error_tilt)<<16)/head_tracking_period; + current_angles[HEAD_TILT]+=(tilt_p*tilt_error+tilt_i*integral_tilt+tilt_d*derivative_tilt)>>10; + prev_error_tilt = tilt_error; + } + } + else + ram_clear_bit(DARWIN_HEAD_STATUS,0x01); +} + diff --git a/src/imu.c b/src/imu.c index c7c6a3960cc09242ea647a082d2abd463c614187..dfe58628627a36d361893ae6c8ea68c89303a92c 100755 --- a/src/imu.c +++ b/src/imu.c @@ -104,9 +104,10 @@ const uint8_t gyro_conf_len=6; uint8_t gyro_conf_data[6]; const uint8_t gyro_data_reg=GYRO_OUT_X_L; const uint8_t gyro_data_len=6; -int16_t gyro_x_center; -int16_t gyro_y_center; -int16_t gyro_z_center; +int16_t gyro_center[3]; +int32_t gyro_data[3]; +uint8_t gyro_calibration; +const uint16_t gyro_cal_num_samples=1024; // accelerometer variables const uint8_t accel_conf_reg=ACCEL_CTRL_REG1; const uint8_t accel_conf_len=5; @@ -247,6 +248,19 @@ void imu_accel_get_data(void) imu_spi_start_read(accel_data_reg,accel_data_len); } +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))<<1; + data_out[i*2]=value%256; + data_out[i*2+1]=value/256; + } +} + // gyroscope private functions uint8_t imu_gyro_detect(void) { @@ -302,6 +316,18 @@ void imu_gyro_get_data(void) imu_spi_start_read(gyro_data_reg,gyro_data_len); } +void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out) +{ + uint8_t i; + + for(i=0;i<3;i++) + { + gyro_data[i]=(((int16_t)(data_in[i*2]+(data_in[i*2+1]<<8)))*70)-gyro_center[i]; + data_out[i*2]=gyro_data[i]%256; + data_out[i*2+1]=gyro_data[i]/256; + } +} + // interrupt handlers void IMU_SPI_IRQHANDLER(void) { @@ -335,10 +361,13 @@ void IMU_SPI_IRQHANDLER(void) void IMU_TIMER_IRQHandler(void) { - static imu_state_t state=IMU_GET_GYRO; + static imu_state_t state=IMU_GET_ACCEL; + static int32_t gyro_accum[3]; + static int16_t num_samples=0; + uint8_t data[6]; TIM_ClearITPendingBit(IMU_TIMER,TIM_IT_Update); - + if(imu_is_op_done()) { if(imu_stop_flag==0x01) @@ -355,12 +384,44 @@ void IMU_TIMER_IRQHandler(void) { switch(state) { - case IMU_GET_ACCEL: imu_spi_get_data(&ram_data[DARWIN_IMU_GYRO_X_L]); - imu_accel_get_data(); + case IMU_GET_ACCEL: if(ram_data[DARWIN_IMU_STATUS]&0x02) + { + imu_spi_get_data(data); + imu_gyro_convert_data(data,&ram_data[DARWIN_IMU_GYRO_X_L]); + if(gyro_calibration) + { + gyro_accum[0]+=gyro_data[0]; + gyro_accum[1]+=gyro_data[1]; + gyro_accum[2]+=gyro_data[2]; + num_samples++; + if(num_samples==gyro_cal_num_samples) + { + num_samples=0; + gyro_center[0]=gyro_accum[0]/gyro_cal_num_samples; + gyro_center[1]=gyro_accum[1]/gyro_cal_num_samples; + gyro_center[2]=gyro_accum[2]/gyro_cal_num_samples; + gyro_accum[0]=0; + gyro_accum[1]=0; + gyro_accum[2]=0; + ram_set_bit(DARWIN_IMU_STATUS,3); + ram_clear_bit(DARWIN_IMU_CONTROL,1); + if((ram_data[DARWIN_IMU_STATUS]&0x04)==0x00) + imu_stop_flag=0x01; + gyro_calibration=0x00; + } + } + } + if(ram_data[DARWIN_IMU_STATUS]&0x01) + imu_accel_get_data(); state=IMU_GET_GYRO; break; - case IMU_GET_GYRO: imu_spi_get_data(&ram_data[DARWIN_IMU_ACCEL_X_L]); - imu_gyro_get_data(); + case IMU_GET_GYRO: if(ram_data[DARWIN_IMU_STATUS]&0x01) + { + imu_spi_get_data(data); + imu_accel_convert_data(data,&ram_data[DARWIN_IMU_ACCEL_X_L]); + } + if(ram_data[DARWIN_IMU_STATUS]&0x02) + imu_gyro_get_data(); state=IMU_GET_ACCEL; break; } @@ -391,11 +452,11 @@ void imu_init(void) GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_Init(IMU_ACCEL_CS_PORT, &GPIO_InitStructure); + GPIO_SetBits(IMU_ACCEL_CS_PORT,IMU_ACCEL_CS_PIN); GPIO_InitStructure.GPIO_Pin = IMU_GYRO_CS_PIN; GPIO_Init(IMU_GYRO_CS_PORT, &GPIO_InitStructure); // disable both sensors - GPIO_SetBits(IMU_ACCEL_CS_PORT,IMU_ACCEL_CS_PIN); GPIO_SetBits(IMU_GYRO_CS_PORT,IMU_GYRO_CS_PIN); // initliaze the SPI peripheral @@ -421,6 +482,10 @@ void imu_init(void) } imu_op_state=IMU_DONE; imu_current_device_id=0x00; + gyro_center[0]=0; + gyro_center[1]=0; + gyro_center[2]=0; + gyro_calibration=0x00; // configure the SPI module SPI_StructInit(&SPI_InitStructure); @@ -430,15 +495,15 @@ void imu_init(void) SPI_InitStructure.SPI_CPOL = SPI_CPOL_High; SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge; SPI_InitStructure.SPI_NSS = SPI_NSS_Soft; - SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; + SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256; SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; SPI_InitStructure.SPI_CRCPolynomial = 7; SPI_Init(IMU_SPI, &SPI_InitStructure); /* Configure the SPI interrupt priority */ NVIC_InitStructure.NVIC_IRQChannel = IMU_SPI_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); SPI_I2S_ITConfig(IMU_SPI, SPI_I2S_IT_RXNE , ENABLE); @@ -454,41 +519,75 @@ void imu_init(void) TIM_TimeBaseInit(IMU_TIMER,&TIM_TimeBaseStructure); NVIC_InitStructure.NVIC_IRQChannel = IMU_TIMER_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); + if(imu_gyro_detect()) + { + imu_gyro_get_config(); + imu_gyro_set_config(); + } if(imu_accel_detect()) { imu_accel_get_config(); imu_accel_set_config(); } // detect the sensors - if(imu_gyro_detect()) - { - imu_gyro_get_config(); - imu_gyro_set_config(); - } } void imu_start(void) { - // start the accelerometer - imu_accel_start(); - imu_gyro_start(); - - // start the timer to get the imu data - TIM_Cmd(IMU_TIMER, ENABLE); - TIM_ITConfig(IMU_TIMER, TIM_IT_Update, ENABLE); - ram_set_bit(DARWIN_IMU_STATUS,2); + if((ram_data[DARWIN_IMU_STATUS]&0x04)==0x00)// imu is not running + { + // start the accelerometer + imu_accel_start(); + imu_gyro_start(); + + // start the timer to get the imu data + TIM_Cmd(IMU_TIMER, ENABLE); + TIM_ITConfig(IMU_TIMER, TIM_IT_Update, ENABLE); + ram_set_bit(DARWIN_IMU_STATUS,2); + ram_set_bit(DARWIN_IMU_CONTROL,0); + } } void imu_stop(void) { - imu_stop_flag=0x01; - while(!imu_stopped); - imu_stopped=0x00; - imu_accel_stop(); - imu_gyro_stop(); + if(ram_data[DARWIN_IMU_STATUS]&0x04) + { + imu_stop_flag=0x01; + while(!imu_stopped); + imu_stopped=0x00; + imu_accel_stop(); + imu_gyro_stop(); + ram_clear_bit(DARWIN_IMU_CONTROL,0); + } +} + +void imu_start_gyro_cal(void) +{ + if((ram_data[DARWIN_IMU_STATUS]&0x04)==0x00)// imu is not running + { + // start the accelerometer + imu_accel_start(); + imu_gyro_start(); + + // start the timer to get the imu data + TIM_Cmd(IMU_TIMER, ENABLE); + TIM_ITConfig(IMU_TIMER, TIM_IT_Update, ENABLE); + } + ram_clear_bit(DARWIN_IMU_STATUS,3); + gyro_center[0]=0; + gyro_center[1]=0; + gyro_center[2]=0; + gyro_calibration=0x01; +} + +void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z) +{ + *x=gyro_data[0]; + *y=gyro_data[1]; + *z=gyro_data[2]; } diff --git a/src/joint_motion.c b/src/joint_motion.c new file mode 100644 index 0000000000000000000000000000000000000000..0672858f8856f35ff03948b60824ac0433f4c637 --- /dev/null +++ b/src/joint_motion.c @@ -0,0 +1,180 @@ +#include "joint_motion.h" +#include "motion_manager.h" +#include "ram.h" +#include "gpio.h" +#include <stdlib.h> + +// private variables +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_current_angles[MANAGER_MAX_NUM_SERVOS];//48|16 +int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16 + +int8_t joint_dir[MANAGER_MAX_NUM_SERVOS];// joint_direction + +uint8_t joint_stop; +uint8_t joint_moving; +int64_t joint_motion_period; + +// public functions +void joint_motion_init(uint16_t period) +{ + uint8_t i; + + /* initialize the internal variables */ + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) + { + // target values + joint_target_angles[i]=current_angles[i]; + joint_target_speeds[i]=0; + joint_target_accels[i]=0; + // current values + joint_current_angles[i]=current_angles[i]; + joint_current_speeds[i]=0; + // the current angles, speeds and accelerations are in RAM + joint_dir[i]=0; + } + joint_stop=0x00; + joint_motion_period=period; +} + +void joint_motion_set_period(uint16_t period) +{ + joint_motion_period=period; +} + +uint16_t joint_motion_get_period(void) +{ + return joint_motion_period; +} + +inline void joint_motion_load_target_angle(uint8_t servo_id, int16_t angle) +{ + if(angle>manager_get_ccw_angle_limit(servo_id)) + angle=manager_get_ccw_angle_limit(servo_id); + else if(angle<manager_get_cw_angle_limit(servo_id)) + angle=manager_get_cw_angle_limit(servo_id); + joint_target_angles[servo_id]=angle<<9; +} + +inline void joint_motion_load_target_speed(uint8_t servo_id, uint8_t speed) +{ + joint_target_speeds[servo_id]=speed<<16; +} + +inline void joint_motion_load_target_accel(uint8_t servo_id, uint8_t accel) +{ + joint_target_accels[servo_id]=accel<<16; +} + +void joint_motion_start(void) +{ + uint8_t i; + + /* initialize the internal variables */ + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) + { + // current values + joint_current_angles[i]=current_angles[i]; + // the current angles, speeds and accelerations are in RAM + if(joint_target_angles[i]>=joint_current_angles[i]) + joint_dir[i]=1; + else + joint_dir[i]=-1; + } + joint_moving=0x01; + ram_set_bit(DARWIN_JOINTS_CONTROL,0); + ram_clear_bit(DARWIN_JOINTS_CONTROL,1); + ram_set_bit(DARWIN_JOINTS_STATUS,0); +} + +void joint_motion_stop(void) +{ + joint_stop=0x01; + ram_set_bit(DARWIN_JOINTS_CONTROL,1); +} + +uint8_t are_joints_moving(void) +{ + return joint_moving; +} + +// motion manager interface functions +void joint_motion_process(void) +{ + uint8_t moving=0x00,i; + + if(joint_moving==0x01) + { + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) + { + if(manager_get_module(i)==MM_JOINTS) + { + if(joint_stop==0x01) + { + joint_target_angles[i]=current_angles[i]; + joint_target_speeds[i]=0; + joint_target_accels[i]=0; + joint_current_speeds[i]=0; + } + else + { + if(abs(joint_target_angles[i]-joint_current_angles[i])>=6553)// 0.5 degrees + { + moving=0x01; + if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i]) + { + if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i]) + { + joint_current_speeds[i]=joint_current_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16); + if(joint_current_speeds[i]<joint_dir[i]*joint_target_speeds[i]) + joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; + } + else + { + joint_current_speeds[i]=joint_current_speeds[i]+((joint_target_accels[i]*joint_motion_period)>>16); + if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i]) + joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; + } + joint_current_angles[i]=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]) + joint_current_angles[i]=joint_target_angles[i]; + if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i]) + joint_current_angles[i]=joint_target_angles[i]; + } + else + { + if(abs(joint_target_angles[i]-joint_current_angles[i])>(joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i]))) + { + joint_current_angles[i]=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]) + joint_current_angles[i]=joint_target_angles[i]; + if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i]) + joint_current_angles[i]=joint_target_angles[i]; + } + else + { + joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16); + joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; + joint_current_angles[i]=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]) + joint_current_angles[i]=joint_target_angles[i]; + if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i]) + joint_current_angles[i]=joint_target_angles[i]; + } + } + } + current_angles[i]=joint_current_angles[i]; + } + } + } + if(moving==0) + { + joint_moving=0x00; + ram_clear_bit(DARWIN_JOINTS_STATUS,0); + } + } +} + diff --git a/src/motion_manager.c b/src/motion_manager.c index de103bc1c928266a40ecbcac89d6f1a872a0b6ea..17ceba7f5aa979d78c63da1e3d5f975cc7a074f9 100755 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -1,10 +1,15 @@ #include "motion_manager.h" #include "dynamixel_master_uart_dma.h" +#include "dynamixel_slave_uart_dma.h" #include "dyn_servos.h" #include "ram.h" #include "action.h" #include "walking.h" +#include "joint_motion.h" +#include "head_tracking.h" #include "time.h" +#include "imu.h" +#include "gpio.h" #define MOTION_TIMER TIM5 #define MOTION_TIMER_IRQn TIM5_IRQn @@ -15,6 +20,16 @@ __IO uint16_t manager_motion_period; uint8_t manager_num_servos; TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS]; +int64_t current_angles[MANAGER_MAX_NUM_SERVOS]; +int64_t balance_offset[MANAGER_MAX_NUM_SERVOS]; +TBulkData bulk_data[MANAGER_MAX_NUM_SERVOS]; +uint8_t balance_enabled; + +// balane variables +float knee_gain=0.3; +float ankle_pitch_gain=0.9; +float hip_roll_gain=0.5; +float ankle_roll_gain=1.0; // private functions void manager_send_motion_command(void) @@ -23,9 +38,22 @@ void manager_send_motion_command(void) uint8_t data[256]; uint8_t i,num=0; - for(i=0;i<manager_num_servos;i++) + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) + { + if(manager_servos[i].enabled && manager_servos[i].model!=0x0000 && manager_servos[i].dyn_version==0x02) + { + servo_ids[num]=manager_servos[i].id; + data[num*2]=manager_servos[i].current_value%256; + data[num*2+1]=manager_servos[i].current_value/256; + num++; + } + } + if(num>0) + dyn2_master_sync_write(num,servo_ids,XL_GOAL_POSITION_L,2,data); + num=0; + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) { - if(manager_servos[i].enabled) + if(manager_servos[i].enabled && manager_servos[i].model!=0x0000 && manager_servos[i].dyn_version==0x01) { servo_ids[num]=manager_servos[i].id; data[num*2]=manager_servos[i].current_value%256; @@ -61,17 +89,37 @@ inline uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value) void manager_get_current_position(void) { - uint8_t i; + uint8_t i,num=0,id; - for(i=0;i<manager_num_servos;i++) + for(i=0;i<27;i++) { - if(!manager_servos[i].enabled)// servo is disabled + if((!manager_servos[i].enabled && manager_servos[i].model!=0x0000 && manager_servos[i].dyn_version==0x01) || manager_servos[i].module==MM_HEAD)// servo is disabled { - dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value); - manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value); + bulk_data[num].id=manager_servos[i].id; + bulk_data[num].length=2; + bulk_data[num].address=P_PRESENT_POSITION_L; + bulk_data[num].data=(uint8_t *)&manager_servos[i].current_value; + num++; + } + else + { + ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF); + ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8); + } + } + if(num>0) + { + if(dyn_master_bulk_read(num,bulk_data)==DYN_SUCCESS) + { + for(i=0;i<num;i++) + { + id=bulk_data[i].id; + manager_servos[id].current_angle=manager_value_to_angle(id,manager_servos[id].current_value); + current_angles[id]=manager_servos[id].current_angle<<9; + ram_data[DARWIN_MM_SERVO0_CUR_POS_L+id*2]=(manager_servos[id].current_angle&0xFF); + ram_data[DARWIN_MM_SERVO0_CUR_POS_H+id*2]=(manager_servos[id].current_angle>>8); + } } - ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF); - ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8); } } @@ -79,49 +127,77 @@ void manager_get_target_position(void) { uint8_t i; - for(i=0;i<manager_num_servos;i++) + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) { if(manager_servos[i].enabled)// servo is enabled { - if(manager_servos[i].module==MM_ACTION) - { - manager_servos[i].current_angle=((action_current_angles[i+1])>>9); - //>>16 from the action codification, <<7 from the manager codification - manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle); - } - else if(manager_servos[i].module==MM_WALKING) - { - manager_servos[i].current_angle=walking_current_angles[i+1]; - manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle); - } + manager_servos[i].current_angle=((current_angles[i]+balance_offset[i])>>9); + //>>16 from the action codification, <<7 from the manager codification + manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle); } } } +void manager_balance(void) +{ + int32_t gyro_x,gyro_y,gyro_z; + + if(balance_enabled) + { + imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z); + // filter out some noise + if(gyro_x>0 && gyro_x<4096) + gyro_x=0; + else if(gyro_x<0 && gyro_x>-4096) + gyro_x=0; + if(gyro_y>0 && gyro_y<4096) + gyro_y=0; + else if(gyro_y<0 && gyro_y>-4096) + gyro_y=0; + + balance_offset[R_HIP_ROLL]=(int32_t)(gyro_x*hip_roll_gain*6.428571); // R_HIP_ROLL + balance_offset[L_HIP_ROLL]=(int32_t)(gyro_x*hip_roll_gain*6.428571); // L_HIP_ROLL + balance_offset[R_KNEE]=(int32_t)(gyro_y*knee_gain*-6.428571); // R_KNEE + balance_offset[L_KNEE]=(int32_t)(gyro_y*knee_gain*6.428571); // L_KNEE + + balance_offset[R_ANKLE_PITCH]=(int32_t)(gyro_y*ankle_pitch_gain*6.428571); // R_ANKLE_PITCH + balance_offset[L_ANKLE_PITCH]=(int32_t)(gyro_y*ankle_pitch_gain*-6.428571); // L_ANKLE_PITCH + balance_offset[R_ANKLE_ROLL]=(int32_t)(gyro_x*ankle_roll_gain*6.428571); // R_ANKLE_ROLL + balance_offset[L_ANKLE_ROLL]=(int32_t)(gyro_x*ankle_roll_gain*6.428571); // L_ANKLE_ROLL + } +} + // interrupt handlers void MOTION_TIMER_IRQHandler(void) { - uint16_t capture; + uint16_t capture_start,capture_end; + int16_t elapsed; if(TIM_GetITStatus(MOTION_TIMER, TIM_IT_CC1)!=RESET) { TIM_ClearITPendingBit(MOTION_TIMER,TIM_IT_CC1); - capture = TIM_GetCapture1(MOTION_TIMER); - TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period); + capture_start = TIM_GetCapture1(MOTION_TIMER); + TIM_SetCompare1(MOTION_TIMER, capture_start + manager_motion_period); + // get the disabled servos position + manager_get_current_position(); // call the action process action_process(); // call the joint motion process - // joint_motion_process(); + joint_motion_process(); // call the walking process walking_process(); + // head tracking process + head_tracking_process(); // balance the robot - // manager_balance(); + manager_balance(); // get the target angles from all modules manager_get_target_position(); // send the motion commands to the servos manager_send_motion_command(); - // get the disabled servos position -// manager_get_current_position(); + capture_end = TIM_GetCounter(MOTION_TIMER); + elapsed=capture_end-capture_start; + if(elapsed<0) elapsed+=0xFFFFF; + ram_write_word(DARWIN_MM_CUR_PERIOD,(uint16_t)elapsed); } } @@ -132,8 +208,8 @@ void manager_init(uint16_t period_us) TIM_OCInitTypeDef TIM_OCInitStructure; NVIC_InitTypeDef NVIC_InitStructure; uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS]; + uint16_t model,value; uint8_t i,num=0; - uint16_t model; RCC_APB1PeriphClockCmd(MOTION_TIMER_CLK,ENABLE); @@ -144,12 +220,13 @@ void manager_init(uint16_t period_us) // detect the servos connected dyn_master_scan(&num,servo_ids); ram_data[DARWIN_MM_NUM_SERVOS]=num; + manager_num_servos=0; for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) { - if(i<num) + if(i==servo_ids[manager_num_servos]) { // read the model of the i-th device - dyn_master_read_word(servo_ids[i],P_MODEL_NUMBER_L,&model); + dyn_master_read_word(servo_ids[manager_num_servos],P_MODEL_NUMBER_L,&model); switch(model) { case SERVO_AX12A: manager_servos[i].encoder_resolution=1023; @@ -214,22 +291,28 @@ void manager_init(uint16_t period_us) break; default: break; } - manager_servos[i].id=servo_ids[i]; + manager_servos[i].id=servo_ids[manager_num_servos]; manager_servos[i].model=model; manager_servos[i].module=MM_NONE; manager_servos[i].enabled=0x00; + manager_servos[i].dyn_version=0x01; // get the servo's current position dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value); manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value); ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF); ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8); + // read the servo limits + dyn_master_read_word(manager_servos[i].id,P_CW_ANGLE_LIMIT_L,&value); + manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value); + dyn_master_read_word(manager_servos[i].id,P_CCW_ANGLE_LIMIT_L,&value); + manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value); // set the action current angles - action_current_angles[i+1]=manager_servos[i].current_angle<<9; + current_angles[i]=manager_servos[i].current_angle<<9; manager_num_servos++; } else { - manager_servos[i].id=servo_ids[i]; + manager_servos[i].id=i; manager_servos[i].model=0x0000; manager_servos[i].module=MM_NONE; manager_servos[i].encoder_resolution=0; @@ -240,35 +323,94 @@ void manager_init(uint16_t period_us) manager_servos[i].current_value=0; manager_servos[i].current_angle=0; manager_servos[i].enabled=0x00; + manager_servos[i].cw_angle_limit=0; + manager_servos[i].ccw_angle_limit=0; + manager_servos[i].dyn_version=0x00; + } + } + // scan for version 2 servos + dyn2_master_scan(&num,servo_ids); + ram_data[DARWIN_MM_NUM_SERVOS]=manager_num_servos; + manager_num_servos=0; + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) + { + if(i==servo_ids[manager_num_servos]) + { + // read the model of the i-th device + dyn2_master_read_word(servo_ids[manager_num_servos],XL_MODEL_NUMBER_L,&model); + switch(model) + { + case SERVO_XL320: manager_servos[i].encoder_resolution=1023; + manager_servos[i].gear_ratio=238; + manager_servos[i].max_angle=300<<7; + manager_servos[i].center_angle=150<<7; + manager_servos[i].max_speed=690; + break; + default: break; + } + manager_servos[i].id=servo_ids[manager_num_servos]; + manager_servos[i].model=model; + manager_servos[i].module=MM_NONE; + manager_servos[i].enabled=0x00; + manager_servos[i].dyn_version=0x02; + // get the servo's current position + dyn2_master_read_word(manager_servos[i].id,XL_PRESENT_POSITION_L,&manager_servos[i].current_value); + manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value); + ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF); + ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8); + // read the servo limits + dyn2_master_read_word(manager_servos[i].id,XL_CW_ANGLE_LIMIT_L,&value); + manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value); + dyn2_master_read_word(manager_servos[i].id,XL_CCW_ANGLE_LIMIT_L,&value); + manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value); + // set the action current angles + current_angles[i]=manager_servos[i].current_angle<<9; + manager_num_servos++; } } dyn_master_disable_power(); - manager_num_servos=num; + manager_num_servos=num+ram_data[DARWIN_MM_NUM_SERVOS]; + ram_data[DARWIN_MM_NUM_SERVOS]=((num&0x07)<<5)+(ram_data[DARWIN_MM_NUM_SERVOS]&0x1F); // initialize the timer interrupts NVIC_InitStructure.NVIC_IRQChannel = MOTION_TIMER_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); /* motion timer configuration */ TIM_TimeBaseStructure.TIM_Period = 0xFFFF; - TIM_TimeBaseStructure.TIM_Prescaler = 36; + TIM_TimeBaseStructure.TIM_Prescaler = 72; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(MOTION_TIMER,&TIM_TimeBaseStructure); - TIM_SetClockDivision(MOTION_TIMER,TIM_CKD_DIV1); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; - manager_motion_period=period_us; + manager_motion_period=(period_us*1000000)>>16; TIM_OCInitStructure.TIM_Pulse = manager_motion_period; TIM_OC1Init(MOTION_TIMER, &TIM_OCInitStructure); TIM_OC1PreloadConfig(MOTION_TIMER, TIM_OCPreload_Disable); ram_clear_bit(DARWIN_MM_STATUS,0); + + /* initialize motion modules */ + action_init(period_us); + walking_init(manager_motion_period); + joint_motion_init(period_us); + head_tracking_init(period_us); + + ram_write_byte(DARWIN_MM_KNEE_GAIN,(uint8_t)(knee_gain*64.0)); + ram_write_byte(DARWIN_MM_ANKLE_PITCH_GAIN,(uint8_t)(ankle_pitch_gain*64.0)); + ram_write_byte(DARWIN_MM_HIP_ROLL_GAIN,(uint8_t)(hip_roll_gain*64.0)); + ram_write_byte(DARWIN_MM_ANKLE_ROLL_GAIN,(uint8_t)(ankle_roll_gain*64.0)); + + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) + balance_offset[i]=0; + balance_enabled=0x01; + ram_data[DARWIN_MM_CONTROL]=0x02; } uint16_t manager_get_period(void) @@ -278,14 +420,24 @@ uint16_t manager_get_period(void) void manager_set_period(uint16_t period_us) { - manager_motion_period=period_us; + manager_motion_period=(period_us*1000000)>>16; + ram_write_word(DARWIN_MM_PERIOD_H,period_us); + action_set_period(period_us); + walking_set_period(manager_motion_period); + joint_motion_set_period(period_us); + head_tracking_set_period(period_us); } inline void manager_enable(void) { + uint16_t capture; + + capture = TIM_GetCapture1(MOTION_TIMER); + TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period); TIM_Cmd(MOTION_TIMER, ENABLE); TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, ENABLE); ram_set_bit(DARWIN_MM_STATUS,0); + ram_set_bit(DARWIN_MM_CONTROL,0); } inline void manager_disable(void) @@ -293,6 +445,7 @@ inline void manager_disable(void) TIM_Cmd(MOTION_TIMER, DISABLE); TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, DISABLE); ram_clear_bit(DARWIN_MM_STATUS,0); + ram_clear_bit(DARWIN_MM_CONTROL,0); } inline uint8_t manager_is_enabled(void) @@ -300,6 +453,20 @@ inline uint8_t manager_is_enabled(void) return ram_data[DARWIN_MM_STATUS]&0x01; } +void manager_enable_balance(void) +{ + ram_set_bit(DARWIN_MM_STATUS,1); + ram_set_bit(DARWIN_MM_CONTROL,1); + balance_enabled=0x01; +} + +void manager_disable_balance(void) +{ + ram_clear_bit(DARWIN_MM_STATUS,1); + ram_clear_bit(DARWIN_MM_CONTROL,1); + balance_enabled=0x00; +} + inline uint8_t manager_get_num_servos(void) { return manager_num_servos; @@ -307,8 +474,18 @@ inline uint8_t manager_get_num_servos(void) void manager_set_module(uint8_t servo_id,TModules module) { + uint8_t byte; + if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS) + { manager_servos[servo_id].module=module; + ram_read_byte(DARWIN_MM_MODULE_EN0+servo_id/2,&byte); + if(servo_id%2) + byte=(byte&0xF8)+((uint8_t)module); + else + byte=(byte&0x8F)+(((uint8_t)module)<<4); + ram_write_byte(DARWIN_MM_MODULE_EN0+servo_id/2,byte); + } } inline TModules manager_get_module(uint8_t servo_id) @@ -321,14 +498,34 @@ inline TModules manager_get_module(uint8_t servo_id) inline void manager_enable_servo(uint8_t servo_id) { + uint8_t byte; + if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS) + { manager_servos[servo_id].enabled=0x01; + ram_read_byte(DARWIN_MM_MODULE_EN0+servo_id/2,&byte); + if(servo_id%2) + byte|=0x08; + else + byte|=0x80; + ram_write_byte(DARWIN_MM_MODULE_EN0+servo_id/2,byte); + } } inline void manager_disable_servo(uint8_t servo_id) { + uint8_t byte; + if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS) + { manager_servos[servo_id].enabled=0x00; + ram_read_byte(DARWIN_MM_MODULE_EN0+servo_id/2,&byte); + if(servo_id%2) + byte&=0xF7; + else + byte&=0x7F; + ram_write_byte(DARWIN_MM_MODULE_EN0+servo_id/2,byte); + } } void manager_disable_servos(void) @@ -339,7 +536,7 @@ void manager_disable_servos(void) for(i=0;i<manager_num_servos;i++) { - if(!manager_servos[i].enabled) + if(!manager_servos[i].enabled && manager_servos[i].dyn_version==0x01) { servo_ids[num]=manager_servos[i].id; data[num]=0x00; @@ -358,3 +555,42 @@ inline uint8_t manager_is_servo_enabled(uint8_t servo_id) return 0x00; } +inline int16_t manager_get_cw_angle_limit(uint8_t servo_id) +{ + return manager_servos[servo_id].cw_angle_limit; +} + +inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id) +{ + return manager_servos[servo_id].ccw_angle_limit; +} + +void manager_set_knee_gain(uint8_t gain) +{ + ram_data[DARWIN_MM_KNEE_GAIN]=gain; + knee_gain=gain/64.0; +} + +void manager_set_ankle_pitch_gain(uint8_t gain) +{ + ram_data[DARWIN_MM_ANKLE_PITCH_GAIN]=gain; + ankle_pitch_gain=gain/64.0; +} + +void manager_set_hip_roll_gain(uint8_t gain) +{ + ram_data[DARWIN_MM_HIP_ROLL_GAIN]=gain; + hip_roll_gain=gain/64.0; +} + +void manager_set_ankle_roll_gain(uint8_t gain) +{ + ram_data[DARWIN_MM_ANKLE_ROLL_GAIN]=gain; + ankle_roll_gain=gain/64.0; +} + +uint8_t manager_get_dyn_version(uint8_t servo_id) +{ + return manager_servos[servo_id].dyn_version; +} + diff --git a/src/ram.c b/src/ram.c index 0bdd49976e589d126604cae8c42e207f25854b58..65cf3bc6a3ffc215f9179f2d3f102e573a79d72e 100755 --- a/src/ram.c +++ b/src/ram.c @@ -4,8 +4,10 @@ uint8_t ram_data[RAM_SIZE]; void ram_init(void) { - uint16_t eeprom_data; + uint16_t eeprom_data,i; + 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); diff --git a/src/walking.c b/src/walking.c index 2c3a264235dafb8fba2a6ea14042479a37838248..13d03c12bb971f658d568c638f3fe2de9ccd2fac 100755 --- a/src/walking.c +++ b/src/walking.c @@ -3,11 +3,10 @@ #include "ram.h" #include "darwin_kinematics.h" #include "darwin_math.h" +#include "time.h" enum {PHASE0 = 0,PHASE1 = 1,PHASE2 = 2, PHASE3 = 3}; -int64_t walking_current_angles[MANAGER_MAX_NUM_SERVOS]; - // leg kinematics const uint8_t servo_seq[14]={R_HIP_YAW,R_HIP_ROLL,R_HIP_PITCH,R_KNEE,R_ANKLE_PITCH,R_ANKLE_ROLL,L_HIP_YAW,L_HIP_ROLL,L_HIP_PITCH,L_KNEE,L_ANKLE_PITCH,L_ANKLE_ROLL,R_SHOULDER_PITCH,L_SHOULDER_PITCH}; @@ -18,7 +17,7 @@ float Y_SWAP_AMPLITUDE=20.0;//mm float Z_SWAP_AMPLITUDE=5;//mm float ARM_SWING_GAIN=1.5; float PELVIS_OFFSET=3.0;//degrees -float HIP_PITCH_OFFSET=23.0;//degrees +float HIP_PITCH_OFFSET=0;//23.0;//degrees uint8_t P_GAIN=32; uint8_t I_GAIN=0; @@ -33,15 +32,21 @@ float P_OFFSET=0; float R_OFFSET=0; // Walking control -float PERIOD_TIME=600; +float PERIOD_TIME=600;//milliseconds float DSP_RATIO=0.1; float STEP_FB_RATIO=0.3; -float X_MOVE_AMPLITUDE=10;//mm +float X_MOVE_AMPLITUDE=0;//mm +float current_x_amplitude=0; float Y_MOVE_AMPLITUDE=0;//mm +float current_y_amplitude=0; float Z_MOVE_AMPLITUDE=40;//mm float A_MOVE_AMPLITUDE=0;//degrees +float current_a_amplitude=0; uint8_t A_MOVE_AIM_ON=0x00; +float max_acceleration=0.02;// m/s/s +float max_rot_accel=0.02;// rad/s/s + // private variables float m_PeriodTime; float m_DSP_Ratio; @@ -98,6 +103,7 @@ float m_Arm_Swing_Gain; volatile uint8_t m_Ctrl_Running; float m_Time; +float walking_period; uint8_t m_Phase; float m_Body_Swing_Y; @@ -106,7 +112,7 @@ float m_Body_Swing_Z; // private functions inline float wsin(float time, float period, float period_shift, float mag, float mag_shift) { - return mag * sin(2 * 3.141592 / period * time - period_shift) + mag_shift; + return mag * sin(((2.0*PI*time)/period)-period_shift) + mag_shift; } void update_param_time() @@ -115,15 +121,16 @@ void update_param_time() m_DSP_Ratio = DSP_RATIO; m_SSP_Ratio = 1 - DSP_RATIO; + m_SSP_Time = m_PeriodTime * m_SSP_Ratio; + m_X_Swap_PeriodTime = m_PeriodTime / 2.0; - m_X_Move_PeriodTime = m_PeriodTime * m_SSP_Ratio; + m_X_Move_PeriodTime = m_SSP_Time; m_Y_Swap_PeriodTime = m_PeriodTime; - m_Y_Move_PeriodTime = m_PeriodTime * m_SSP_Ratio; + m_Y_Move_PeriodTime = m_SSP_Time; m_Z_Swap_PeriodTime = m_PeriodTime / 2.0; - m_Z_Move_PeriodTime = m_PeriodTime * m_SSP_Ratio / 2.0; - m_A_Move_PeriodTime = m_PeriodTime * m_SSP_Ratio; + m_Z_Move_PeriodTime = m_SSP_Time / 2.0; + m_A_Move_PeriodTime = m_SSP_Time; - m_SSP_Time = m_PeriodTime * m_SSP_Ratio; m_SSP_Time_Start_L = (1 - m_SSP_Ratio) * m_PeriodTime / 4.0; m_SSP_Time_End_L = (1 + m_SSP_Ratio) * m_PeriodTime / 4.0; m_SSP_Time_Start_R = (3 - m_SSP_Ratio) * m_PeriodTime / 4.0; @@ -141,12 +148,36 @@ void update_param_time() void update_param_move() { - // Forward/Back - m_X_Move_Amplitude = X_MOVE_AMPLITUDE; - m_X_Swap_Amplitude = X_MOVE_AMPLITUDE * STEP_FB_RATIO; + // change longitudinal and transversal velocities to get to the target ones + if(current_x_amplitude<X_MOVE_AMPLITUDE) + { + current_x_amplitude+=max_acceleration*PERIOD_TIME; + if(current_x_amplitude>X_MOVE_AMPLITUDE) + current_x_amplitude=X_MOVE_AMPLITUDE; + } + else if(current_x_amplitude>X_MOVE_AMPLITUDE) + { + current_x_amplitude-=max_acceleration*PERIOD_TIME; + if(current_x_amplitude<X_MOVE_AMPLITUDE) + current_x_amplitude=X_MOVE_AMPLITUDE; + } + m_X_Move_Amplitude = current_x_amplitude; + m_X_Swap_Amplitude = current_x_amplitude * STEP_FB_RATIO; // Right/Left - m_Y_Move_Amplitude = Y_MOVE_AMPLITUDE / 2.0; + if(current_y_amplitude<Y_MOVE_AMPLITUDE) + { + current_y_amplitude+=max_acceleration*PERIOD_TIME; + if(current_y_amplitude>Y_MOVE_AMPLITUDE) + current_y_amplitude=Y_MOVE_AMPLITUDE; + } + else if(current_y_amplitude>Y_MOVE_AMPLITUDE) + { + current_y_amplitude-=max_acceleration*PERIOD_TIME; + if(current_y_amplitude<Y_MOVE_AMPLITUDE) + current_y_amplitude=Y_MOVE_AMPLITUDE; + } + m_Y_Move_Amplitude = current_y_amplitude / 2.0; if(m_Y_Move_Amplitude > 0) m_Y_Move_Amplitude_Shift = m_Y_Move_Amplitude; else @@ -159,6 +190,19 @@ void update_param_move() m_Z_Swap_Amplitude_Shift = m_Z_Swap_Amplitude; // Direction + if(current_a_amplitude<A_MOVE_AMPLITUDE) + { + current_a_amplitude+=max_acceleration*PERIOD_TIME; + if(current_a_amplitude>A_MOVE_AMPLITUDE) + current_a_amplitude=A_MOVE_AMPLITUDE; + } + else if(current_a_amplitude>A_MOVE_AMPLITUDE) + { + current_a_amplitude-=max_acceleration*PERIOD_TIME; + if(current_a_amplitude<A_MOVE_AMPLITUDE) + current_a_amplitude=A_MOVE_AMPLITUDE; + } + if(A_MOVE_AIM_ON == 0x00) { m_A_Move_Amplitude = A_MOVE_AMPLITUDE * PI / 180.0 / 2.0; @@ -189,30 +233,32 @@ void update_param_balance() } // public functions -void walking_init(void) +void walking_init(uint16_t period) { // initialize RAM - ram_write_byte(DARWIN_X_OFFSET,(uint8_t)X_OFFSET); - ram_write_byte(DARWIN_Y_OFFSET,(uint8_t)Y_OFFSET); - ram_write_byte(DARWIN_Z_OFFSET,(uint8_t)Z_OFFSET); - ram_write_byte(DARWIN_ROLL,(uint8_t)(A_OFFSET*10)); - ram_write_byte(DARWIN_PITCH,(uint8_t)(P_OFFSET*10)); - ram_write_byte(DARWIN_YAW,(uint8_t)(R_OFFSET*10)); - ram_write_word(DARWIN_HIP_PITCH_OFF_L,(uint16_t)(HIP_PITCH_OFFSET*10)); - ram_write_word(DARWIN_PERIOD_TIME_L,(uint16_t)PERIOD_TIME); - ram_write_byte(DARWIN_DSP_RATIO,(uint8_t)(DSP_RATIO*10)); - ram_write_byte(DARWIN_STEP_FW_BW_RATIO,(uint8_t)(STEP_FB_RATIO*100)); - ram_write_byte(DARWIN_STEP_FW_BW,(uint8_t)(X_MOVE_AMPLITUDE)); - ram_write_byte(DARWIN_STEP_LEFT_RIGHT,(uint8_t)(Y_MOVE_AMPLITUDE)); - ram_write_byte(DARWIN_STEP_DIRECTION,(uint8_t)(A_MOVE_AMPLITUDE)); - ram_write_byte(DARWIN_FOOT_HEIGHT,(uint8_t)(Z_MOVE_AMPLITUDE)); - ram_write_byte(DARWIN_SWING_RIGHT_LEFT,(uint8_t)(Y_SWAP_AMPLITUDE*10)); - ram_write_byte(DARWIN_SWING_TOP_DOWN,(uint8_t)(Z_SWAP_AMPLITUDE*10)); - ram_write_byte(DARWIN_PELVIS_OFFSET,(uint8_t)(PELVIS_OFFSET)); - ram_write_byte(DARWIN_ARM_SWING_GAIN,(uint8_t)(ARM_SWING_GAIN*10)); - ram_write_byte(DARWIN_P_GAIN,(uint8_t)(P_GAIN)); - ram_write_byte(DARWIN_I_GAIN,(uint8_t)(I_GAIN)); - ram_write_byte(DARWIN_D_GAIN,(uint8_t)(D_GAIN)); + ram_write_byte(DARWIN_X_OFFSET,(uint8_t)X_OFFSET);//mm + ram_write_byte(DARWIN_Y_OFFSET,(uint8_t)Y_OFFSET);//mm + ram_write_byte(DARWIN_Z_OFFSET,(uint8_t)Z_OFFSET);//mm + ram_write_byte(DARWIN_ROLL,(uint8_t)(A_OFFSET*8));//degrees format 5|3 + ram_write_byte(DARWIN_PITCH,(uint8_t)(P_OFFSET*8));//degrees format 5|3 + ram_write_byte(DARWIN_YAW,(uint8_t)(R_OFFSET*8));//degrees format 5|3 + ram_write_word(DARWIN_HIP_PITCH_OFF_L,(uint16_t)(HIP_PITCH_OFFSET*1024));//degrees format 6|10 + ram_write_word(DARWIN_PERIOD_TIME_L,(uint16_t)PERIOD_TIME);// milliseconds + ram_write_byte(DARWIN_DSP_RATIO,(uint8_t)(DSP_RATIO*128));//ratio [0,1] format 1|7 + ram_write_byte(DARWIN_STEP_FW_BW_RATIO,(uint8_t)(STEP_FB_RATIO*128));//ratio [0,1] format 1|7 + ram_write_byte(DARWIN_STEP_FW_BW,(uint8_t)(X_MOVE_AMPLITUDE));//mm + ram_write_byte(DARWIN_STEP_LEFT_RIGHT,(uint8_t)(Y_MOVE_AMPLITUDE));//mm + ram_write_byte(DARWIN_STEP_DIRECTION,(uint8_t)(A_MOVE_AMPLITUDE*8));//degrees 5|3 + ram_write_byte(DARWIN_FOOT_HEIGHT,(uint8_t)(Z_MOVE_AMPLITUDE));//mm + ram_write_byte(DARWIN_SWING_RIGHT_LEFT,(uint8_t)(Y_SWAP_AMPLITUDE));//mm + ram_write_byte(DARWIN_SWING_TOP_DOWN,(uint8_t)(Z_SWAP_AMPLITUDE));//mm + ram_write_byte(DARWIN_PELVIS_OFFSET,(uint8_t)(PELVIS_OFFSET*8));//degrees 5|3 + ram_write_byte(DARWIN_ARM_SWING_GAIN,(uint8_t)(ARM_SWING_GAIN*32));// gain 3|5 + ram_write_byte(DARWIN_P_GAIN,(uint8_t)(P_GAIN));// servo gain + ram_write_byte(DARWIN_I_GAIN,(uint8_t)(I_GAIN));// servo gain + ram_write_byte(DARWIN_D_GAIN,(uint8_t)(D_GAIN));// servo gain + ram_write_byte(DARWIN_MAX_ACC,(uint8_t)(max_acceleration*1024));// maximum linear acceleration + ram_write_byte(DARWIN_MAX_ROT_ACC,(uint8_t)(max_rot_accel*1024));// maximum rotational acceleration // initialize the internal variables m_X_Swap_Phase_Shift=PI; m_X_Swap_Amplitude_Shift=0.0; @@ -227,6 +273,7 @@ void walking_init(void) m_A_Move_Phase_Shift=PI/2.0; m_Time=0; + walking_period=((double)period)/1000.0; m_Body_Swing_Y=0.0; m_Body_Swing_Z=0.0; @@ -239,53 +286,98 @@ void walking_init(void) walking_process(); } -void walking_set_param(uint8_t param_id,uint16_t param_value) -{ - -} - -void walking_enable_autobalance(void) -{ - -} - -void walking_disable_autobalance(void) +inline void walking_set_period(uint16_t period) { - + walking_period=((double)period)/1000.0; } -uint8_t walking_is_autobalance_enabled(void) +inline uint16_t walking_get_period(void) { - return 0x00; + return (uint16_t)(walking_period*1000); } -void walking_enable_aim(void) +void walking_set_param(uint8_t param_id,int8_t param_value) { + uint16_t value; -} - -void walking_disable_aim(void) -{ - -} - -uint8_t walking_is_aim_enabled(void) -{ - return 0x00; + switch(param_id) + { + case DARWIN_X_OFFSET: X_OFFSET=param_value; + break; + case DARWIN_Y_OFFSET: Y_OFFSET=param_value; + break; + case DARWIN_Z_OFFSET: Z_OFFSET=param_value; + break; + case DARWIN_ROLL: A_OFFSET=((float)param_value)/8.0; + break; + case DARWIN_PITCH: P_OFFSET=((float)param_value)/8.0; + break; + case DARWIN_YAW: R_OFFSET=((float)param_value)/8.0; + break; + case DARWIN_HIP_PITCH_OFF_L: value=(uint16_t)(HIP_PITCH_OFFSET*1024); + value&=0xFF00; + value+=param_value; + HIP_PITCH_OFFSET=((float)value)/1024.0; + break; + case DARWIN_HIP_PITCH_OFF_H: value=(uint16_t)(HIP_PITCH_OFFSET*1024); + value&=0x00FF; + value+=param_value*256; + HIP_PITCH_OFFSET=((float)value)/1024.0; + break; + case DARWIN_PERIOD_TIME_L: value=(uint16_t)PERIOD_TIME; + value&=0xFF00; + value+=param_value; + PERIOD_TIME=value; + break; + case DARWIN_PERIOD_TIME_H: value=(uint16_t)PERIOD_TIME; + value&=0x00FF; + value+=param_value*256; + PERIOD_TIME=value; + break; + case DARWIN_DSP_RATIO: DSP_RATIO=((float)param_value)/128.0; + break; + case DARWIN_STEP_FW_BW_RATIO: STEP_FB_RATIO=((float)param_value)/128.0; + break; + case DARWIN_STEP_FW_BW: X_MOVE_AMPLITUDE=param_value; + break; + case DARWIN_STEP_LEFT_RIGHT: Y_MOVE_AMPLITUDE=param_value; + break; + case DARWIN_STEP_DIRECTION: A_MOVE_AMPLITUDE=((float)param_value)/8.0; + break; + case DARWIN_FOOT_HEIGHT: Z_MOVE_AMPLITUDE=param_value; + break; + case DARWIN_SWING_RIGHT_LEFT: Y_SWAP_AMPLITUDE=param_value; + break; + case DARWIN_SWING_TOP_DOWN: Z_SWAP_AMPLITUDE=param_value; + break; + case DARWIN_PELVIS_OFFSET: PELVIS_OFFSET=((float)param_value)/8.0; + break; + case DARWIN_ARM_SWING_GAIN: ARM_SWING_GAIN=((float)param_value)/32.0; + break; + case DARWIN_P_GAIN: P_GAIN=param_value; + break; + case DARWIN_I_GAIN: I_GAIN=param_value; + break; + case DARWIN_D_GAIN: D_GAIN=param_value; + break; + case DARWIN_MAX_ACC: max_acceleration=param_value/1024.0; + break; + case DARWIN_MAX_ROT_ACC: max_rot_accel=param_value/1024.0; + break; + } + ram_write_byte(param_id,param_value); } void walking_start(void) { - ram_write_byte(DARWIN_WALK_STATUS,0x01); - ram_set_bit(DARWIN_WALK_CONTROL,0x00); - ram_clear_bit(DARWIN_WALK_CONTROL,0x01); + ram_set_bit(DARWIN_WALK_STATUS,0); + ram_set_bit(DARWIN_WALK_CONTROL,0); m_Ctrl_Running=0x01; } void walking_stop(void) { - ram_set_bit(DARWIN_WALK_CONTROL,0x01); - ram_clear_bit(DARWIN_WALK_CONTROL,0x00); + ram_set_bit(DARWIN_WALK_CONTROL,1); m_Ctrl_Running=0x00; } @@ -327,12 +419,12 @@ void walking_process(void) } } } - else if(m_Time >= (m_Phase_Time1 - manager_get_period()/2.0) && m_Time < (m_Phase_Time1 + manager_get_period()/2.0)) + else if(m_Time >= (m_Phase_Time1 - walking_period/2.0) && m_Time < (m_Phase_Time1 + walking_period/2.0)) { update_param_move(); m_Phase = PHASE1; } - else if(m_Time >= (m_Phase_Time2 - manager_get_period()/2.0) && m_Time < (m_Phase_Time2 + manager_get_period()/2.0)) + else if(m_Time >= (m_Phase_Time2 - walking_period/2.0) && m_Time < (m_Phase_Time2 + walking_period/2.0)) { update_param_time(); m_Time = m_Phase_Time2; @@ -349,7 +441,7 @@ void walking_process(void) } } } - else if(m_Time >= (m_Phase_Time3 - manager_get_period()/2.0) && m_Time < (m_Phase_Time3 + manager_get_period()/2.0)) + else if(m_Time >= (m_Phase_Time3 - walking_period/2.0) && m_Time < (m_Phase_Time3 + walking_period/2.0)) { update_param_move(); m_Phase = PHASE3; @@ -472,7 +564,7 @@ void walking_process(void) ram_read_byte(DARWIN_WALK_STATUS,&walking); if(walking == 0x01) { - m_Time += 7.8;//manager_get_period(); + m_Time += walking_period; if(m_Time >= m_PeriodTime) m_Time = 0; } @@ -489,14 +581,17 @@ void walking_process(void) // Compute motor value for(i=0; i<14; i++) { - if(i==1) - walking_current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] + pelvis_offset_r))*128;// format 9|7 - else if(i==7) - walking_current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] + pelvis_offset_l))*128;// format 9|7 - else if(i==2 || i==8) - walking_current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] - HIP_PITCH_OFFSET))*128;// format 9|7 - else - walking_current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i] * angle[i])*128;//format 9|7 + if(manager_get_module(i)==MM_WALKING) + { + if(i==1) + current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] + pelvis_offset_r))*65536;// format 9|7 + else if(i==7) + current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] + pelvis_offset_l))*65536;// format 9|7 + else if(i==2 || i==8) + current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] - HIP_PITCH_OFFSET))*65536;// format 9|7 + else + current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i] * angle[i])*65536;//format 9|7 + } } }