diff --git a/include/darwin_conf.h b/include/darwin_conf.h
index 548407c5d6a6fe6bbb1adcc94d271dc2e2f57a47..c922b66055fe760373dc581ad210cbd9e109c172 100644
--- a/include/darwin_conf.h
+++ b/include/darwin_conf.h
@@ -27,7 +27,7 @@
#define EEPROM_BALANCE_BASE_ADDRESS ((unsigned short int)0x0007)
#define RAM_BALANCE_BASE_ADDRESS ((unsigned short int)0x00AF)
-#define NUM_MOTION_PAGES 256
+#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
@@ -46,9 +46,9 @@
/* EEPROM default values */
/* Dynamixel slave */
-#define DARWIN_DEVICE_MODEL 0x7300
-#define DARWIN_FIRMWARE_VERSION 0x0001
-#define DARWIN_DEVICE_ID 0x0001
+#define DARWIN_DEVICE_MODEL 0x7300
+#define DARWIN_FIRMWARE_VERSION 0x0001
+#define DARWIN_DEVICE_ID 0x0001
#define DEFAULT_BAUDRATE 0x0010
#define DEFAULT_RETURN_DELAY 0x0000
diff --git a/include/darwin_dyn_master.h b/include/darwin_dyn_master.h
index cca8fc192ff897c7d4d33b188fc57d7a432238ad..2fe3a091e389607393849b133065d870929fdc8b 100644
--- a/include/darwin_dyn_master.h
+++ b/include/darwin_dyn_master.h
@@ -11,7 +11,7 @@ extern "C" {
extern TDynamixelMaster darwin_dyn_master;
-void darwin_dyn_master_init(void);
+TDynamixelMaster *darwin_dyn_master_init(void);
void darwin_dyn_master_enable_power(void);
void darwin_dyn_master_disable_power(void);
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index a82710058781506979167a31e2440ac5b4bb2c99..cfb80e21ce0376d79f537dd3d41f73fb2c33ccc0 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -6,20 +6,12 @@ SET(sources ${CMAKE_CURRENT_SOURCE_DIR}/cm730_fw.c
#${CMAKE_CURRENT_SOURCE_DIR}/adc_dma.c
${CMAKE_CURRENT_SOURCE_DIR}/darwin_imu.c
${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_slave.c
- #${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_master.c
- #${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_master_v2.c
+ ${CMAKE_CURRENT_SOURCE_DIR}/darwin_dyn_master.c
${CMAKE_CURRENT_SOURCE_DIR}/stm32f1xx_hal_msp.c
- #${CMAKE_CURRENT_SOURCE_DIR}/motion_manager.c
- #${CMAKE_CURRENT_SOURCE_DIR}/action.c
- #${CMAKE_CURRENT_SOURCE_DIR}/motion_pages.c
- #${CMAKE_CURRENT_SOURCE_DIR}/walking.c
- #${CMAKE_CURRENT_SOURCE_DIR}/darwin_math.c
- #${CMAKE_CURRENT_SOURCE_DIR}/darwin_kinematics.c
- #${CMAKE_CURRENT_SOURCE_DIR}/joint_motion.c
- #${CMAKE_CURRENT_SOURCE_DIR}/head_tracking.c
- #${CMAKE_CURRENT_SOURCE_DIR}/grippers.c
+ ${CMAKE_CURRENT_SOURCE_DIR}/darwin_balance.c
+ ${CMAKE_CURRENT_SOURCE_DIR}/darwin_motion.c
+ ${CMAKE_CURRENT_SOURCE_DIR}/darwin_motion_pages.c
#${CMAKE_CURRENT_SOURCE_DIR}/smart_charger.c
- #${CMAKE_CURRENT_SOURCE_DIR}/stairs.c
${PROJECT_SOURCE_DIR}/stm32_libraries/f1/usart/src/usart3.c
${PROJECT_SOURCE_DIR}/stm32_libraries/f1/usart/src/usart1.c PARENT_SCOPE)
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index 1f4527b531da504ae15f53879e4407bc0ce63f52..b1c17b782fd46a3f4cdc91c66b87964a791e3dcf 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -6,15 +6,12 @@
#include "darwin_time.h"
#include "eeprom.h"
#include "darwin_imu.h"
-//#include "gpio.h"
-//#include "adc_dma.h"
-//#include "darwin_dyn_master.h"
-//#include "darwin_dyn_master_v2.h"
-//#include "motion_manager.h"
-//#include "action.h"
-//#include "action_id.h"
+#include "darwin_motion.h"
+#include "darwin_imu.h"
+#include "darwin_balance.h"
TMemory *darwin_memory;
+TDynamixelMaster *dyn_master;
TScheduler *scheduler;
int main(void)
@@ -37,7 +34,8 @@ int main(void)
darwin_dyn_slave_init(&darwin_memory,scheduler);
// initialize imu
imu_init(darwin_memory,RAM_IMU_MM_BASE_ADDRESS);
- /* initialize motion manager module */
+ // initialize motion manager
+ darwin_mm_init(scheduler,darwin_memory);
/* initialize the ram module */
ram_init(darwin_memory);
diff --git a/src/darwin_dyn_master.c b/src/darwin_dyn_master.c
index 9762cf6fee1474dc5059537fdd592db6530a0320..b09259372d45bcedd9f6a16d588668313dc86684 100755
--- a/src/darwin_dyn_master.c
+++ b/src/darwin_dyn_master.c
@@ -1,7 +1,6 @@
#include "darwin_dyn_master.h"
#include "darwin_time.h"
#include "usart1.h"
-#include "ram.h"
#define ENABLE_RX_EN_GPIO_CLK __GPIOB_CLK_ENABLE()
#define RX_EN_PIN GPIO_PIN_5
@@ -34,11 +33,11 @@ void darwin_dyn_master_set_tx_mode(void)
}
// public functions
-void darwin_dyn_master_init(void)
+TDynamixelMaster *darwin_dyn_master_init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
TUSART_IRQ_Priorities priorities;
- UART_InitTypeDef Init;
+ USART_InitTypeDef Init;
// initialize timer structure
time_init(&darwin_dyn_master_timer,darwin_time_get_counts_per_us(),darwin_time_get_counts);
@@ -68,12 +67,13 @@ void darwin_dyn_master_init(void)
/* initialize the comm object */
comm_init(&darwin_dyn_master_comm,0x01,&darwin_dyn_master_timer);
Init.BaudRate = 1000000; //57600;
- Init.WordLength = UART_WORDLENGTH_8B;
- Init.StopBits = UART_STOPBITS_1;
- Init.Parity = UART_PARITY_NONE;
- Init.Mode = UART_MODE_TX_RX;
- Init.HwFlowCtl = UART_HWCONTROL_NONE;
- Init.OverSampling = UART_OVERSAMPLING_16;
+ Init.WordLength = USART_WORDLENGTH_8B;
+ Init.StopBits = USART_STOPBITS_1;
+ Init.Parity = USART_PARITY_NONE;
+ Init.Mode = USART_MODE_TX_RX;
+ Init.CLKPolarity = USART_POLARITY_LOW;
+ Init.CLKPhase = USART_PHASE_1EDGE;
+ Init.CLKLastBit = USART_LASTBIT_DISABLE;
priorities.irq_priority=0;
priorities.irq_subpriority=1;
@@ -89,18 +89,20 @@ void darwin_dyn_master_init(void)
/* configure dynamixel master module */
dyn_master_set_rx_timeout(&darwin_dyn_master,50);
- dyn_master_set_return_level(&darwin_dyn_master,RETURN_ALL);
+ dyn_master_set_return_level(&darwin_dyn_master,return_all);
+
+ return &darwin_dyn_master;
}
void darwin_dyn_master_enable_power(void)
{
HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET);
- ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR;
+// ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR;
}
void darwin_dyn_master_disable_power(void)
{
HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET);
- ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR);
+// ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR);
}