Commit 35ff55ec authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added the dynamixel master module for the servos.

parent 70decacb
......@@ -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))
......
#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
#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);
}
......@@ -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);
}
......
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