Skip to content
Snippets Groups Projects
Commit a60463df authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added support for version 2 protocol. 

removed all dynamixel master and slave modules.
Added a function to get the motion period in us for the motion modules.
parent 2763aae1
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 25 additions and 1466 deletions
include/dynamixel_master_uart_dma.h deleted 100755 → 0
+ 0
− 49
View file @ 2763aae1
#ifndef _DYNAMIXEL_MASTER_DMA_H
#define _DYNAMIXEL_MASTER_DMA_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f4xx.h"
#include "dynamixel.h"
#include "dynamixel2.h"
// dynamixel master functions
void dyn_master_init(void);
void dyn_master_reset(void);
void dyn_master_set_timeout(uint16_t timeout_ms);
void dyn_master_scan(uint8_t *num,uint8_t *ids);
void dyn2_master_scan(uint8_t *num,uint8_t *ids);
inline void dyn_master_enable_power(void);
inline void dyn_master_disable_power(void);
uint8_t dyn_master_is_power_enabled(void);
uint8_t dyn_master_ping(uint8_t id);
uint8_t dyn2_master_ping(uint8_t id);
uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data);
uint8_t dyn2_master_read_byte(uint8_t id,uint16_t address,uint8_t *data);
uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data);
uint8_t dyn2_master_read_word(uint8_t id,uint16_t address,uint16_t *data);
uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data);
uint8_t dyn2_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data);
uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data);
uint8_t dyn2_master_write_byte(uint8_t id, uint16_t address, uint8_t data);
uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data);
uint8_t dyn2_master_write_word(uint8_t id, uint16_t address, uint16_t data);
uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data);
uint8_t dyn2_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data);
uint8_t dyn2_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
uint8_t dyn_master_action(void);
uint8_t dyn2_master_action(void);
uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, TWriteData *data);
uint8_t dyn2_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, TWriteData *data);
uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint8_t *address, uint8_t *length, TWriteData *data);
// repeater functions
uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet);
#ifdef __cplusplus
}
#endif
#endif
include/dynamixel_slave_uart_dma.h deleted 100644 → 0
+ 0
− 29
View file @ 2763aae1
#ifndef _DYNAMIXEL_SLAVE_UART_DMA_H
#define _DYNAXIXEL_SLAVE_UART_DMA_H
#ifdef __cplusplus
extern "C" {
#endif
#include "dynamixel.h"
#include "dynamixel2.h"
#include "stm32f4xx.h"
// public functions
void dyn_slave_init(void);
inline void dyn_slave_set_address(uint8_t id);
inline uint8_t dyn_slave_get_address(void);
inline void dyn_slave_set_return_delay(uint8_t delay);
inline void dyn_slave_set_return_level(uint8_t level);
inline uint8_t dyn_slave_get_return_level(void);
uint8_t dyn_slave_is_packet_ready(void);
TDynVersion dyn_slave_get_inst_packet(uint8_t *packet);
void dyn_slave_send_status_packet(uint8_t error,uint16_t length, uint8_t *data);
void dyn_slave_resend_status_packet(uint8_t *packet);
void dyn_slave_reset(void);
#ifdef __cplusplus
}
#endif
#endif
include/motion_manager.h
+ 14
− 2
View file @ a60463df
......@@ -7,13 +7,20 @@ extern "C" {
#include "stm32f4xx_hal.h"
#define MANAGER_MAX_NUM_SERVOS 31
typedef enum{MM_NONE = 0,
MM_ACTION = 1,
MM_WALKING = 2,
MM_JOINTS = 3} TModules;
typedef struct
{
void (*process_fnct)(void);
void (*init_fnct)(void);
uint8_t name[16];
}TMotionModule;
#define MAX_MOTION_MODULES 8
// servo information structure
typedef struct{
uint8_t id;
......@@ -32,12 +39,15 @@ typedef struct{
uint8_t dyn_version;
}TServoInfo;
#define MANAGER_MAX_NUM_SERVOS 31
// public variables
extern int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
// public functions
void manager_init(uint16_t period_us);
inline uint16_t manager_get_period(void);
inline uint16_t manager_get_period_us(void);
inline void manager_set_period(uint16_t period_us);
inline void manager_enable(void);
inline void manager_disable(void);
......@@ -52,6 +62,8 @@ inline void manager_disable_servo(uint8_t servo_id);
inline uint8_t manager_is_servo_enabled(uint8_t servo_id);
inline int16_t manager_get_cw_angle_limit(uint8_t servo_id);
inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id);
// motion modules handling
//void manager_add_module(
// operation functions
uint8_t manager_in_range(unsigned short int address, unsigned short int length);
void manager_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
......
src/bioloid_dyn_master_sensors.c
+ 1
− 1
View file @ a60463df
......@@ -72,7 +72,7 @@ void bioloid_dyn_master_sensors_init(void)
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);
dyn_master_init(&bioloid_dyn_master_sensors,&bioloid_dyn_master_sensors_comm,DYN_VER1);
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;
......
src/bioloid_dyn_master_servos.c
+ 1
− 1
View file @ a60463df
......@@ -83,7 +83,7 @@ void bioloid_dyn_master_servos_init(void)
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);
dyn_master_init(&bioloid_dyn_master_servos,&bioloid_dyn_master_servos_comm,DYN_VER1);
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;
......
src/bioloid_dyn_slave.c
+ 1
− 1
View file @ a60463df
......@@ -135,7 +135,7 @@ void bioloid_dyn_slave_init(void)
priorities.dma_tx_subpriority=0;
usart3_init(&bioloid_dyn_slave_comm,&bioloid_comm_init,&priorities);
dyn_slave_init(&bioloid_dyn_slave,&bioloid_dyn_slave_comm,ram_data[BIOLOID_ID]);
dyn_slave_init(&bioloid_dyn_slave,&bioloid_dyn_slave_comm,ram_data[BIOLOID_ID],DYN_VER1);
bioloid_dyn_slave.on_read=bioloid_on_read;
bioloid_dyn_slave.on_write=bioloid_on_write;
dyn_slave_set_return_delay(&bioloid_dyn_slave,ram_data[BIOLOID_RETURN_DELAY_TIME]);
......
src/dynamixel_master_uart_dma.c deleted 100755 → 0
+ 0
− 979
View file @ 2763aae1
This diff is collapsed.
src/dynamixel_slave_uart_dma.c deleted 100644 → 0
+ 0
− 404
View file @ 2763aae1
#include "dynamixel_slave_uart_dma.h"
#define DYN_SLAVE USART3
#define DYN_SLAVE_CLK RCC_APB1Periph_USART3
#define DYN_SLAVE_CLK_INIT RCC_APB1PeriphClockCmd
#define DYN_SLAVE_IRQn USART3_IRQn
#define DYN_SLAVE_IRQHandler USART3_IRQHandler
#define DYN_SLAVE_TX_PIN GPIO_Pin_10
#define DYN_SLAVE_TX_GPIO_PORT GPIOC
#define DYN_SLAVE_TX_GPIO_CLK RCC_AHB1Periph_GPIOC
#define DYN_SLAVE_TX_SOURCE GPIO_PinSource10
#define DYN_SLAVE_TX_AF GPIO_AF_USART3
#define DYN_SLAVE_RX_PIN GPIO_Pin_11
#define DYN_SLAVE_RX_GPIO_PORT GPIOC
#define DYN_SLAVE_RX_GPIO_CLK RCC_AHB1Periph_GPIOB
#define DYN_SLAVE_RX_SOURCE GPIO_PinSource11
#define DYN_SLAVE_RX_AF GPIO_AF_USART3
/* DMA configuration */
#define DYN_SLAVE_DR_ADDRESS ((uint32_t)USART3 + 0x04)
#define DYN_SLAVE_DMA DMA1
#define DYN_SLAVE_DMA_CLK RCC_AHB1Periph_DMA1
#define DYN_SLAVE_TX_DMA_CHANNEL DMA_Channel_4
#define DYN_SLAVE_TX_DMA_STREAM DMA1_Stream3
#define DYN_SLAVE_TX_DMA_FLAG_FEIF DMA_FLAG_FEIF3
#define DYN_SLAVE_TX_DMA_FLAG_DMEIF DMA_FLAG_DMEIF3
#define DYN_SLAVE_TX_DMA_FLAG_TEIF DMA_FLAG_TEIF3
#define DYN_SLAVE_TX_DMA_FLAG_HTIF DMA_FLAG_HTIF3
#define DYN_SLAVE_TX_DMA_FLAG_TCIF DMA_FLAG_TCIF3
#define DYN_SLAVE_RX_DMA_CHANNEL DMA_Channel_4
#define DYN_SLAVE_RX_DMA_STREAM DMA1_Stream1
#define DYN_SLAVE_RX_DMA_FLAG_FEIF DMA_FLAG_FEIF1
#define DYN_SLAVE_RX_DMA_FLAG_DMEIF DMA_FLAG_DMEIF1
#define DYN_SLAVE_RX_DMA_FLAG_TEIF DMA_FLAG_TEIF1
#define DYN_SLAVE_RX_DMA_FLAG_HTIF DMA_FLAG_HTIF1
#define DYN_SLAVE_RX_DMA_FLAG_TCIF DMA_FLAG_TCIF1
#define DYN_SLAVE_DMA_TX_IRQn DMA1_Stream3_IRQn
#define DYN_SLAVE_DMA_RX_IRQn DMA1_Stream1_IRQn
#define DYN_SLAVE_DMA_TX_IRQHandler DMA1_Stream3_IRQHandler
#define DYN_SLAVE_DMA_RX_IRQHandler DMA1_Stream1_IRQHandler
#define MAX_BUFFER_LEN 1024
// private variables
uint8_t dyn_slave_address;// this module slave address
uint8_t dyn_slave_return_level;// type of response
uint8_t dyn_slave_return_delay;// delay in the response
TDynVersion dyn_slave_version;// version of the dynamixel packet being received
// input buffer
uint8_t dyn_slave_rx_buffer[MAX_BUFFER_LEN];
// output buffer
uint8_t dyn_slave_tx_buffer[MAX_BUFFER_LEN];
// instruction packet ready flag
volatile uint8_t dyn_slave_packet_ready;
// sending status packet flag
volatile uint8_t dyn_slave_sending_packet;
// DMA initialization data structures
DMA_InitTypeDef DYN_SLAVE_DMA_TX_InitStructure;
DMA_InitTypeDef DYN_SLAVE_DMA_RX_InitStructure;
// private functions
// interrupt handlers
void DYN_SLAVE_IRQHandler(void)
{
static uint16_t v2_length=0;
static uint8_t num_bytes=0;
uint8_t data;
if(USART_GetITStatus(DYN_SLAVE, USART_IT_RXNE) != RESET)
{
USART_ClearFlag(DYN_SLAVE,USART_FLAG_RXNE);
data=USART_ReceiveData(DYN_SLAVE);
switch(num_bytes)
{
case 0: if(data==0xFF)
{
dyn_slave_rx_buffer[num_bytes]=data;
num_bytes++;
}
break;
case 1: if(data==0xFF)
{
dyn_slave_rx_buffer[num_bytes]=data;
num_bytes++;
}
else num_bytes--;
break;
case 2: if(data==0xFD)// version 2 packet
{
dyn_slave_rx_buffer[num_bytes]=data;
dyn_slave_version=DYN_VER2;
num_bytes++;
}
else if(data!=0xFF)// version 1 packet
{
dyn_slave_rx_buffer[num_bytes]=data;
dyn_slave_version=DYN_VER1;
num_bytes++;
}
break;
case 3: dyn_slave_rx_buffer[num_bytes]=data;
if(dyn_slave_version==DYN_VER1)
{
num_bytes=0;
/* disable USART RX interrupts */
USART_ITConfig(DYN_SLAVE,USART_IT_RXNE,DISABLE);
/* enable DMA RX */
DYN_SLAVE_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_slave_rx_buffer[4];
DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = data;
DMA_Init(DYN_SLAVE_RX_DMA_STREAM,&DYN_SLAVE_DMA_RX_InitStructure);
DMA_Cmd(DYN_SLAVE_RX_DMA_STREAM,ENABLE);
USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
}
else
num_bytes++;
break;
case 4: dyn_slave_rx_buffer[num_bytes]=data;
num_bytes++;
break;
case 5: dyn_slave_rx_buffer[num_bytes]=data;
v2_length=data;
num_bytes++;
break;
case 6: dyn_slave_rx_buffer[num_bytes]=data;
v2_length+=(data<<8);
num_bytes=0;
/* disable USART RX interrupts */
USART_ITConfig(DYN_SLAVE,USART_IT_RXNE,DISABLE);
/* enable DMA RX */
DYN_SLAVE_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_slave_rx_buffer[7];
DMA_SetCurrDataCounter(DYN_SLAVE_RX_DMA_STREAM,v2_length);
DMA_Cmd(DYN_SLAVE_RX_DMA_STREAM,ENABLE);
USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
break;
default: break;
}
}
else if(USART_GetITStatus(DYN_SLAVE, USART_IT_TC) != RESET)
{
USART_ClearFlag(DYN_SLAVE,USART_FLAG_TC);
USART_ITConfig(DYN_SLAVE, USART_IT_TC, DISABLE);
dyn_slave_sending_packet=0x00;
}
}
void DYN_SLAVE_DMA_TX_IRQHandler(void)
{
DMA_Cmd(DYN_SLAVE_TX_DMA_STREAM,DISABLE);
DMA_ClearFlag(DYN_SLAVE_TX_DMA_STREAM,DYN_SLAVE_TX_DMA_FLAG_TCIF);
DMA_ClearITPendingBit(DYN_SLAVE_TX_DMA_STREAM,DYN_SLAVE_TX_DMA_FLAG_TCIF);
USART_ITConfig(DYN_SLAVE, USART_IT_TC, ENABLE);
}
void DYN_SLAVE_DMA_RX_IRQHandler(void)
{
DMA_Cmd(DYN_SLAVE_RX_DMA_STREAM,DISABLE);
DMA_ClearFlag(DYN_SLAVE_RX_DMA_STREAM,DYN_SLAVE_RX_DMA_FLAG_TCIF);
DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_STREAM,DYN_SLAVE_RX_DMA_FLAG_TCIF);
USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, DISABLE);
// enable USART RX interrupt
USART_ITConfig(DYN_SLAVE,USART_IT_RXNE,ENABLE);
dyn_slave_packet_ready=0x01;
}
// public functions
void dyn_slave_init(void)
{
uint16_t i;
GPIO_InitTypeDef GPIO_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
USART_InitTypeDef USART_InitStructure;
/* Enable GPIO clock */
DYN_SLAVE_CLK_INIT(DYN_SLAVE_CLK, ENABLE);
RCC_AHB1PeriphClockCmd(DYN_SLAVE_TX_GPIO_CLK | DYN_SLAVE_RX_GPIO_CLK, ENABLE);
// configure the GPIO pins
/* Connect USART pins to AF7 */
GPIO_PinAFConfig(DYN_SLAVE_TX_GPIO_PORT, DYN_SLAVE_TX_SOURCE, DYN_SLAVE_TX_AF);
GPIO_PinAFConfig(DYN_SLAVE_RX_GPIO_PORT, DYN_SLAVE_RX_SOURCE, DYN_SLAVE_RX_AF);
/* Configure USART Tx and Rx as alternate function push-pull */
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_InitStructure.GPIO_Pin = DYN_SLAVE_TX_PIN;
GPIO_Init(DYN_SLAVE_TX_GPIO_PORT, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = DYN_SLAVE_RX_PIN;
GPIO_Init(DYN_SLAVE_RX_GPIO_PORT, &GPIO_InitStructure);
// initialize the buffers
for(i=0;i<MAX_BUFFER_LEN;i++)
{
dyn_slave_rx_buffer[i]=0x00;
dyn_slave_tx_buffer[i]=0x00;
}
// initialize the flags
dyn_slave_packet_ready=0x00;
dyn_slave_sending_packet=0x00;
dyn_slave_address=0x01;// this module slave address
dyn_slave_return_level=0x00;// type of response
dyn_slave_return_delay=0x00;// delay in the response
dyn_slave_version=DYN_VER1;// version 1
USART_DeInit(DYN_SLAVE);
USART_StructInit(&USART_InitStructure);
// configure the serial port
USART_InitStructure.USART_BaudRate = 115200;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(DYN_SLAVE, &USART_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel = DYN_SLAVE_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* disable all interrupts of the USART */
USART_ITConfig(DYN_SLAVE, USART_IT_RXNE |
USART_IT_ORE |
USART_IT_TXE |
USART_IT_CTS |
USART_IT_LBD |
USART_IT_IDLE |
USART_IT_PE |
USART_IT_ERR |
USART_IT_TC, DISABLE);
/* Clear all USART interrupt flags */
USART_ClearFlag(DYN_SLAVE,USART_FLAG_RXNE |
USART_FLAG_ORE |
USART_FLAG_TXE |
USART_FLAG_CTS |
USART_FLAG_LBD |
USART_FLAG_IDLE |
USART_FLAG_PE |
USART_FLAG_TC);
/* Clear all USART interrupt pending bits */
USART_ClearITPendingBit(DYN_SLAVE, USART_FLAG_RXNE |
USART_FLAG_ORE |
USART_FLAG_TXE |
USART_FLAG_CTS |
USART_FLAG_LBD |
USART_FLAG_IDLE |
USART_FLAG_PE |
USART_FLAG_TC);
/* Enable the DYN_SLAVE3 */
USART_Cmd(DYN_SLAVE, ENABLE);
// configure the DMA channels
/* Configure TX DMA */
RCC_AHB1PeriphClockCmd(DYN_SLAVE_DMA_CLK, ENABLE);
DMA_DeInit(DYN_SLAVE_TX_DMA_STREAM);
DYN_SLAVE_DMA_TX_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
DYN_SLAVE_DMA_TX_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
DYN_SLAVE_DMA_TX_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DYN_SLAVE_DMA_TX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DYN_SLAVE_DMA_TX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DYN_SLAVE_DMA_TX_InitStructure.DMA_Mode = DMA_Mode_Normal;
DYN_SLAVE_DMA_TX_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) (&(DYN_SLAVE->DR));
DYN_SLAVE_DMA_TX_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DYN_SLAVE_DMA_TX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DYN_SLAVE_DMA_TX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DYN_SLAVE_DMA_TX_InitStructure.DMA_Priority = DMA_Priority_High;
DYN_SLAVE_DMA_TX_InitStructure.DMA_Channel = DYN_SLAVE_TX_DMA_CHANNEL;
DYN_SLAVE_DMA_TX_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
DYN_SLAVE_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_slave_tx_buffer;
DMA_Init(DYN_SLAVE_TX_DMA_STREAM,&DYN_SLAVE_DMA_TX_InitStructure);
/* initialize DMA interrupts */
NVIC_InitStructure.NVIC_IRQChannel = DYN_SLAVE_DMA_TX_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
DMA_ITConfig(DYN_SLAVE_TX_DMA_STREAM,DMA_IT_TC,ENABLE);
DMA_ITConfig(DYN_SLAVE_TX_DMA_STREAM,DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,DISABLE);
/* Configure RX DMA */
DMA_DeInit(DYN_SLAVE_RX_DMA_STREAM);
DYN_SLAVE_DMA_RX_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
DYN_SLAVE_DMA_RX_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DYN_SLAVE_DMA_RX_InitStructure.DMA_Mode = DMA_Mode_Normal;
DYN_SLAVE_DMA_RX_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) (&(DYN_SLAVE->DR));
DYN_SLAVE_DMA_RX_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DYN_SLAVE_DMA_RX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DYN_SLAVE_DMA_RX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DYN_SLAVE_DMA_RX_InitStructure.DMA_Priority = DMA_Priority_High;
DYN_SLAVE_DMA_RX_InitStructure.DMA_Channel = DYN_SLAVE_RX_DMA_CHANNEL;
DYN_SLAVE_DMA_RX_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
DYN_SLAVE_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_slave_rx_buffer[4];
DMA_Init(DYN_SLAVE_RX_DMA_STREAM,&DYN_SLAVE_DMA_RX_InitStructure);
/* initialize DMA interrupts */
NVIC_InitStructure.NVIC_IRQChannel = DYN_SLAVE_DMA_RX_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
DMA_ITConfig(DYN_SLAVE_RX_DMA_STREAM,DMA_IT_TC,ENABLE);
DMA_ITConfig(DYN_SLAVE_RX_DMA_STREAM,DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,DISABLE);
/* enable USART RX interrupts */
USART_ITConfig(DYN_SLAVE,USART_IT_RXNE,ENABLE);
}
inline void dyn_slave_set_address(uint8_t id)
{
dyn_slave_address=id;
}
inline uint8_t dyn_slave_get_address(void)
{
return dyn_slave_address;
}
inline void dyn_slave_set_return_delay(uint8_t delay)
{
dyn_slave_return_delay=delay;
}
inline void dyn_slave_set_return_level(uint8_t level)
{
dyn_slave_return_level=level;
}
inline uint8_t dyn_slave_get_return_level(void)
{
return dyn_slave_return_level;
}
uint8_t dyn_slave_is_packet_ready(void)
{
return dyn_slave_packet_ready;
}
TDynVersion dyn_slave_get_inst_packet(uint8_t *packet)
{
uint8_t i;
if(dyn_slave_version==DYN_VER1)
{
for(i=0;i<dyn_get_length(dyn_slave_rx_buffer)+4;i++)
packet[i]=dyn_slave_rx_buffer[i];
}
else
{
for(i=0;i<dyn2_get_length(dyn_slave_rx_buffer)+7;i++)
packet[i]=dyn_slave_rx_buffer[i];
}
dyn_slave_packet_ready=0x00;
return dyn_slave_version;
}
void dyn_slave_send_status_packet(uint8_t error,uint16_t length, uint8_t *data)
{
// wait until the previous transmission has ended (if any)
while(dyn_slave_sending_packet==0x01);
if(dyn_slave_version==DYN_VER1)
{
// create the status packet
dyn_init_status_packet(dyn_slave_tx_buffer,dyn_slave_address,error,length,data);
// set the DMA transfer
DMA_SetCurrDataCounter(DYN_SLAVE_TX_DMA_STREAM,dyn_get_length(dyn_slave_tx_buffer)+4);
}
else
{
// create the status packet
dyn2_init_status_packet(dyn_slave_tx_buffer,dyn_slave_address,error,length,data);
// set the DMA transfer
DMA_SetCurrDataCounter(DYN_SLAVE_TX_DMA_STREAM,dyn_get_length(dyn_slave_tx_buffer)+8);
}
DMA_Cmd(DYN_SLAVE_TX_DMA_STREAM,ENABLE);
USART_ClearFlag(DYN_SLAVE,USART_FLAG_TC);
USART_DMACmd(DYN_SLAVE, USART_DMAReq_Tx, ENABLE);
dyn_slave_sending_packet=0x01;
}
void dyn_slave_resend_status_packet(uint8_t *packet)
{
// wait until the previous transmission has ended (if any)
while(dyn_slave_sending_packet==0x01);
// create the status packet
dyn_copy_packet(packet,dyn_slave_tx_buffer);
// set the DMA transfer
DMA_SetCurrDataCounter(DYN_SLAVE_TX_DMA_STREAM,dyn_get_length(dyn_slave_tx_buffer)+4);
DMA_Cmd(DYN_SLAVE_TX_DMA_STREAM,ENABLE);
USART_ClearFlag(DYN_SLAVE,USART_FLAG_TC);
USART_DMACmd(DYN_SLAVE, USART_DMAReq_Tx, ENABLE);
dyn_slave_sending_packet=0x01;
}
src/motion_manager.c
+ 8
− 0
View file @ a60463df
......@@ -14,6 +14,7 @@
TIM_HandleTypeDef MANAGER_TIM_Handle;
uint16_t manager_motion_period;
uint16_t manager_motion_period_us;
uint8_t manager_num_servos;
TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
// current angles used for all motion modules
......@@ -280,6 +281,7 @@ void manager_init(uint16_t period_us)
HAL_TIMEx_MasterConfigSynchronization(&MANAGER_TIM_Handle, &sMasterConfig);
/* configure ouptut counter channel 4 */
manager_motion_period=(period_us*1000000)>>16;
manager_motion_period_us=period_us;
/* initialize balance parameters */
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
......@@ -297,9 +299,15 @@ uint16_t manager_get_period(void)
return manager_motion_period;
}
uint16_t manager_get_period_us(void)
{
return manager_motion_period_us;
}
void manager_set_period(uint16_t period_us)
{
manager_motion_period=(period_us*1000000)>>16;
manager_motion_period_us=period_us;
ram_data[BIOLOID_MM_PERIOD_L]=period_us&0x00FF;
ram_data[BIOLOID_MM_PERIOD_H]=(period_us&0xFF00)>>8;
}
......
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