diff --git a/Makefile b/Makefile
index 96d9771a07750574c3bf785d4247b4c017aa9e08..6e70113ac0619acc80e1990f9ebc4a9718c73085 100755
--- a/Makefile
+++ b/Makefile
@@ -11,6 +11,7 @@ TARGET_FILES+=src/eeprom.c
TARGET_FILES+=src/ram.c
TARGET_FILES+=src/bioloid_time.c
TARGET_FILES+=src/bioloid_dyn_slave.c
+TARGET_FILES+=src/bioloid_dyn_master_sensors.c
TARGET_PROCESSOR=STM32F407VG
HAL_PATH=../../STM32_processor/hal/f4
@@ -71,6 +72,7 @@ TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal.c
TARGET_FILES+=$(HAL_PATH)/src/stm32f4xx_hal_uart.c
TARGET_FILES+=$(USART_PATH)/src/usart3.c
+TARGET_FILES+=$(USART_PATH)/src/usart2.c
BIOLOID_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
BIOLOID_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(BIOLOID_OBJS_TMP))
diff --git a/include/bioloid_dyn_master_sensors.h b/include/bioloid_dyn_master_sensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..db8a6c449dfc14fe3f811c010fa71949f36a8642
--- /dev/null
+++ b/include/bioloid_dyn_master_sensors.h
@@ -0,0 +1,12 @@
+#ifndef _BIOLOID_DYN_MASTER_SENSORS_H
+#define _BIOLOID_DYN_MASTER_SENSORS_H
+
+#include "stm32f4xx_hal.h"
+#include "stm32_time.h"
+#include "dynamixel_master.h"
+#include "comm.h"
+
+void bioloid_dyn_master_sensors_init(void);
+
+#endif
+
diff --git a/src/bioloid_dyn_master_sensors.c b/src/bioloid_dyn_master_sensors.c
new file mode 100755
index 0000000000000000000000000000000000000000..783f116c17276b6485735206372ba3758a7dae24
--- /dev/null
+++ b/src/bioloid_dyn_master_sensors.c
@@ -0,0 +1,83 @@
+#include "bioloid_dyn_master_sensors.h"
+#include "bioloid_time.h"
+#include "usart2.h"
+#include "gpio.h"
+
+#define ENABLE_RX_EN_GPIO_CLK __HAL_RCC_GPIOA_CLK_ENABLE()
+#define RX_EN_PIN GPIO_PIN_1
+#define RX_EN_GPIO_PORT GPIOA
+
+#define ENABLE_TX_EN_GPIO_CLK __HAL_RCC_GPIOA_CLK_ENABLE()
+#define TX_EN_PIN GPIO_PIN_0
+#define TX_EN_GPIO_PORT GPIOA
+
+/* private variables */
+TDynamixelMaster bioloid_dyn_master_sensors;
+TTime bioloid_dyn_master_sensors_timer;
+TComm bioloid_dyn_master_sensors_comm;
+
+// private functions
+void bioloid_dyn_master_sensors_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_sensors_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_sensors_init(void)
+{
+ GPIO_InitTypeDef GPIO_InitStructure;
+ TUSART_IRQ_Priorities priorities;
+ UART_InitTypeDef Init;
+
+ // initialize timer structure
+ time_init(&bioloid_dyn_master_sensors_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_sensors_set_rx_mode();
+
+ /* initialize the comm object */
+ comm_init(&bioloid_dyn_master_sensors_comm,0x01,&bioloid_dyn_master_sensors_timer);
+ Init.BaudRate = 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;
+
+ 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;
+
+ usart2_init(&bioloid_dyn_master_sensors_comm,&Init,&priorities);
+ dyn_master_init(&bioloid_dyn_master_sensors,&bioloid_dyn_master_sensors_comm);
+ bioloid_dyn_master_sensors.set_rx_mode=bioloid_dyn_master_sensors_set_rx_mode;
+ bioloid_dyn_master_sensors.set_tx_mode=bioloid_dyn_master_sensors_set_tx_mode;
+
+ if(dyn_master_ping(&bioloid_dyn_master_sensors,0xC0)==DYN_SUCCESS)
+ gpio_set_led(RXD_LED);
+ else
+ gpio_clear_led(RXD_LED);
+}
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index 7b8d3f0a3e93d31a03ad2155a719893d79f8ed72..8c9688988924eb475345fd6bf33728f666450102 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -4,6 +4,7 @@
#include "ram.h"
#include "bioloid_time.h"
#include "bioloid_dyn_slave.h"
+#include "bioloid_dyn_master_sensors.h"
int32_t main(void)
{
@@ -19,6 +20,8 @@ int32_t main(void)
/* initialize the dynamixel slave interface */
bioloid_dyn_slave_init();
bioloid_dyn_slave_start();
+ /* initialize the dynamixel master module for the sensors */
+ bioloid_dyn_master_sensors_init();
while(1);
}
diff --git a/src/gpio.c b/src/gpio.c
index 95dd2f500ccf88431b5da50437873346b7e6246f..2d1e1fb798045af17722ee0fd6490e2708439643 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -97,10 +97,7 @@ void GPI_EXTI1_IRQHandler(void)
{
ram_data[BIOLOID_USER_PB_CNTRL]|=GPIO_INT_FLAG;
if(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_EN)
- {
bioloid_dyn_slave_set_int();
- gpio_toggle_led(TXD_LED);
- }
}
}
}
@@ -530,30 +527,42 @@ void gpio_process_read_cmd(unsigned short int address,unsigned short int length,
if(ram_in_range(BIOLOID_RESET_PB_CNTRL,address,length))
if(!(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
{
- ram_data[BIOLOID_RESET_PB_CNTRL]&=(~GPIO_INT_FLAG);
- if(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_EN)
- bioloid_dyn_slave_clear_int();
+ if(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_FLAG)
+ {
+ ram_data[BIOLOID_RESET_PB_CNTRL]&=(~GPIO_INT_FLAG);
+ if(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_clear_int();
+ }
}
if(ram_in_range(BIOLOID_USER_PB_CNTRL,address,length))
if(!(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
{
- ram_data[BIOLOID_USER_PB_CNTRL]&=(~GPIO_INT_FLAG);
- if(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_EN)
- bioloid_dyn_slave_clear_int();
+ if(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_FLAG)
+ {
+ ram_data[BIOLOID_USER_PB_CNTRL]&=(~GPIO_INT_FLAG);
+ if(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_clear_int();
+ }
}
if(ram_in_range(BIOLOID_START_PB_CNTRL,address,length))
if(!(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
{
- ram_data[BIOLOID_START_PB_CNTRL]&=(~GPIO_INT_FLAG);
- if(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_EN)
- bioloid_dyn_slave_clear_int();
+ if(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_FLAG)
+ {
+ ram_data[BIOLOID_START_PB_CNTRL]&=(~GPIO_INT_FLAG);
+ if(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_clear_int();
+ }
}
if(ram_in_range(BIOLOID_MODE_PB_CNTRL,address,length))
if(!(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
{
- ram_data[BIOLOID_MODE_PB_CNTRL]&=(~GPIO_INT_FLAG);
- if(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_EN)
- bioloid_dyn_slave_clear_int();
+ if(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_FLAG)
+ {
+ ram_data[BIOLOID_MODE_PB_CNTRL]&=(~GPIO_INT_FLAG);
+ if(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_clear_int();
+ }
}
}
@@ -720,10 +729,7 @@ void gpio_process_write_cmd(unsigned short int address,unsigned short int length
ram_data[BIOLOID_RESET_PB_CNTRL]=data[BIOLOID_RESET_PB_CNTRL-address];
if(ram_in_range(BIOLOID_USER_PB_CNTRL,address,length))
if(!(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
- {
- gpio_toggle_led(RXD_LED);
ram_data[BIOLOID_USER_PB_CNTRL]=data[BIOLOID_USER_PB_CNTRL-address];
- }
if(ram_in_range(BIOLOID_START_PB_CNTRL,address,length))
if(!(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
ram_data[BIOLOID_START_PB_CNTRL]=data[BIOLOID_START_PB_CNTRL-address];