Commit d884d1d3 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added the dynamixel master interface for the sensors.

parent ec3d0c2f
......@@ -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))
......
#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
#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);
}
......@@ -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);
}
......
......@@ -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];
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment