Added the dynamixel master module for the servos.

Makefile

@@ -14,6 +14,7 @@ TARGET_FILES+=src/zigbee.c
 TARGET_FILES+=src/bioloid_time.c
 TARGET_FILES+=src/bioloid_dyn_slave.c
 TARGET_FILES+=src/bioloid_dyn_master_sensors.c
+TARGET_FILES+=src/bioloid_dyn_master_servos.c
 TARGET_PROCESSOR=STM32F407VG
 
 HAL_PATH=../../STM32_processor/hal/f4
@@ -77,6 +78,7 @@ TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal_adc_ex.c
 
 TARGET_FILES+=$(USART_PATH)/src/usart3.c
 TARGET_FILES+=$(USART_PATH)/src/usart2.c
+TARGET_FILES+=$(USART_PATH)/src/usart1.c
 
 BIOLOID_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
 BIOLOID_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(BIOLOID_OBJS_TMP))

include/bioloid_dyn_master_servos.h

+#ifndef _BIOLOID_DYN_MASTER_SERVOS_H
+#define _BIOLOID_DYN_MASTER_SERVOS_H
+
+#include "stm32f4xx_hal.h"
+#include "stm32_time.h"
+#include "dynamixel_master.h"
+#include "comm.h"
+
+extern TDynamixelMaster bioloid_dyn_master_servos;
+
+void bioloid_dyn_master_servos_init(void);
+inline void bioloid_dyn_master_servos_enable_power(void);
+inline void bioloid_dyn_master_servos_disable_power(void);
+
+#endif
+

src/bioloid_dyn_master_servos.c

+#include "bioloid_dyn_master_servos.h"
+#include "bioloid_time.h"
+#include "usart1.h"
+#include "gpio.h"
+
+#define ENABLE_RX_EN_GPIO_CLK      __GPIOA_CLK_ENABLE()
+#define RX_EN_PIN                  GPIO_PIN_8
+#define RX_EN_GPIO_PORT            GPIOA                       
+
+#define ENABLE_TX_EN_GPIO_CLK      __GPIOC_CLK_ENABLE()
+#define TX_EN_PIN                  GPIO_PIN_9               
+#define TX_EN_GPIO_PORT            GPIOC               
+
+#define POWER_GPIO_CLK             __GPIOB_CLK_ENABLE()
+#define POWER_PIN                  GPIO_PIN_12
+#define POWER_GPIO_PORT            GPIOB                       
+
+/* private variables */
+TDynamixelMaster bioloid_dyn_master_servos;
+TTime bioloid_dyn_master_servos_timer;
+TComm bioloid_dyn_master_servos_comm;
+
+// private functions
+void bioloid_dyn_master_servos_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 bioloid_dyn_master_servos_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 bioloid_dyn_master_servos_init(void)
+{
+  GPIO_InitTypeDef GPIO_InitStructure;
+  TUSART_IRQ_Priorities priorities;
+  UART_InitTypeDef Init;
+
+  // initialize timer structure
+  time_init(&bioloid_dyn_master_servos_timer,bioloid_time_get_counts_per_us(),bioloid_time_get_counts);
+
+  // initialize GPIO
+  ENABLE_RX_EN_GPIO_CLK;
+  ENABLE_TX_EN_GPIO_CLK;
+
+  GPIO_InitStructure.Pin = RX_EN_PIN;
+  GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
+  GPIO_InitStructure.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(RX_EN_GPIO_PORT, &GPIO_InitStructure);
+
+  GPIO_InitStructure.Pin = TX_EN_PIN;
+  HAL_GPIO_Init(TX_EN_GPIO_PORT, &GPIO_InitStructure);
+
+  bioloid_dyn_master_servos_set_rx_mode();
+
+  GPIO_InitStructure.Pin = POWER_PIN;
+  GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
+  HAL_GPIO_Init(POWER_GPIO_PORT, &GPIO_InitStructure);
+
+  bioloid_dyn_master_servos_disable_power();
+
+  /* initialize the comm object */
+  comm_init(&bioloid_dyn_master_servos_comm,0x01,&bioloid_dyn_master_servos_timer);
+  Init.BaudRate     = 1000000;
+  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;
+  
+  priorities.irq_priority=0;
+  priorities.irq_subpriority=1;
+  priorities.dma_rx_priority=1;
+  priorities.dma_rx_subpriority=3;
+  priorities.dma_tx_priority=1;
+  priorities.dma_tx_subpriority=2;
+
+  usart1_init(&bioloid_dyn_master_servos_comm,&Init,&priorities);
+  dyn_master_init(&bioloid_dyn_master_servos,&bioloid_dyn_master_servos_comm);
+  bioloid_dyn_master_servos.set_rx_mode=bioloid_dyn_master_servos_set_rx_mode;
+  bioloid_dyn_master_servos.set_tx_mode=bioloid_dyn_master_servos_set_tx_mode;
+
+  bioloid_dyn_master_servos_enable_power();
+  HAL_Delay(2000);
+  if(dyn_master_ping(&bioloid_dyn_master_servos,0x04)==DYN_SUCCESS)
+    gpio_set_led(RXD_LED);
+  else
+    gpio_clear_led(RXD_LED);
+  bioloid_dyn_master_servos_disable_power();
+
+}
+
+inline void bioloid_dyn_master_servos_enable_power(void)
+{
+  HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET);
+}
+
+inline void bioloid_dyn_master_servos_disable_power(void)
+{
+  HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET);
+}
+

src/bioloid_stm32.c

@@ -7,6 +7,7 @@
 #include "bioloid_time.h"
 #include "bioloid_dyn_slave.h"
 #include "bioloid_dyn_master_sensors.h"
+#include "bioloid_dyn_master_servos.h"
 
 int32_t main(void)
 {
@@ -28,6 +29,8 @@ int32_t main(void)
   bioloid_dyn_slave_start();
   /* initialize the dynamixel master module for the sensors */
   bioloid_dyn_master_sensors_init();
+  /* initialize the dynamixel master module for the servos */
+  bioloid_dyn_master_servos_init();
 
   while(1);
 }