Added the second dynamixel master.

include/darwin_dyn_master.h

@@ -9,8 +9,6 @@ extern "C" {
 #include "comm.h"
 #include "dynamixel_master.h"
 
-extern TDynamixelMaster darwin_dyn_master;
-
 TDynamixelMaster *darwin_dyn_master_init(void);
 void darwin_dyn_master_enable_power(void);
 void darwin_dyn_master_disable_power(void);

include/darwin_dyn_master_v2.h → include/darwin_dyn_master2.h

-#ifndef _DARWIN_DYN_MASTER_V2_H
-#define _DARWIN_DYN_MASTER_V2_H
+#ifndef _DARWIN_DYN_MASTER2_H
+#define _DARWIN_DYN_MASTER2_H
 
 #ifdef __cplusplus
 extern "C" {
@@ -9,11 +9,9 @@ extern "C" {
 #include "comm.h"
 #include "dynamixel_master.h"
 
-extern TDynamixelMaster darwin_dyn_master_v2;
-
-void darwin_dyn_master_v2_init(void);
-void darwin_dyn_master_v2_enable_power(void);
-void darwin_dyn_master_v2_disable_power(void);
+TDynamixelMaster *darwin_dyn_master2_init(void);
+void darwin_dyn_master2_enable_power(void);
+void darwin_dyn_master2_disable_power(void);
 
 #ifdef __cplusplus
 }

src/CMakeLists.txt

@@ -7,6 +7,7 @@ SET(sources ${CMAKE_CURRENT_SOURCE_DIR}/cm730_fw.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_master2.c
             ${CMAKE_CURRENT_SOURCE_DIR}/stm32f1xx_hal_msp.c
             ${CMAKE_CURRENT_SOURCE_DIR}/darwin_balance.c
             ${CMAKE_CURRENT_SOURCE_DIR}/darwin_motion.c
@@ -15,6 +16,7 @@ SET(sources ${CMAKE_CURRENT_SOURCE_DIR}/cm730_fw.c
             ${CMAKE_CURRENT_SOURCE_DIR}/darwin_motion_pages.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/usart2.c
             ${PROJECT_SOURCE_DIR}/stm32_libraries/f1/usart/src/usart1.c PARENT_SCOPE)
 
 set(header_include_dir ${CMAKE_CURRENT_SOURCE_DIR}/../include ${PROJECT_SOURCE_DIR}/stm32_libraries/f1/usart/include PARENT_SCOPE)

src/darwin_dyn_master_v2.c → src/darwin_dyn_master2.c

-#include "darwin_dyn_master_v2.h"
+#include "darwin_dyn_master2.h"
 #include "darwin_time.h"
 #include "usart2.h"
 #include "ram.h"
@@ -16,32 +16,32 @@
 #define POWER_GPIO_PORT            GPIOC                       
 
 /* private variables */
-TDynamixelMaster darwin_dyn_master_v2;
-TTime darwin_dyn_master_v2_timer;
-TComm darwin_dyn_master_v2_comm;
+TDynamixelMaster darwin_dyn_master2;
+TTime darwin_dyn_master2_timer;
+TComm darwin_dyn_master2_comm;
 
 // private functions
-void darwin_dyn_master_v2_set_rx_mode(void)
+void darwin_dyn_master2_set_rx_mode(void)
 {
   HAL_GPIO_WritePin(TX_EN_GPIO_PORT,TX_EN_PIN,GPIO_PIN_RESET);
   HAL_GPIO_WritePin(RX_EN_GPIO_PORT,RX_EN_PIN,GPIO_PIN_SET);
 }
 
-void darwin_dyn_master_v2_set_tx_mode(void)
+void darwin_dyn_master2_set_tx_mode(void)
 {
   HAL_GPIO_WritePin(RX_EN_GPIO_PORT,RX_EN_PIN,GPIO_PIN_RESET);
   HAL_GPIO_WritePin(TX_EN_GPIO_PORT,TX_EN_PIN,GPIO_PIN_SET);
 }
 
 // public functions
-void darwin_dyn_master_v2_init(void)
+TDynamixelMaster *darwin_dyn_master2_init(void)
 {
   GPIO_InitTypeDef GPIO_InitStructure;
   TUSART_IRQ_Priorities priorities;
-  UART_InitTypeDef Init;
+  USART_InitTypeDef Init;
 
   // initialize timer structure
-  time_init(&darwin_dyn_master_v2_timer,darwin_time_get_counts_per_us(),darwin_time_get_counts);
+  time_init(&darwin_dyn_master2_timer,darwin_time_get_counts_per_us(),darwin_time_get_counts);
 
   // initialize GPIO
   ENABLE_RX_EN_GPIO_CLK;
@@ -56,17 +56,18 @@ void darwin_dyn_master_v2_init(void)
   GPIO_InitStructure.Pin = TX_EN_PIN;
   HAL_GPIO_Init(TX_EN_GPIO_PORT, &GPIO_InitStructure);
 
-  darwin_dyn_master_v2_set_rx_mode();
+  darwin_dyn_master2_set_rx_mode();
 
   /* initialize the comm object */
-  comm_init(&darwin_dyn_master_v2_comm,0x01,&darwin_dyn_master_v2_timer);
+  comm_init(&darwin_dyn_master2_comm,0x01,&darwin_dyn_master2_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=2;
@@ -75,26 +76,27 @@ void darwin_dyn_master_v2_init(void)
   priorities.dma_tx_priority=1;
   priorities.dma_tx_subpriority=0;
 
-  usart2_init(&darwin_dyn_master_v2_comm,&Init,&priorities);
-  dyn_master_init(&darwin_dyn_master_v2,&darwin_dyn_master_v2_comm,DYN_VER2);
-  darwin_dyn_master_v2.set_rx_mode=darwin_dyn_master_v2_set_rx_mode;
-  darwin_dyn_master_v2.set_tx_mode=darwin_dyn_master_v2_set_tx_mode;
+  usart2_init(&darwin_dyn_master2_comm,&Init,&priorities);
+  dyn_master_init(&darwin_dyn_master2,&darwin_dyn_master2_comm,DYN_VER2);
+  darwin_dyn_master2.set_rx_mode=darwin_dyn_master2_set_rx_mode;
+  darwin_dyn_master2.set_tx_mode=darwin_dyn_master2_set_tx_mode;
 
   /* configure dynamixel master module */
-  dyn_master_set_rx_timeout(&darwin_dyn_master_v2,50);
-  dyn_master_set_return_level(&darwin_dyn_master_v2,RETURN_ALL);
-  darwin_dyn_master_v2_disable_power();
+  dyn_master_set_rx_timeout(&darwin_dyn_master2,50);
+  dyn_master_set_return_level(&darwin_dyn_master2,return_all);
+
+  return &darwin_dyn_master2;
 }
 
-void darwin_dyn_master_v2_enable_power(void)
+void darwin_dyn_master2_enable_power(void)
 {
   HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET);
-  ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR_V2;
+//  ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR2;
 }
 
-void darwin_dyn_master_v2_disable_power(void)
+void darwin_dyn_master2_disable_power(void)
 {
   HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET);
-  ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR_V2);
+//  ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR2);
 }
 

src/darwin_motion.c

 #include "darwin_motion.h"
 #include "darwin_balance.h"
 #include "darwin_dyn_master.h"
+#include "darwin_dyn_master2.h"
 #include "motion_manager.h"
 #include "action.h"
 #include "joint_motion.h"
@@ -11,6 +12,7 @@
 
 TDynManager darwin_dyn_manager;
 TDynamixelMaster *darwin_master;
+TDynamixelMaster *darwin_master2;
 TMotionManager darwin_mmanager;
 TActionMModule action_mm;
 TJointMModule joints_mm;
@@ -31,8 +33,10 @@ unsigned char darwin_mm_init(TScheduler *scheduler,TMemory *memory)
   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_master2=darwin_dyn_master2_init();
   // add master to manager
   dyn_manager_add_master(&darwin_dyn_manager,darwin_master);
+  dyn_manager_add_master(&darwin_dyn_manager,darwin_master2);
 
   // initialize motion manager
   mmanager_init(&darwin_mmanager,memory,RAM_MMANAGER_BASE_ADDRESS,EEPROM_MMANAGER_BASE_ADDRESS);