Skip to content
Snippets Groups Projects
Commit a9b471a1 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added the joint_motion, head_tracking and wallk motion modules.

parent 74e6c047
No related branches found
No related tags found
No related merge requests found
...@@ -22,6 +22,9 @@ SET(lib_sources ./stm32_libraries/utils/src/buffer.c ...@@ -22,6 +22,9 @@ SET(lib_sources ./stm32_libraries/utils/src/buffer.c
./stm32_libraries/dynamixel_manager/src/modules/motion_manager.c ./stm32_libraries/dynamixel_manager/src/modules/motion_manager.c
./stm32_libraries/dynamixel_manager/src/modules/motion_module.c ./stm32_libraries/dynamixel_manager/src/modules/motion_module.c
./stm32_libraries/dynamixel_manager/src/modules/action.c ./stm32_libraries/dynamixel_manager/src/modules/action.c
./stm32_libraries/dynamixel_manager/src/modules/joint_motion.c
./stm32_libraries/dynamixel_manager/src/modules/head_tracking.c
./stm32_libraries/dynamixel_manager/src/modules/walk.c
./stm32_libraries/dynamixel_manager/src/modules/motion_pages.c ./stm32_libraries/dynamixel_manager/src/modules/motion_pages.c
./stm32_libraries/dynamixel_manager/src/modules/dyn_servos.c) ./stm32_libraries/dynamixel_manager/src/modules/dyn_servos.c)
# application header files # application header files
...@@ -50,6 +53,13 @@ SET(lib_headers ./stm32_libraries/utils/include/buffer.h ...@@ -50,6 +53,13 @@ SET(lib_headers ./stm32_libraries/utils/include/buffer.h
./stm32_libraries/dynamixel_manager/include/modules/motion_module.h ./stm32_libraries/dynamixel_manager/include/modules/motion_module.h
./stm32_libraries/dynamixel_manager/include/modules/action.h ./stm32_libraries/dynamixel_manager/include/modules/action.h
./stm32_libraries/dynamixel_manager/include/modules/motion_pages.h ./stm32_libraries/dynamixel_manager/include/modules/motion_pages.h
./stm32_libraries/dynamixel_manager/include/modules/action_registers.h
./stm32_libraries/dynamixel_manager/include/modules/joint_motion.h
./stm32_libraries/dynamixel_manager/include/modules/joint_motion_registers.h
./stm32_libraries/dynamixel_manager/include/modules/head_tracking.h
./stm32_libraries/dynamixel_manager/include/modules/head_tracking_registers.h
./stm32_libraries/dynamixel_manager/include/modules/walk.h
./stm32_libraries/dynamixel_manager/include/modules/walk_registers.h
./stm32_libraries/dynamixel_manager/include/modules/dyn_servos.h) ./stm32_libraries/dynamixel_manager/include/modules/dyn_servos.h)
# add include directories # add include directories
...@@ -99,7 +109,7 @@ set(TARGET_PROCESSOR STM32F103RE) ...@@ -99,7 +109,7 @@ set(TARGET_PROCESSOR STM32F103RE)
ADD_SUBDIRECTORY(src) ADD_SUBDIRECTORY(src)
# compiler settings # compiler settings
SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -mlittle-endian -mcpu=cortex-m3 -mthumb -mthumb-interwork -Wall -g -fno-common -msoft-float -ffreestanding -imacros ${configuration_file}") SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -mlittle-endian -mcpu=cortex-m3 -mthumb -mthumb-interwork -Wall -g -fno-common -msoft-float -ffreestanding -fno-math-errno -imacros ${configuration_file}")
# linker settings # linker settings
set(LINKER_SCRIPT "${CMAKE_CURRENT_SOURCE_DIR}/linker_script/darwin.ld") set(LINKER_SCRIPT "${CMAKE_CURRENT_SOURCE_DIR}/linker_script/darwin.ld")
...@@ -115,6 +125,8 @@ add_definitions(-DEEPROM_START_ADDRESS=0x08003800) ...@@ -115,6 +125,8 @@ add_definitions(-DEEPROM_START_ADDRESS=0x08003800)
add_executable(${PROJECT_NAME}.elf ${lib_sources} ${hal_sources} ${asm_source} ${sources}) add_executable(${PROJECT_NAME}.elf ${lib_sources} ${hal_sources} ${asm_source} ${sources})
target_link_libraries(${PROJECT_NAME}.elf -lm)
add_custom_command(TARGET "${PROJECT_NAME}.elf" POST_BUILD add_custom_command(TARGET "${PROJECT_NAME}.elf" POST_BUILD
# Build .hex and .bin files # Build .hex and .bin files
COMMAND ${CMAKE_OBJCOPY} -O binary ${PROJECT_NAME}.elf ${PROJECT_NAME}.bin COMMAND ${CMAKE_OBJCOPY} -O binary ${PROJECT_NAME}.elf ${PROJECT_NAME}.bin
......
#ifndef _DARWIN_CONF_H #ifndef _DARWIN_CONF_H
#define _DARWIN_CONF_H #define _DARWIN_CONF_H
#define RAM_SIZE 4096 #define RAM_SIZE 4096
#define EEPROM_SIZE 128 #define EEPROM_SIZE 128
#define NUM_MOTION_PAGES 46
#define MAX_DYN_MASTER_TX_BUFFER_LEN 1024
#define MAX_DYN_MASTER_RX_BUFFER_LEN 1024
#define MAX_DYN_SLAVE_TX_BUFFER_LEN 1024
#define MAX_DYN_SLAVE_RX_BUFFER_LEN 1024
#define MAX_DYN_SLAVE_REG_BUFFER_LEN 1024
#define MAX_NUM_SLAVE_DEVICES 4
#define MAX_DYN_SLAVE_REG_BUFFER_LEN 1024
#define DYN_MANAGER_MAX_NUM_MASTERS 4
#define DYN_MANAGER_MAX_NUM_MODULES 8
#define DYN_MANAGER_MAX_NUM_DEVICES 32
#define DYN_MANAGER_MAX_NUM_SINGLE_OP 16
#define DYN_MANAGER_MAX_NUM_SYNC_OP 4
#define DYN_MANAGER_MAX_NUM_BULK_OP 4
#define MODULE_MAX_NUM_MODELS 32
#define MM_MAX_NUM_MOTION_MODULES 8
#define NUM_JOINT_GROUPS 4
/* Dynamixel slave memory map */ /* Dynamixel slave memory map */
/* dynamixel slave */ /* dynamixel slave */
#define EEPROM_DYN_SLAVE_BASE_ADDRESS1 ((unsigned short int)0x0000) #define EEPROM_DYN_SLAVE_BASE_ADDRESS1 ((unsigned short int)0x0000)
#define EEPROM_DYN_SLAVE_BASE_ADDRESS2 ((unsigned short int)0x0010) #define EEPROM_DYN_SLAVE_BASE_ADDRESS2 ((unsigned short int)0x0010)
/* dynamixel manager */ /* dynamixel manager */
#define EEPROM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0006) #define EEPROM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0006)
#define RAM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0080) #define RAM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0080)
/* motion manager */ /* motion manager */
#define EEPROM_MMANAGER_BASE_ADDRESS ((unsigned short int)0x0011) #define EEPROM_MMANAGER_BASE_ADDRESS ((unsigned short int)0x0011)
#define RAM_MMANAGER_BASE_ADDRESS ((unsigned short int)0x0088) #define RAM_MMANAGER_BASE_ADDRESS ((unsigned short int)0x0088)
/* action motion module */ /* action motion module */
#define RAM_ACTION_MM_BASE_ADDRESS ((unsigned short int)0x009E) #define RAM_ACTION_MM_BASE_ADDRESS ((unsigned short int)0x009E)
/* IMU */
#define RAM_IMU_MM_BASE_ADDRESS ((unsigned short int)0x00A0)
/* balance */ /* balance */
#define EEPROM_BALANCE_BASE_ADDRESS ((unsigned short int)0x0007) #define EEPROM_BALANCE_BASE_ADDRESS ((unsigned short int)0x0007)
#define RAM_BALANCE_BASE_ADDRESS ((unsigned short int)0x00AF) #define RAM_BALANCE_BASE_ADDRESS ((unsigned short int)0x00A0)
#define NUM_MOTION_PAGES 46 /* action motion module */
#define MAX_DYN_MASTER_TX_BUFFER_LEN 1024 #define RAM_JOINT_MOTION_MM_BASE_ADDRESS ((unsigned short int)0x00A1)
#define MAX_DYN_MASTER_RX_BUFFER_LEN 1024
#define MAX_DYN_SLAVE_TX_BUFFER_LEN 1024 /* head tracking motion module */
#define MAX_DYN_SLAVE_RX_BUFFER_LEN 1024 #define RAM_HEAD_TRACKING_MM_BASE_ADDRESS ((unsigned short int)0x0135)
#define MAX_DYN_SLAVE_REG_BUFFER_LEN 1024 #define EEPROM_HEAD_TRACKING_MM_BASE_ADDRESS ((unsigned short int)0x0031)
#define MAX_NUM_SLAVE_DEVICES 4
#define MAX_DYN_SLAVE_REG_BUFFER_LEN 1024 /* head tracking motion module */
#define DYN_MANAGER_MAX_NUM_MASTERS 4 #define RAM_WALK_MM_BASE_ADDRESS ((unsigned short int)0x0142)
#define DYN_MANAGER_MAX_NUM_MODULES 8 #define EEPROM_WALK_MM_BASE_ADDRESS ((unsigned short int)0x0043)
#define DYN_MANAGER_MAX_NUM_DEVICES 32
#define DYN_MANAGER_MAX_NUM_SINGLE_OP 16 /* IMU */
#define DYN_MANAGER_MAX_NUM_SYNC_OP 4 #define RAM_IMU_MM_BASE_ADDRESS ((unsigned short int)0x0146)
#define DYN_MANAGER_MAX_NUM_BULK_OP 4
#define MODULE_MAX_NUM_MODELS 32
#define MM_MAX_NUM_MOTION_MODULES 8
/* EEPROM default values */ /* EEPROM default values */
/* Dynamixel slave */ /* Dynamixel slave */
#define DARWIN_DEVICE_MODEL 0x7300 #define DARWIN_DEVICE_MODEL 0x7300
#define DARWIN_FIRMWARE_VERSION 0x0001 #define DARWIN_FIRMWARE_VERSION 0x0001
#define DARWIN_DEVICE_ID 0x0001 #define DARWIN_DEVICE_ID 0x0001
#define DEFAULT_BAUDRATE 0x0010 #define DEFAULT_BAUDRATE 0x0010
#define DEFAULT_RETURN_DELAY 0x0000 #define DEFAULT_RETURN_DELAY 0x0000
#define DEFAULT_RETURN_LEVEL 0x0002 #define DEFAULT_RETURN_LEVEL 0x0002
/* dynamixel manager */ /* dynamixel manager */
#define DYN_MANAGER_PERIOD 0x0007 #define DYN_MANAGER_PERIOD 0x0007
/* motion manager */ /* motion manager */
#define MMANAGER_PERIOD 0x0001 #define MMANAGER_PERIOD 0x0001
/* balance */ /* balance */
#define DEFAULT_BALANCE_KNEE_GAIN 0x4CCD // 0.3 in fixed point format 0|16 #define DEFAULT_BALANCE_KNEE_GAIN 0x4CCD // 0.3 in fixed point format 0|16
#define DEFAULT_BALANCE_ANKLE_ROLL_GAIN 0xFFFF // 0.99999 #define DEFAULT_BALANCE_ANKLE_ROLL_GAIN 0xFFFF // 0.99999
#define DEFAULT_BALANCE_ANKLE_PITCH_GAIN 0xE666 // 0.9 #define DEFAULT_BALANCE_ANKLE_PITCH_GAIN 0xE666 // 0.9
#define DEFAULT_BALANCE_HIP_ROLL_GAIN 0x8000 // 0.5 #define DEFAULT_BALANCE_HIP_ROLL_GAIN 0x8000 // 0.5
/* head tracking */
#define DEFAULT_HEAD_PAN_P_GAIN 0x028F // 0.01 in fixed point format 0|16
#define DEFAULT_HEAD_PAN_I_GAIN 0x0000 // 0.0 in fixed point format 0|16
#define DEFAULT_HEAD_PAN_D_GAIN 0x0000 // 0.0 in fixed point format 0|16
#define DEFAULT_HEAD_PAN_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7
#define DEFAULT_HEAD_TILT_P_GAIN 0x028F // 0.01 in fixed point format 0|16
#define DEFAULT_HEAD_TILT_I_GAIN 0x0000 // 0.0 in fixed point format 0|16
#define DEFAULT_HEAD_TILT_D_GAIN 0x0000 // 0.0 in fixed point format 0|16
#define DEFAULT_HEAD_TILT_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7
#define DEFAULT_HEAD_PAN_SERVO_ID 0x0013
#define DEFAULT_HEAD_TILT_SERVO_ID 0x0014
/* walk */
#define DEFAULT_WALK_X_OFFSET 0xFFF6 // -10 mm
#define DEFAULT_WALK_Y_OFFSET 0x0005 // 5 mm
#define DEFAULT_WALK_Z_OFFSET 0x0014 // 20 mm
#define DEFAULT_WALK_ROLL_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_WALK_PITCH_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_WALK_YAW_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_WALK_HIP_PITCH_OFFSET 0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10
#define DEFAULT_WALK_PERIOD_TIME 0x0258 // 600 ms
#define DEFAULT_WALK_DSP_RATIO 0x0019 // 0.1 in fixed point format 0 | 8
#define DEFAULT_WALK_STEP_FW_BW_RATIO 0x004C // 0.3 in fixed point format 0 | 8
#define DEFAULT_WALK_FOOT_HEIGHT 0x0028 // 40 mm
#define DEFAULT_WALK_SWING_RIGHT_LEFT 0x0014 // 20 mm
#define DEFAULT_WALK_SWING_TOP_DOWN 0x0005 // 5 mm
#define DEFAULT_WALK_PELVIS_OFFSET 0x0018 // 3 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_WALK_ARM_SWING_GAIN 0x0030 // 1.5 in fixed point format 3 | 5
#define DEFAULT_WALK_MAX_VEL 0x0016 // 20 mm/s
#define DEFAULT_WALK_MAX_ROT_VEL 0x0040 // 8 degrees/s in fixed point format 5 | 3
#define DEFAULT_WALK_R_HIP_YAW_SERVO_ID 0x0007
#define DEFAULT_WALK_R_HIP_ROLL_SERVO_ID 0x0009
#define DEFAULT_WALK_R_HIP_PITCH_SERVO_ID 0x000B
#define DEFAULT_WALK_R_KNEE_SERVO_ID 0x000D
#define DEFAULT_WALK_R_ANKLE_PITCH_SERVO_ID 0x000F
#define DEFAULT_WALK_R_ANKLE_ROLL_SERVO_ID 0x0011
#define DEFAULT_WALK_R_SHOULDER_PITCH_SERVO_ID 0x0001
#define DEFAULT_WALK_L_HIP_YAW_SERVO_ID 0x0008
#define DEFAULT_WALK_L_HIP_ROLL_SERVO_ID 0x000A
#define DEFAULT_WALK_L_HIP_PITCH_SERVO_ID 0x000C
#define DEFAULT_WALK_L_KNEE_SERVO_ID 0x000E
#define DEFAULT_WALK_L_ANKLE_PITCH_SERVO_ID 0x0010
#define DEFAULT_WALK_L_ANKLE_ROLL_SERVO_ID 0x0012
#define DEFAULT_WALK_L_SHOULDER_PITCH_SERVO_ID 0x0002
#endif #endif
...@@ -10,6 +10,8 @@ SET(sources ${CMAKE_CURRENT_SOURCE_DIR}/cm730_fw.c ...@@ -10,6 +10,8 @@ SET(sources ${CMAKE_CURRENT_SOURCE_DIR}/cm730_fw.c
${CMAKE_CURRENT_SOURCE_DIR}/stm32f1xx_hal_msp.c ${CMAKE_CURRENT_SOURCE_DIR}/stm32f1xx_hal_msp.c
${CMAKE_CURRENT_SOURCE_DIR}/darwin_balance.c ${CMAKE_CURRENT_SOURCE_DIR}/darwin_balance.c
${CMAKE_CURRENT_SOURCE_DIR}/darwin_motion.c ${CMAKE_CURRENT_SOURCE_DIR}/darwin_motion.c
${CMAKE_CURRENT_SOURCE_DIR}/darwin_kinematics.c
${CMAKE_CURRENT_SOURCE_DIR}/darwin_math.c
${CMAKE_CURRENT_SOURCE_DIR}/darwin_motion_pages.c ${CMAKE_CURRENT_SOURCE_DIR}/darwin_motion_pages.c
#${CMAKE_CURRENT_SOURCE_DIR}/smart_charger.c #${CMAKE_CURRENT_SOURCE_DIR}/smart_charger.c
${PROJECT_SOURCE_DIR}/stm32_libraries/f1/usart/src/usart3.c ${PROJECT_SOURCE_DIR}/stm32_libraries/f1/usart/src/usart3.c
......
...@@ -3,25 +3,59 @@ ...@@ -3,25 +3,59 @@
#include "darwin_dyn_master.h" #include "darwin_dyn_master.h"
#include "motion_manager.h" #include "motion_manager.h"
#include "action.h" #include "action.h"
#include "joint_motion.h"
#include "head_tracking.h"
#include "walk.h"
#include "darwin_kinematics.h"
#include "eeprom.h" #include "eeprom.h"
TDynManager darwin_dyn_manager; TDynManager darwin_dyn_manager;
TDynamixelMaster *darwin_master; TDynamixelMaster *darwin_master;
TMotionManager darwin_mmanager; TMotionManager darwin_mmanager;
TActionMModule action_mm; TActionMModule action_mm;
TJointMModule joints_mm;
THeadMModule head_mm;
TWalkMModule walk_mm;
dyn_manager_eeprom_data(darwin_dyn_manager,".eeprom",EEPROM_DYN_MANAGER_BASE_ADDRESS,DYN_MANAGER_PERIOD); dyn_manager_eeprom_data(darwin_dyn_manager,".eeprom",EEPROM_DYN_MANAGER_BASE_ADDRESS,DYN_MANAGER_PERIOD);
dyn_mm_eeprom_data(darwin_mmanager,".eeprom",EEPROM_MMANAGER_BASE_ADDRESS,MMANAGER_PERIOD,0,EEPROM_MMANAGER_BASE_ADDRESS+1,0,EEPROM_MMANAGER_BASE_ADDRESS+2,0,EEPROM_MMANAGER_BASE_ADDRESS+3,0,EEPROM_MMANAGER_BASE_ADDRESS+4,0,EEPROM_MMANAGER_BASE_ADDRESS+5,0,EEPROM_MMANAGER_BASE_ADDRESS+6,0,EEPROM_MMANAGER_BASE_ADDRESS+7,0,EEPROM_MMANAGER_BASE_ADDRESS+8,0,EEPROM_MMANAGER_BASE_ADDRESS+9,0,EEPROM_MMANAGER_BASE_ADDRESS+10,0,EEPROM_MMANAGER_BASE_ADDRESS+11,0,EEPROM_MMANAGER_BASE_ADDRESS+12,0,EEPROM_MMANAGER_BASE_ADDRESS+13,0,EEPROM_MMANAGER_BASE_ADDRESS+14,0,EEPROM_MMANAGER_BASE_ADDRESS+15,0,EEPROM_MMANAGER_BASE_ADDRESS+16,0,EEPROM_MMANAGER_BASE_ADDRESS+17,0,EEPROM_MMANAGER_BASE_ADDRESS+18,0,EEPROM_MMANAGER_BASE_ADDRESS+19,0,EEPROM_MMANAGER_BASE_ADDRESS+20,0,EEPROM_MMANAGER_BASE_ADDRESS+21,0,EEPROM_MMANAGER_BASE_ADDRESS+22,0,EEPROM_MMANAGER_BASE_ADDRESS+23,0,EEPROM_MMANAGER_BASE_ADDRESS+24,0,EEPROM_MMANAGER_BASE_ADDRESS+25,0,EEPROM_MMANAGER_BASE_ADDRESS+26,0,EEPROM_MMANAGER_BASE_ADDRESS+27,0,EEPROM_MMANAGER_BASE_ADDRESS+28,0,EEPROM_MMANAGER_BASE_ADDRESS+29,0,EEPROM_MMANAGER_BASE_ADDRESS+30,0,EEPROM_MMANAGER_BASE_ADDRESS+31,0,EEPROM_MMANAGER_BASE_ADDRESS+32); dyn_mm_eeprom_data(darwin_mmanager,".eeprom",EEPROM_MMANAGER_BASE_ADDRESS,MMANAGER_PERIOD,0,EEPROM_MMANAGER_BASE_ADDRESS+1,0,EEPROM_MMANAGER_BASE_ADDRESS+2,0,EEPROM_MMANAGER_BASE_ADDRESS+3,0,EEPROM_MMANAGER_BASE_ADDRESS+4,0,EEPROM_MMANAGER_BASE_ADDRESS+5,0,EEPROM_MMANAGER_BASE_ADDRESS+6,0,EEPROM_MMANAGER_BASE_ADDRESS+7,0,EEPROM_MMANAGER_BASE_ADDRESS+8,0,EEPROM_MMANAGER_BASE_ADDRESS+9,0,EEPROM_MMANAGER_BASE_ADDRESS+10,0,EEPROM_MMANAGER_BASE_ADDRESS+11,0,EEPROM_MMANAGER_BASE_ADDRESS+12,0,EEPROM_MMANAGER_BASE_ADDRESS+13,0,EEPROM_MMANAGER_BASE_ADDRESS+14,0,EEPROM_MMANAGER_BASE_ADDRESS+15,0,EEPROM_MMANAGER_BASE_ADDRESS+16,0,EEPROM_MMANAGER_BASE_ADDRESS+17,0,EEPROM_MMANAGER_BASE_ADDRESS+18,0,EEPROM_MMANAGER_BASE_ADDRESS+19,0,EEPROM_MMANAGER_BASE_ADDRESS+20,0,EEPROM_MMANAGER_BASE_ADDRESS+21,0,EEPROM_MMANAGER_BASE_ADDRESS+22,0,EEPROM_MMANAGER_BASE_ADDRESS+23,0,EEPROM_MMANAGER_BASE_ADDRESS+24,0,EEPROM_MMANAGER_BASE_ADDRESS+25,0,EEPROM_MMANAGER_BASE_ADDRESS+26,0,EEPROM_MMANAGER_BASE_ADDRESS+27,0,EEPROM_MMANAGER_BASE_ADDRESS+28,0,EEPROM_MMANAGER_BASE_ADDRESS+29,0,EEPROM_MMANAGER_BASE_ADDRESS+30,0,EEPROM_MMANAGER_BASE_ADDRESS+31,0,EEPROM_MMANAGER_BASE_ADDRESS+32);
head_tracking_eeprom_data(darwin_head_tracking,".eeprom",EEPROM_HEAD_TRACKING_MM_BASE_ADDRESS,DEFAULT_HEAD_PAN_P_GAIN,DEFAULT_HEAD_PAN_I_GAIN,DEFAULT_HEAD_PAN_D_GAIN,DEFAULT_HEAD_PAN_I_CLAMP,DEFAULT_HEAD_TILT_P_GAIN,DEFAULT_HEAD_TILT_I_GAIN,DEFAULT_HEAD_TILT_D_GAIN,DEFAULT_HEAD_TILT_I_CLAMP,DEFAULT_HEAD_PAN_SERVO_ID,DEFAULT_HEAD_TILT_SERVO_ID);
walk_eeprom_data(darwin_walk,".eeprom",EEPROM_WALK_MM_BASE_ADDRESS,DEFAULT_WALK_X_OFFSET,DEFAULT_WALK_Y_OFFSET,DEFAULT_WALK_Z_OFFSET,DEFAULT_WALK_ROLL_OFFSET,DEFAULT_WALK_PITCH_OFFSET,DEFAULT_WALK_YAW_OFFSET,DEFAULT_WALK_HIP_PITCH_OFFSET,DEFAULT_WALK_PERIOD_TIME,DEFAULT_WALK_DSP_RATIO,DEFAULT_WALK_STEP_FW_BW_RATIO,DEFAULT_WALK_FOOT_HEIGHT,DEFAULT_WALK_SWING_RIGHT_LEFT,DEFAULT_WALK_SWING_TOP_DOWN,DEFAULT_WALK_PELVIS_OFFSET,DEFAULT_WALK_ARM_SWING_GAIN,DEFAULT_WALK_MAX_VEL,DEFAULT_WALK_MAX_ROT_VEL,DEFAULT_WALK_R_HIP_YAW_SERVO_ID,DEFAULT_WALK_R_HIP_ROLL_SERVO_ID,DEFAULT_WALK_R_HIP_PITCH_SERVO_ID,DEFAULT_WALK_R_KNEE_SERVO_ID,DEFAULT_WALK_R_ANKLE_PITCH_SERVO_ID,DEFAULT_WALK_R_ANKLE_ROLL_SERVO_ID,DEFAULT_WALK_R_SHOULDER_PITCH_SERVO_ID,DEFAULT_WALK_L_HIP_YAW_SERVO_ID,DEFAULT_WALK_L_HIP_ROLL_SERVO_ID,DEFAULT_WALK_L_HIP_PITCH_SERVO_ID,DEFAULT_WALK_L_KNEE_SERVO_ID,DEFAULT_WALK_L_ANKLE_PITCH_SERVO_ID,DEFAULT_WALK_L_ANKLE_ROLL_SERVO_ID,DEFAULT_WALK_L_SHOULDER_PITCH_SERVO_ID);
unsigned char darwin_mm_init(TScheduler *scheduler,TMemory *memory) unsigned char darwin_mm_init(TScheduler *scheduler,TMemory *memory)
{ {
// initialize dynamixel manager
dyn_manager_init(&darwin_dyn_manager,memory,scheduler,SCHED_CH3,SCHED_CH2,RAM_DYN_MANAGER_BASE_ADDRESS,EEPROM_DYN_MANAGER_BASE_ADDRESS); dyn_manager_init(&darwin_dyn_manager,memory,scheduler,SCHED_CH3,SCHED_CH2,RAM_DYN_MANAGER_BASE_ADDRESS,EEPROM_DYN_MANAGER_BASE_ADDRESS);
// initialize dynamixel master
darwin_master=darwin_dyn_master_init(); darwin_master=darwin_dyn_master_init();
// add master to manager
dyn_manager_add_master(&darwin_dyn_manager,darwin_master); dyn_manager_add_master(&darwin_dyn_manager,darwin_master);
// initialize motion manager
mmanager_init(&darwin_mmanager,memory,RAM_MMANAGER_BASE_ADDRESS,EEPROM_MMANAGER_BASE_ADDRESS); mmanager_init(&darwin_mmanager,memory,RAM_MMANAGER_BASE_ADDRESS,EEPROM_MMANAGER_BASE_ADDRESS);
// initialize action motion module
action_init(&action_mm,memory,RAM_ACTION_MM_BASE_ADDRESS); action_init(&action_mm,memory,RAM_ACTION_MM_BASE_ADDRESS);
// add action module to motion manager
mmanager_add_module(&darwin_mmanager,action_get_module(&action_mm)); mmanager_add_module(&darwin_mmanager,action_get_module(&action_mm));
// initialize joint motion module
joint_motion_init(&joints_mm,memory,RAM_JOINT_MOTION_MM_BASE_ADDRESS);
// add joint motion module to motion manager
mmanager_add_module(&darwin_mmanager,joint_motion_get_module(&joints_mm));
// initialize head tracking motion module
head_tracking_init(&head_mm,memory,RAM_HEAD_TRACKING_MM_BASE_ADDRESS,EEPROM_HEAD_TRACKING_MM_BASE_ADDRESS);
// add head tracking module to motion manager
mmanager_add_module(&darwin_mmanager,head_tracking_get_module(&head_mm));
EE_update_num_variables(EEPROM_HEAD_TRACKING_LENGTH);
// initialize walking motion module
walk_init(&walk_mm,memory,RAM_WALK_MM_BASE_ADDRESS,EEPROM_WALK_MM_BASE_ADDRESS);
walk_mm.leg_ik_function=darwin_leg_ik;
// add walk module to motion manager
mmanager_add_module(&darwin_mmanager,walk_get_module(&walk_mm));
EE_update_num_variables(EEPROM_WALK_LENGTH);
// add motion manager to dynamixel manager
dyn_manager_add_module(&darwin_dyn_manager,mmanager_get_dyn_module(&darwin_mmanager)); dyn_manager_add_module(&darwin_dyn_manager,mmanager_get_dyn_module(&darwin_mmanager));
EE_update_num_variables(EEPROM_DYN_MANAGER_LENGTH+EEPROM_DYN_MODULE_LENGTH+EEPROM_MM_LENGTH); EE_update_num_variables(EEPROM_DYN_MANAGER_LENGTH+EEPROM_DYN_MODULE_LENGTH+EEPROM_MM_LENGTH);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment