Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
D
darwin_stm32_fw
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
humanoides
darwin
darwin_stm32_fw
Commits
a9b471a1
Commit
a9b471a1
authored
5 years ago
by
Sergi Hernandez
Browse files
Options
Downloads
Patches
Plain Diff
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
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
CMakeLists.txt
+13
-1
13 additions, 1 deletion
CMakeLists.txt
include/darwin_conf.h
+102
-44
102 additions, 44 deletions
include/darwin_conf.h
src/CMakeLists.txt
+2
-0
2 additions, 0 deletions
src/CMakeLists.txt
src/darwin_motion.c
+34
-0
34 additions, 0 deletions
src/darwin_motion.c
with
151 additions
and
45 deletions
CMakeLists.txt
+
13
−
1
View file @
a9b471a1
...
@@ -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
...
...
This diff is collapsed.
Click to expand it.
include/darwin_conf.h
+
102
−
44
View file @
a9b471a1
#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
This diff is collapsed.
Click to expand it.
src/CMakeLists.txt
+
2
−
0
View file @
a9b471a1
...
@@ -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
...
...
This diff is collapsed.
Click to expand it.
src/darwin_motion.c
+
34
−
0
View file @
a9b471a1
...
@@ -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
);
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment