Skip to content
GitLab
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
Expand all Hide whitespace changes
Inline Side-by-side
include/dynamixel_master_uart_dma.h deleted 100755 → 0
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
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
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
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
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
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
View file @ 2763aae1
This diff is collapsed.
src/dynamixel_slave_uart_dma.c deleted 100644 → 0
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
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;
}
......
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