From 35ff55ecfda15b69ea48a1a306df95b148a80491 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Sun, 16 Aug 2015 20:42:42 +0000
Subject: [PATCH] Added the dynamixel master module for the servos.
---
Makefile | 2 +
include/bioloid_dyn_master_servos.h | 16 ++++
src/bioloid_dyn_master_servos.c | 109 ++++++++++++++++++++++++++++
src/bioloid_stm32.c | 3 +
4 files changed, 130 insertions(+)
create mode 100644 include/bioloid_dyn_master_servos.h
create mode 100755 src/bioloid_dyn_master_servos.c
diff --git a/Makefile b/Makefile
index 6e78610..3850d75 100755
--- a/Makefile
+++ b/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))
diff --git a/include/bioloid_dyn_master_servos.h b/include/bioloid_dyn_master_servos.h
new file mode 100644
index 0000000..b512dd2
--- /dev/null
+++ b/include/bioloid_dyn_master_servos.h
@@ -0,0 +1,16 @@
+#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
+
diff --git a/src/bioloid_dyn_master_servos.c b/src/bioloid_dyn_master_servos.c
new file mode 100755
index 0000000..955e10f
--- /dev/null
+++ b/src/bioloid_dyn_master_servos.c
@@ -0,0 +1,109 @@
+#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);
+}
+
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index ac89b46..e747769 100644
--- a/src/bioloid_stm32.c
+++ b/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);
}
--
GitLab