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

Implemented the dynamixel interface with DMA. 

Solved some minor bugs.
parent 34b0ea1c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/bioloid_stm32.c
+ 11
− 20
View file @ 21e9d818
......@@ -53,12 +53,22 @@ int32_t main(void)
time_init();
/* initialize the dynamixel master interface */
dyn_master_init();
dyn_master_set_timeout(10);
dyn_master_set_timeout(20);
/* initialize the dynamixel slave interface*/
dyn_slave_init();
EE_ReadVariable(DEVICE_ID_OFFSET,&address);
dyn_slave_set_address((uint8_t)address);
while(1)
{
if(dyn_master_ping(1))
GPIO_SetBits(GPIOD,GPIO_Pin_12);
else
GPIO_ResetBits(GPIOD,GPIO_Pin_12);
GPIO_ToggleBits(GPIOD,GPIO_Pin_14);
delay_ms(100);
}
while(1) /* main function does not return */
{
if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
......@@ -71,7 +81,6 @@ int32_t main(void)
if(dyn_get_id(inst_packet)==dyn_slave_get_address())// the packet is addressed to this device
{
// process the packet
GPIO_ToggleBits(GPIOD, GPIO_Pin_15);
switch(dyn_get_instruction(inst_packet))
{
case DYN_PING: dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
......@@ -104,24 +113,6 @@ int32_t main(void)
dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00);
}
}
// polling example
// GPIO_ToggleBits(GPIOD, GPIO_Pin_13);
// if(dyn_master_ping(4))
// GPIO_SetBits(GPIOD, GPIO_Pin_12);
// else
// GPIO_ResetBits(GPIOD, GPIO_Pin_12);
// delay_ms(10);
// if(dyn_master_ping(3))
// GPIO_SetBits(GPIOD, GPIO_Pin_14);
// else
// GPIO_ResetBits(GPIOD, GPIO_Pin_14);
// delay_ms(1000);
// while(!SPI_I2S_GetFlagStatus(SPIx, SPI_I2S_FLAG_RXNE));
// data=SPI_I2S_ReceiveData(SPIx);
// while(!SPI_I2S_GetFlagStatus(SPIx, SPI_I2S_FLAG_TXE));
// SPI_I2S_SendData(SPIx,data);
// GPIO_ToggleBits(GPIOD, GPIO_Pin_12);
}
}
src/dynamixel_master_uart.c → src/dynamixel_master_uart_dma.c
+ 134
− 63
View file @ 21e9d818
......@@ -29,40 +29,60 @@
#define USART_RX_EN_GPIO_CLK RCC_AHB1Periph_GPIOA
#define USART_RX_EN_SOURCE GPIO_PinSource0
/* DMA configuration */
#define USART_DR_ADDRESS ((uint32_t)USART2 + 0x04)
#define USART_DMA DMA1
#define USART_DMA_CLK RCC_AHB1Periph_DMA1
#define USART_TX_DMA_CHANNEL DMA_Channel_4
#define USART_TX_DMA_STREAM DMA1_Stream6
#define USART_TX_DMA_FLAG_FEIF DMA_FLAG_FEIF6
#define USART_TX_DMA_FLAG_DMEIF DMA_FLAG_DMEIF6
#define USART_TX_DMA_FLAG_TEIF DMA_FLAG_TEIF6
#define USART_TX_DMA_FLAG_HTIF DMA_FLAG_HTIF6
#define USART_TX_DMA_FLAG_TCIF DMA_FLAG_TCIF6
#define USART_RX_DMA_CHANNEL DMA_Channel_4
#define USART_RX_DMA_STREAM DMA1_Stream5
#define USART_RX_DMA_FLAG_FEIF DMA_FLAG_FEIF5
#define USART_RX_DMA_FLAG_DMEIF DMA_FLAG_DMEIF5
#define USART_RX_DMA_FLAG_TEIF DMA_FLAG_TEIF5
#define USART_RX_DMA_FLAG_HTIF DMA_FLAG_HTIF5
#define USART_RX_DMA_FLAG_TCIF DMA_FLAG_TCIF5
#define USART_DMA_TX_IRQn DMA1_Stream6_IRQn
#define USART_DMA_RX_IRQn DMA1_Stream5_IRQn
#define USART_DMA_TX_IRQHandler DMA1_Stream6_IRQHandler
#define USART_DMA_RX_IRQHandler DMA1_Stream5_IRQHandler
#define MAX_BUFFER_LEN 1024
// private variables
uint16_t dyn_master_timeout;// answer reception timeout
// input buffer
uint8_t dyn_master_rx_buffer[MAX_BUFFER_LEN];
uint16_t dyn_master_rx_num_data;
volatile uint8_t dyn_master_receiving_header;
// output buffer
uint8_t dyn_master_tx_buffer[MAX_BUFFER_LEN];
uint16_t dyn_master_tx_num_data;
uint16_t dyn_master_tx_ptr;
// instruction packet ready flag
volatile uint8_t dyn_master_packet_ready;
// sending status packet flag
volatile uint8_t dyn_master_sending_packet;
// DMA initialization data structures
DMA_InitTypeDef DMA_TX_InitStructure;
DMA_InitTypeDef DMA_RX_InitStructure;
// private functions
void dyn_parse_status_packet(void)
{
if(dyn_master_rx_num_data>3)// the length byte has been received
{
if(dyn_master_rx_num_data==(dyn_get_length(dyn_master_rx_buffer)+4))
dyn_master_packet_ready=0x01;
}
}
void dyn_master_send(void)
{
// wait until any previous transmission ends
while(dyn_master_sending_packet);
// send the first byte
dyn_master_tx_num_data=dyn_get_length(dyn_master_tx_buffer)+4;
// set the DMA transfer
DMA_SetCurrDataCounter(USART_TX_DMA_STREAM,dyn_get_length(dyn_master_tx_buffer)+4);
DMA_Cmd(USART_TX_DMA_STREAM,ENABLE);
USART_ClearFlag(USART,USART_FLAG_TC);
USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE);
dyn_master_sending_packet=0x01;
USART_ITConfig(USART, USART_IT_TC, ENABLE);
}
uint8_t dyn_master_receive(void)
......@@ -78,7 +98,6 @@ uint8_t dyn_master_receive(void)
return DYN_TIMEOUT;
}
dyn_master_packet_ready=0x00;
dyn_master_rx_num_data=0x00;
// check the input packet checksum
if(dyn_check_checksum(dyn_master_rx_buffer)==0xFF)
return dyn_get_status_error(dyn_master_rx_buffer);
......@@ -168,34 +187,50 @@ uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *d
}
// interrupt handlers
/**
* @brief This function handles USART1 global interrupt request.
* @param None
* @retval None
*/
void USART_IRQHandler(void)
{
if(USART_GetITStatus(USART, USART_IT_RXNE) != RESET)
if(USART_GetITStatus(USART, USART_IT_TC) != RESET)
{
/* Read one byte from the receive data register */
dyn_master_rx_buffer[dyn_master_rx_num_data++] = USART_ReceiveData(USART2);
dyn_parse_status_packet();
USART_ClearFlag(USART,USART_FLAG_TC);
USART_ITConfig(USART, USART_IT_TC, DISABLE);
dyn_master_sending_packet=0x00;
// set up the DMA RX transaction
DMA_RX_InitStructure.DMA_BufferSize = 4;
DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_master_rx_buffer;
DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC,ENABLE);
DMA_Cmd(USART_RX_DMA_STREAM,ENABLE);
USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
dyn_master_receiving_header=0x01;
}
}
if(USART_GetITStatus(USART, USART_IT_TC) != RESET)
void USART_DMA_TX_IRQHandler(void)
{
DMA_ClearFlag(USART_TX_DMA_STREAM,USART_TX_DMA_FLAG_TCIF);
DMA_ClearITPendingBit(USART_TX_DMA_STREAM,USART_TX_DMA_FLAG_TCIF);
USART_ITConfig(USART, USART_IT_TC, ENABLE);
}
void USART_DMA_RX_IRQHandler(void)
{
if(dyn_master_receiving_header==0x01)
{
if(dyn_master_tx_num_data==0x00)// there is no more data to be sent
{
dyn_master_tx_ptr=0x00;
dyn_master_sending_packet=0x00;
// disable interrupts
USART_ITConfig(USART, USART_IT_TC, DISABLE);
}
else// there is still data to be sent
{
dyn_master_tx_num_data--;
USART_SendData(USART,dyn_master_tx_buffer[dyn_master_tx_ptr++]);// send the next_byte
}
DMA_ClearFlag(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
DMA_ClearITPendingBit(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_master_rx_buffer);
DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_master_rx_buffer[4];
DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
DMA_Cmd(USART_RX_DMA_STREAM,ENABLE);
USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
dyn_master_receiving_header=0x00;
}
else
{
DMA_ClearFlag(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
DMA_ClearITPendingBit(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC,DISABLE);
dyn_master_packet_ready=0x01;
}
}
......@@ -250,14 +285,12 @@ void dyn_master_init(void)
dyn_master_rx_buffer[i]=0x00;
dyn_master_tx_buffer[i]=0x00;
}
dyn_master_rx_num_data=0x00;
dyn_master_tx_num_data=0x00;
dyn_master_tx_ptr=0x00;
// initialize the flags
dyn_master_packet_ready=0x00;
dyn_master_sending_packet=0x00;
dyn_master_receiving_header=0x01;
USART_DeInit(USART2);
USART_DeInit(USART);
USART_StructInit(&USART_InitStructure);
// configure the serial port
USART_InitStructure.USART_BaudRate = 1000000;
......@@ -267,37 +300,75 @@ void dyn_master_init(void)
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART, &USART_InitStructure);
// configure the interrupts
NVIC_InitStructure.NVIC_IRQChannel = USART_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
USART_ITConfig(USART, USART_IT_RXNE, ENABLE);
USART_ITConfig(USART, USART_IT_ORE, DISABLE);
USART_ITConfig(USART, USART_IT_TXE, DISABLE);
USART_ITConfig(USART, USART_IT_TC, ENABLE);
USART_ITConfig(USART, USART_IT_CTS, DISABLE);
USART_ITConfig(USART, USART_IT_LBD, DISABLE);
USART_ITConfig(USART, USART_IT_IDLE, DISABLE);
USART_ITConfig(USART, USART_IT_PE, DISABLE);
USART_ITConfig(USART, USART_IT_ERR, DISABLE);
/* Enable the USART1 */
USART_ITConfig(USART, 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);
// configure the DMA channels
/* Configure TX DMA */
RCC_AHB1PeriphClockCmd(USART_DMA_CLK, ENABLE);
DMA_DeInit(USART_TX_DMA_STREAM);
DMA_TX_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
DMA_TX_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
DMA_TX_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DMA_TX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_TX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_TX_InitStructure.DMA_Mode = DMA_Mode_Normal;
DMA_TX_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) (&(USART->DR));
DMA_TX_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_TX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_TX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_TX_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_TX_InitStructure.DMA_Channel = USART_TX_DMA_CHANNEL;
DMA_TX_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_master_tx_buffer;
DMA_Init(USART_TX_DMA_STREAM,&DMA_TX_InitStructure);
/* initialize DMA interrupts */
NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_TX_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
DMA_ITConfig(USART_TX_DMA_STREAM,DMA_IT_TC,ENABLE);
DMA_ITConfig(USART_TX_DMA_STREAM,DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,DISABLE);
/* Configure RX DMA */
DMA_DeInit(USART_RX_DMA_STREAM);
DMA_RX_InitStructure.DMA_BufferSize = 4;// transfer the first 3 bytes
DMA_RX_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
DMA_RX_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
DMA_RX_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DMA_RX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_RX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_RX_InitStructure.DMA_Mode = DMA_Mode_Normal;
DMA_RX_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) (&(USART->DR));
DMA_RX_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_RX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_RX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_RX_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_RX_InitStructure.DMA_Channel = USART_RX_DMA_CHANNEL;
DMA_RX_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_master_rx_buffer;
DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
/* initialize DMA interrupts */
NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_RX_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC | DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,DISABLE);
/* Enable the USART2 */
USART_Cmd(USART, ENABLE);
}
void dyn_master_flush(void)
{
// flush only the reception buffer to avoid interrupting a sync_write command
dyn_master_rx_num_data=0x00;
// dyn_master_tx_num_data=0x00;
// dyn_master_tx_ptr=0x00;
// initialize the flags
dyn_master_packet_ready=0x00;
// dyn_master_sending_packet=0x00;
}
void dyn_master_set_timeout(uint16_t timeout_ms)
......@@ -339,7 +410,7 @@ uint8_t dyn_master_ping(uint8_t id)
// send the data
dyn_master_send();
// wait for the transmission to end
while(dyn_master_sending_packet);
while (dyn_master_sending_packet==0x01);
dyn_master_enable_rx();
// wait for the replay within the given timeout
error=dyn_master_receive();
......
src/dynamixel_slave_spi.c
+ 0
− 7
View file @ 21e9d818
......@@ -142,11 +142,6 @@ void dyn_slave_init(void)
RCC_AHB1PeriphClockCmd(SPI_SCK_GPIO_CLK | SPI_MISO_GPIO_CLK | SPI_MOSI_GPIO_CLK | SPI_NSS_GPIO_CLK, ENABLE);
/* SPI GPIO Configuration --------------------------------------------------*/
/* GPIO Deinitialisation */
GPIO_DeInit(SPI_SCK_GPIO_PORT);
GPIO_DeInit(SPI_MISO_GPIO_PORT);
GPIO_DeInit(SPI_MOSI_GPIO_PORT);
GPIO_DeInit(SPI_NSS_GPIO_PORT);
/* Connect SPI pins to AF5 */
GPIO_PinAFConfig(SPI_SCK_GPIO_PORT, SPI_SCK_SOURCE, SPI_SCK_AF);
......@@ -187,8 +182,6 @@ void dyn_slave_init(void)
SPI_InitStructure.SPI_CRCPolynomial = 7;
SPI_InitStructure.SPI_Mode = SPI_Mode_Slave;
SPI_Init(SPI, &SPI_InitStructure);
/* Configure the Priority Group to 1 bit */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
/* Configure the SPI interrupt priority */
NVIC_InitStructure.NVIC_IRQChannel = SPI_IRQn;
......
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