diff --git a/f1/dynamixel/implementations/dynamixel_master_uart.c b/f1/dynamixel/implementations/dynamixel_master_uart.c
deleted file mode 100755
index ee0728675fbe705f9c92d9cb70bafce32a4bb04c..0000000000000000000000000000000000000000
--- a/f1/dynamixel/implementations/dynamixel_master_uart.c
+++ /dev/null
@@ -1,459 +0,0 @@
-#include "dynamixel_master_uart.h"
-#include "time.h"
-
-#define USART USART2
-#define USART_CLK RCC_APB1Periph_USART2
-#define USART_CLK_INIT RCC_APB1PeriphClockCmd
-#define USART_IRQn USART2_IRQn
-#define USART_IRQHandler USART2_IRQHandler
-
-#define USART_TX_PIN GPIO_Pin_2
-#define USART_TX_GPIO_PORT GPIOA
-#define USART_TX_GPIO_CLK RCC_AHB1Periph_GPIOA
-#define USART_TX_SOURCE GPIO_PinSource2
-#define USART_TX_AF GPIO_AF_USART2
-
-#define USART_RX_PIN GPIO_Pin_3
-#define USART_RX_GPIO_PORT GPIOA
-#define USART_RX_GPIO_CLK RCC_AHB1Periph_GPIOA
-#define USART_RX_SOURCE GPIO_PinSource3
-#define USART_RX_AF GPIO_AF_USART2
-
-#define USART_TX_EN_PIN GPIO_Pin_1
-#define USART_TX_EN_GPIO_PORT GPIOA
-#define USART_TX_EN_GPIO_CLK RCC_AHB1Periph_GPIOA
-#define USART_TX_EN_SOURCE GPIO_PinSource1
-
-#define USART_RX_EN_PIN GPIO_Pin_0
-#define USART_RX_EN_GPIO_PORT GPIOA
-#define USART_RX_EN_GPIO_CLK RCC_AHB1Periph_GPIOA
-#define USART_RX_EN_SOURCE GPIO_PinSource0
-
-#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;
-// 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;
-
-// 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;
- dyn_master_sending_packet=0x01;
- USART_ITConfig(USART, USART_IT_TC, ENABLE);
-}
-
-uint8_t dyn_master_receive(void)
-{
- int16_t timeout_left=dyn_master_timeout;
-
- // wait for the status packet
- while(!dyn_master_packet_ready)
- {
- delay_ms(10);
- timeout_left-=10;
- if(timeout_left<=0)
- 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);
- else
- return DYN_CHECKSUM_ERROR;
-}
-
-void dyn_master_enable_tx(void)
-{
- GPIO_ResetBits(USART_RX_EN_GPIO_PORT, USART_RX_EN_PIN);
- GPIO_SetBits(USART_TX_EN_GPIO_PORT, USART_TX_EN_PIN);
-}
-
-void dyn_master_enable_rx(void)
-{
- GPIO_ResetBits(USART_TX_EN_GPIO_PORT, USART_TX_EN_PIN);
- GPIO_SetBits(USART_RX_EN_GPIO_PORT, USART_RX_EN_PIN);
-}
-
-uint8_t dyn_master_read(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
-{
- uint8_t error;
-
- // generate the read packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_READ);
- // set the device id
- if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // set the length to read
- dyn_set_read_length(dyn_master_tx_buffer,length);
- // sert the start address to start reading
- dyn_set_read_address(dyn_master_tx_buffer,address);
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error==DYN_NO_ERROR)
- {
- // read the data from the status packet
- dyn_get_status_data(dyn_master_rx_buffer,data);
- }
- return error;
- }
- else
- return error;
-}
-
-uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
-{
- uint8_t error;
-
- // generate the read packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_WRITE);
- // set the device id
- if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // sert the start address to start reading
- dyn_set_read_address(dyn_master_tx_buffer,address);
- // set the data to write and its length
- dyn_set_write_data(dyn_master_tx_buffer,length,data);
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- return error;
- }
- else
- return error;
-}
-
-// 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)
- {
- /* Read one byte from the receive data register */
- dyn_master_rx_buffer[dyn_master_rx_num_data++] = USART_ReceiveData(USART2);
- dyn_parse_status_packet();
- }
-
- if(USART_GetITStatus(USART, USART_IT_TC) != RESET)
- {
- 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
- }
- }
-}
-
-// dynamixel master functions
-void dyn_master_init(void)
-{
- uint16_t i;
- GPIO_InitTypeDef GPIO_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
- USART_InitTypeDef USART_InitStructure;
-
- /* Enable GPIO clock */
- USART_CLK_INIT(USART_CLK, ENABLE);
- RCC_AHB1PeriphClockCmd(USART_TX_GPIO_CLK | USART_RX_GPIO_CLK | USART_TX_EN_GPIO_CLK | USART_RX_EN_GPIO_CLK, ENABLE);
- // configure the GPIO pins
-
- /* Connect USART pins to AF7 */
- GPIO_PinAFConfig(USART_TX_GPIO_PORT, USART_TX_SOURCE, USART_TX_AF);
- GPIO_PinAFConfig(USART_RX_GPIO_PORT, USART_RX_SOURCE, USART_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 = USART_TX_PIN;
- GPIO_Init(USART_TX_GPIO_PORT, &GPIO_InitStructure);
-
- GPIO_InitStructure.GPIO_Pin = USART_RX_PIN;
- GPIO_Init(USART_RX_GPIO_PORT, &GPIO_InitStructure);
-
- /* configure the control pins */
- GPIO_InitStructure.GPIO_Pin = USART_TX_EN_PIN;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_Init(USART_TX_EN_GPIO_PORT, &GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = USART_RX_EN_PIN;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_Init(USART_RX_EN_GPIO_PORT, &GPIO_InitStructure);
-
- dyn_master_enable_rx();
-
- dyn_master_timeout=0x00;
- // initialize the buffers
- for(i=0;i<MAX_BUFFER_LEN;i++)
- {
- 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;
-
- USART_DeInit(USART2);
- USART_StructInit(&USART_InitStructure);
- // configure the serial port
- USART_InitStructure.USART_BaudRate = 1000000;
- 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(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_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_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)
-{
- // save the desired timeout value
- dyn_master_timeout=timeout_ms;
-}
-
-void dyn_master_scan(uint8_t *num,uint8_t *ids)
-{
- uint8_t i;
-
- *num=0;
- for(i=0;i<254;i++)
- {
- if(dyn_master_ping(i)==DYN_NO_ERROR)// the device exists
- {
- ids[*num]=i;
- (*num)++;
- }
- }
-}
-
-uint8_t dyn_master_ping(uint8_t id)
-{
- uint8_t error;
-
- // generate the ping packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_PING);
- // set the device id
- if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- return error;
- }
- else
- return error;
-}
-
-uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data)
-{
- return dyn_master_read(id,address,1,data);
-}
-
-uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data)
-{
- uint8_t value[2];
- uint8_t error;
-
- error=dyn_master_read(id,address,2,value);
- if(error==DYN_NO_ERROR)
- (*data)=value[0]+value[1]*256;
-
- return error;
-}
-
-uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
-{
- return dyn_master_read(id,address,length,data);
-}
-
-uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data)
-{
- return dyn_master_write(id,address,1,&data);
-}
-
-uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data)
-{
- uint8_t value[2];
-
- value[0]=data%256;
- value[1]=data/256;
- return dyn_master_write(id,address,2,value);
-}
-
-uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
-{
- return dyn_master_write(id,address,length,data);
-}
-
-uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
-{
- return DYN_SUCCESS;
-}
-
-void dyn_master_action(void)
-{
-
-}
-
-void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data)
-{
- uint8_t i;
-
- // generate the sync write packet
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_SYNC_WRITE);
- // set the start address
- dyn_set_write_address(dyn_master_tx_buffer,address);
- // set the data length
- dyn_set_sync_write_length(dyn_master_tx_buffer,length);
- // load the data for each device
- for(i=0;i<num;i++)
- dyn_set_sync_write_data(dyn_master_tx_buffer,ids[i],&data[i*length]);
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
-// while(dyn_master_sending_packet);
-}
-
-uint8_t dyn_master_reset(uint8_t id)
-{
- return DYN_SUCCESS;
-}
-
-// repeater functions
-uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet)
-{
- uint8_t error;
-
- // generate the read packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // copy the contents of the instruction packet to the output buffer
- dyn_copy_packet(inst_packet,dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error!=DYN_TIMEOUT)
- {
- // copy the contents of the status of the packet to the output buffer
- dyn_copy_packet(dyn_master_rx_buffer,status_packet);
- }
-
- return error;
-}
-
diff --git a/f1/dynamixel/implementations/dynamixel_master_uart_dma.c b/f1/dynamixel/implementations/dynamixel_master_uart_dma.c
deleted file mode 100755
index 936781e745d7bdda06b85cae10e1e8cdf68816a0..0000000000000000000000000000000000000000
--- a/f1/dynamixel/implementations/dynamixel_master_uart_dma.c
+++ /dev/null
@@ -1,882 +0,0 @@
-#include "dynamixel_master_uart_dma.h"
-#include "motion_manager.h"
-#include "time.h"
-#include "ram.h"
-
-#define USART USART1
-#define USART_CLK RCC_APB2Periph_USART1
-#define USART_CLK_INIT RCC_APB2PeriphClockCmd
-#define USART_IRQn USART1_IRQn
-#define USART_IRQHandler USART1_IRQHandler
-
-#define USART_TX_PIN GPIO_Pin_6
-#define USART_TX_GPIO_PORT GPIOB
-#define USART_TX_GPIO_CLK RCC_APB2Periph_GPIOB
-#define USART_TX_SOURCE GPIO_PinSource6
-
-#define USART_RX_PIN GPIO_Pin_7
-#define USART_RX_GPIO_PORT GPIOB
-#define USART_RX_GPIO_CLK RCC_APB2Periph_GPIOB
-#define USART_RX_SOURCE GPIO_PinSource7
-
-#define USART_TX_EN_PIN GPIO_Pin_4
-#define USART_TX_EN_GPIO_PORT GPIOB
-#define USART_TX_EN_GPIO_CLK RCC_APB2Periph_GPIOB
-#define USART_TX_EN_SOURCE GPIO_PinSource4
-
-#define USART_RX_EN_PIN GPIO_Pin_5
-#define USART_RX_EN_GPIO_PORT GPIOB
-#define USART_RX_EN_GPIO_CLK RCC_APB2Periph_GPIOB
-#define USART_RX_EN_SOURCE GPIO_PinSource5
-
-/* DMA configuration */
-#define USART_DR_ADDRESS ((uint32_t)USART1 + 0x04)
-#define USART_DMA DMA1
-#define USART_DMA_CLK RCC_AHBPeriph_DMA1
-
-#define USART_TX_DMA_CHANNEL DMA1_Channel4
-#define USART_TX_DMA_FLAG_GLIF DMA1_FLAG_GL4
-#define USART_TX_DMA_FLAG_TEIF DMA1_FLAG_TE4
-#define USART_TX_DMA_FLAG_HTIF DMA1_FLAG_HT4
-#define USART_TX_DMA_FLAG_TCIF DMA1_FLAG_TC4
-
-#define USART_RX_DMA_CHANNEL DMA1_Channel5
-#define USART_RX_DMA_FLAG_GLIF DMA1_FLAG_GL5
-#define USART_RX_DMA_FLAG_TEIF DMA1_FLAG_TE5
-#define USART_RX_DMA_FLAG_HTIF DMA1_FLAG_HT5
-#define USART_RX_DMA_FLAG_TCIF DMA1_FLAG_TC5
-
-#define USART_DMA_TX_IRQn DMA1_Channel4_IRQn
-#define USART_DMA_RX_IRQn DMA1_Channel5_IRQn
-#define USART_DMA_TX_IRQHandler DMA1_Channel4_IRQHandler
-#define USART_DMA_RX_IRQHandler DMA1_Channel5_IRQHandler
-
-#define POWER_GPIO_CLK RCC_APB2Periph_GPIOB
-#define POWER_PIN GPIO_Pin_8
-#define POWER_GPIO_PORT GPIOB
-#define POWER_SOURCE GPIO_PinSource8
-
-
-#define MAX_BUFFER_LEN 1024
-
-// private variables
-uint8_t dyn_master_version;
-uint16_t dyn_master_timeout;// answer reception timeout
-// input buffer
-uint8_t dyn_master_rx_buffer[MAX_BUFFER_LEN];
-volatile uint8_t dyn_master_receiving_header;
-// output buffer
-uint8_t dyn_master_tx_buffer[MAX_BUFFER_LEN];
-// instruction packet ready flag
-volatile uint8_t dyn_master_packet_ready;
-// sending status packet flag
-volatile uint8_t dyn_master_sending_packet;
-// no answer for sync write operation
-uint8_t dyn_master_no_answer;
-// power enabled variable
-uint8_t dyn_master_power_enabled;
-// receiving a bulk read backet
-uint8_t dyn_bulk_read_packet;
-uint8_t dyn_bulk_read_length;
-// DMA initialization data structures
-DMA_InitTypeDef DMA_TX_InitStructure;
-DMA_InitTypeDef DMA_RX_InitStructure;
-
-// private functions
-void dyn_master_reset(void)
-{
- DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,DISABLE);
- USART_DMACmd(USART, USART_DMAReq_Rx, DISABLE);
- DMA_Cmd(USART_RX_DMA_CHANNEL,DISABLE);
- /* wait until the transaction ends */
- DMA_ClearFlag(USART_RX_DMA_FLAG_GLIF);
- DMA_ClearITPendingBit(USART_RX_DMA_FLAG_GLIF);
- /* clear any pending data */
- USART_ReceiveData(USART);
- dyn_master_receiving_header=0x01;
- dyn_master_packet_ready=0x00;
-}
-
-void dyn_master_send(void)
-{
- dyn_master_version=1;
- // wait until any previous transmission ends
- while(dyn_master_sending_packet);
- // set the DMA transfer
- DMA_SetCurrDataCounter(USART_TX_DMA_CHANNEL,dyn_get_length(dyn_master_tx_buffer)+4);
- DMA_Cmd(USART_TX_DMA_CHANNEL,ENABLE);
- USART_ClearFlag(USART,USART_FLAG_TC);
- USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE);
- dyn_master_sending_packet=0x01;
-}
-
-void dyn2_master_send(void)
-{
- dyn_master_version=2;
- // wait until any previous transmission ends
- while(dyn_master_sending_packet);
- // set the DMA transfer
- DMA_SetCurrDataCounter(USART_TX_DMA_CHANNEL,dyn2_get_length(dyn_master_tx_buffer)+7);
- DMA_Cmd(USART_TX_DMA_CHANNEL,ENABLE);
- USART_ClearFlag(USART,USART_FLAG_TC);
- USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE);
- dyn_master_sending_packet=0x01;
-}
-
-uint8_t dyn_master_receive(void)
-{
- int16_t timeout_left=dyn_master_timeout;
-
- // wait for the status packet
- while(!dyn_master_packet_ready)
- {
- delay_ms(1);
- timeout_left-=1;
- if(timeout_left<=0)
- {
- dyn_master_reset();
- return DYN_TIMEOUT;
- }
- }
- dyn_master_packet_ready=0x00;
- // check the input packet checksum
- if(dyn_check_checksum(dyn_master_rx_buffer)==0xFF)
- return dyn_get_status_error(dyn_master_rx_buffer);
- else
- {
- dyn_master_reset();
- return DYN_CHECKSUM_ERROR;
- }
-}
-
-uint8_t dyn2_master_receive(void)
-{
- int16_t timeout_left=dyn_master_timeout;
-
- // wait for the status packet
- while(!dyn_master_packet_ready)
- {
- delay_ms(1);
- timeout_left-=1;
- if(timeout_left<=0)
- {
- dyn_master_reset();
- return DYN_TIMEOUT;
- }
- }
- dyn_master_packet_ready=0x00;
- // check the input packet checksum
- if(dyn2_check_status_crc(dyn_master_rx_buffer)==0x01)
- return dyn2_get_status_error(dyn_master_rx_buffer);
- else
- {
- dyn_master_reset();
- return DYN_CHECKSUM_ERROR;
- }
-}
-
-void dyn_master_enable_tx(void)
-{
- GPIO_ResetBits(USART_RX_EN_GPIO_PORT, USART_RX_EN_PIN);
- GPIO_SetBits(USART_TX_EN_GPIO_PORT, USART_TX_EN_PIN);
-}
-
-void dyn_master_enable_rx(void)
-{
- GPIO_ResetBits(USART_TX_EN_GPIO_PORT, USART_TX_EN_PIN);
- GPIO_SetBits(USART_RX_EN_GPIO_PORT, USART_RX_EN_PIN);
-}
-
-uint8_t dyn_master_read(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
-{
- uint8_t error;
-
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- // generate the read packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_READ);
- // set the device id
- if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // set the length to read
- dyn_set_read_length(dyn_master_tx_buffer,length);
- // sert the start address to start reading
- dyn_set_read_address(dyn_master_tx_buffer,address);
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error==DYN_NO_ERROR)
- dyn_get_status_data(dyn_master_rx_buffer,data);// read the data from the status packet
- else
- dyn_master_reset();
- return error;
- }
- else
- return error;
-}
-
-uint8_t dyn2_master_read(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
-{
- uint8_t error;
-
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- // generate the read packet for the desired device
- dyn2_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn2_set_instruction(dyn_master_tx_buffer,DYN_READ);
- // set the device id
- if((error=dyn2_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // set the length to read
- dyn2_set_read_length(dyn_master_tx_buffer,length);
- // sert the start address to start reading
- dyn2_set_read_address(dyn_master_tx_buffer,address);
- // set the checksum
- dyn2_set_inst_crc(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn2_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn2_master_receive();
- if(error==DYN_NO_ERROR)
- dyn2_get_status_data(dyn_master_rx_buffer,data);// read the data from the status packet
- else
- dyn_master_reset();
- return error;
- }
- else
- return error;
-}
-
-uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
-{
- uint8_t error;
-
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- // generate the read packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_WRITE);
- // set the device id
- if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // sert the start address to start reading
- dyn_set_read_address(dyn_master_tx_buffer,address);
- // set the data to write and its length
- dyn_set_write_data(dyn_master_tx_buffer,length,data);
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error!=DYN_NO_ERROR)
- dyn_master_reset();
- return error;
- }
- else
- return error;
-}
-
-uint8_t dyn2_master_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
-{
- uint8_t error;
-
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- // generate the read packet for the desired device
- dyn2_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn2_set_instruction(dyn_master_tx_buffer,DYN_WRITE);
- // set the device id
- if((error=dyn2_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // sert the start address to start reading
- dyn2_set_read_address(dyn_master_tx_buffer,address);
- // set the data to write and its length
- dyn2_set_write_data(dyn_master_tx_buffer,length,data);
- // set the checksum
- dyn2_set_inst_crc(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn2_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn2_master_receive();
- if(error!=DYN_NO_ERROR)
- dyn_master_reset();
- return error;
- }
- else
- return error;
-}
-
-// interrupt handlers
-void USART_IRQHandler(void)
-{
- if(USART_GetITStatus(USART, USART_IT_TC) != RESET)
- {
- USART_ClearFlag(USART,USART_FLAG_TC);
- USART_ITConfig(USART, USART_IT_TC, DISABLE);
- dyn_master_sending_packet=0x00;
- if(dyn_master_no_answer)
- dyn_master_no_answer=0x00;
- else
- {
- if(dyn_bulk_read_packet==0x00)
- {
- // set up the DMA RX transaction
- if(dyn_master_version==1)
- DMA_RX_InitStructure.DMA_BufferSize = 4;
- else
- DMA_RX_InitStructure.DMA_BufferSize = 7;
- DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_rx_buffer;
- DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
- DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
- DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE);
- USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
- dyn_master_receiving_header=0x01;
- }
- else
- {
- // set up the DMA RX transaction
- DMA_RX_InitStructure.DMA_BufferSize = dyn_bulk_read_length;
- DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_rx_buffer;
- DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
- DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
- DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE);
- USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
- dyn_master_receiving_header=0x00;
- dyn_bulk_read_packet=0x00;
- }
- }
- }
-}
-
-void USART_DMA_TX_IRQHandler(void)
-{
- DMA_Cmd(USART_TX_DMA_CHANNEL,DISABLE);
- DMA_ClearFlag(USART_TX_DMA_FLAG_GLIF);
- DMA_ClearITPendingBit(USART_TX_DMA_FLAG_GLIF);
- USART_ITConfig(USART, USART_IT_TC, ENABLE);
-}
-
-void USART_DMA_RX_IRQHandler(void)
-{
- if(dyn_master_receiving_header==0x01)
- {
- DMA_Cmd(USART_RX_DMA_CHANNEL,DISABLE);
- if(dyn_master_version==1)
- {
- DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_master_rx_buffer);
- DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_master_rx_buffer[4];
- }
- else
- {
- DMA_RX_InitStructure.DMA_BufferSize = dyn2_get_length(dyn_master_rx_buffer);
- DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_master_rx_buffer[7];
- }
- DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
- DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE);
- USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
- DMA_ClearFlag(USART_RX_DMA_FLAG_GLIF);
- DMA_ClearITPendingBit(USART_RX_DMA_FLAG_GLIF);
- dyn_master_receiving_header=0x00;
- }
- else
- {
- DMA_Cmd(USART_RX_DMA_CHANNEL,DISABLE);
- DMA_ClearFlag(USART_RX_DMA_FLAG_GLIF);
- DMA_ClearITPendingBit(USART_RX_DMA_FLAG_GLIF);
- DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,DISABLE);
- dyn_master_packet_ready=0x01;
- }
-}
-
-// dynamixel master functions
-void dyn_master_init(void)
-{
- uint16_t i;
- GPIO_InitTypeDef GPIO_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
- USART_InitTypeDef USART_InitStructure;
-
- /* Enable GPIO clock */
- USART_CLK_INIT(USART_CLK, ENABLE);
- RCC_APB1PeriphClockCmd(USART_TX_GPIO_CLK | USART_RX_GPIO_CLK | USART_TX_EN_GPIO_CLK | USART_RX_EN_GPIO_CLK, ENABLE);
- RCC_APB1PeriphClockCmd(POWER_GPIO_CLK, ENABLE);
- // configure the GPIO pins
-
- /* Connect USART pins to AF7 */
- GPIO_PinRemapConfig(GPIO_Remap_USART1, ENABLE);
-
- /* Configure USART Tx and Rx as alternate function push-pull */
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Pin = USART_TX_PIN;
- GPIO_Init(USART_TX_GPIO_PORT, &GPIO_InitStructure);
-
- GPIO_InitStructure.GPIO_Pin = USART_RX_PIN;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
- GPIO_Init(USART_RX_GPIO_PORT, &GPIO_InitStructure);
-
- /* configure the control pins */
- GPIO_InitStructure.GPIO_Pin = USART_TX_EN_PIN;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(USART_TX_EN_GPIO_PORT, &GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = USART_RX_EN_PIN;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(USART_RX_EN_GPIO_PORT, &GPIO_InitStructure);
-
- dyn_master_enable_rx();
-
- dyn_master_timeout=0x00;
- // initialize the buffers
- for(i=0;i<MAX_BUFFER_LEN;i++)
- {
- dyn_master_rx_buffer[i]=0x00;
- dyn_master_tx_buffer[i]=0x00;
- }
- // initialize the flags
- dyn_master_packet_ready=0x00;
- dyn_master_sending_packet=0x00;
- dyn_master_receiving_header=0x01;
- dyn_master_no_answer=0x00;
- dyn_bulk_read_packet=0x00;
- dyn_bulk_read_length=0x00;
- dyn_master_version=1;
-
- USART_DeInit(USART);
- USART_StructInit(&USART_InitStructure);
- // configure the serial port
- USART_InitStructure.USART_BaudRate = 1000000;
- 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(USART, &USART_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannel = USART_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- 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_AHBPeriphClockCmd(USART_DMA_CLK, ENABLE);
- DMA_DeInit(USART_TX_DMA_CHANNEL);
- 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_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
- DMA_TX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- DMA_TX_InitStructure.DMA_Priority = DMA_Priority_High;
- DMA_TX_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
- DMA_TX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_tx_buffer;
- DMA_Init(USART_TX_DMA_CHANNEL,&DMA_TX_InitStructure);
- /* initialize DMA interrupts */
- NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_TX_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- DMA_ITConfig(USART_TX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
- DMA_ITConfig(USART_TX_DMA_CHANNEL,DMA_IT_HT | DMA_IT_TE,DISABLE);
-
- /* Configure RX DMA */
- DMA_DeInit(USART_RX_DMA_CHANNEL);
- DMA_RX_InitStructure.DMA_BufferSize = 4;// transfer the first 3 bytes
- 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_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
- DMA_RX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- DMA_RX_InitStructure.DMA_Priority = DMA_Priority_High;
- DMA_RX_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
- DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_rx_buffer;
- DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
- /* initialize DMA interrupts */
- NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_RX_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC | DMA_IT_HT | DMA_IT_TE,DISABLE);
-
- /* Enable the USART2 */
- USART_Cmd(USART, ENABLE);
-
- // initialize the power enable gpio
- GPIO_InitStructure.GPIO_Pin = POWER_PIN;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(POWER_GPIO_PORT, &GPIO_InitStructure);
-
- dyn_master_disable_power();
-}
-
-void dyn_master_set_timeout(uint16_t timeout_ms)
-{
- // save the desired timeout value
- dyn_master_timeout=timeout_ms;
-}
-
-void dyn_master_scan(uint8_t *num,uint8_t *ids)
-{
- uint8_t i;
-
- for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
- ids[0]=0;
- *num=0;
- for(i=0;i<254;i++)
- {
- if(dyn_master_ping(i)==DYN_NO_ERROR)// the device exists
- {
- ids[*num]=i;
- (*num)++;
- }
- }
-}
-
-void dyn2_master_scan(uint8_t *num,uint8_t *ids)
-{
- uint8_t i;
-
- for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
- ids[0]=0;
- *num=0;
- for(i=0;i<254;i++)
- {
- if(dyn2_master_ping(i)==DYN_NO_ERROR)// the device exists
- {
- ids[*num]=i;
- (*num)++;
- }
- }
-}
-
-inline void dyn_master_enable_power(void)
-{
- GPIO_SetBits(POWER_GPIO_PORT,POWER_PIN);
- dyn_master_power_enabled=0x01;
-}
-
-inline void dyn_master_disable_power(void)
-{
- GPIO_ResetBits(POWER_GPIO_PORT,POWER_PIN);
- dyn_master_power_enabled=0x00;
-}
-
-uint8_t dyn_master_is_power_enabled(void)
-{
- return dyn_master_power_enabled;
-}
-
-uint8_t dyn_master_ping(uint8_t id)
-{
- uint8_t error;
-
- // generate the ping packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_PING);
- // set the device id
- if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while (dyn_master_sending_packet==0x01);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- return error;
- }
- else
- return error;
-}
-
-uint8_t dyn2_master_ping(uint8_t id)
-{
- uint8_t error;
-
- // generate the ping packet for the desired device
- dyn2_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn2_set_instruction(dyn_master_tx_buffer,DYN_PING);
- // set the device id
- if((error=dyn2_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // set the checksum
- dyn2_set_inst_crc(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn2_master_send();
- // wait for the transmission to end
- while (dyn_master_sending_packet==0x01);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn2_master_receive();
- return error;
- }
- else
- return error;
-}
-
-uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data)
-{
- return dyn_master_read(id,address,1,data);
-}
-
-uint8_t dyn2_master_read_byte(uint8_t id,uint16_t address,uint8_t *data)
-{
- return dyn2_master_read(id,address,1,data);
-}
-
-uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data)
-{
- uint8_t value[2];
- uint8_t error;
-
- error=dyn_master_read(id,address,2,value);
- if(error==DYN_NO_ERROR)
- (*data)=value[0]+value[1]*256;
-
- return error;
-}
-
-uint8_t dyn2_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
-{
- uint8_t value[2];
- uint8_t error;
-
- error=dyn2_master_read(id,address,2,value);
- if(error==DYN_NO_ERROR)
- (*data)=value[0]+value[1]*256;
-
- return error;
-}
-
-uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
-{
- return dyn_master_read(id,address,length,data);
-}
-
-uint8_t dyn2_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
-{
- return dyn2_master_read(id,address,length,data);
-}
-
-uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data)
-{
- return dyn_master_write(id,address,1,&data);
-}
-
-uint8_t dyn2_master_write_byte(uint8_t id, uint16_t address, uint8_t data)
-{
- return dyn2_master_write(id,address,1,&data);
-}
-
-uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data)
-{
- uint8_t value[2];
-
- value[0]=data%256;
- value[1]=data/256;
- return dyn_master_write(id,address,2,value);
-}
-
-uint8_t dyn2_master_write_word(uint8_t id, uint16_t address, uint16_t data)
-{
- uint8_t value[2];
-
- value[0]=data%256;
- value[1]=data/256;
- return dyn2_master_write(id,address,2,value);
-}
-
-uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
-{
- return dyn_master_write(id,address,length,data);
-}
-
-uint8_t dyn2_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
-{
- return dyn2_master_write(id,address,length,data);
-}
-
-uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
-{
- return DYN_SUCCESS;
-}
-
-void dyn_master_action(void)
-{
-
-}
-
-void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data)
-{
- uint8_t i;
-
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_no_answer=0x01;
- // generate the sync write packet
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_SYNC_WRITE);
- // set the start address
- dyn_set_write_address(dyn_master_tx_buffer,address);
- // set the data length
- dyn_set_sync_write_length(dyn_master_tx_buffer,length);
- // load the data for each device
- for(i=0;i<num;i++)
- dyn_set_sync_write_data(dyn_master_tx_buffer,ids[i],&data[i*length]);
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
-}
-
-void dyn2_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, uint8_t *data)
-{
- uint8_t i;
-
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_no_answer=0x01;
- // generate the sync write packet
- dyn2_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn2_set_instruction(dyn_master_tx_buffer,DYN_SYNC_WRITE);
- // set the start address
- dyn2_set_write_address(dyn_master_tx_buffer,address);
- // set the data length
- dyn2_set_sync_write_length(dyn_master_tx_buffer,length);
- // load the data for each device
- for(i=0;i<num;i++)
- dyn2_set_sync_write_data(dyn_master_tx_buffer,ids[i],&data[i*length]);
- // set the checksum
- dyn2_set_inst_crc(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- for(i=0;i<27;i++)
- ram_data[DARWIN_SERVO_0_SPEED+i]=dyn_master_tx_buffer[i];
-
- dyn2_master_send();
- // wait for the transmission to end
-}
-
-// repeater functions
-uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet)
-{
- uint8_t error;
-
- // generate the read packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // copy the contents of the instruction packet to the output buffer
- dyn_copy_packet(inst_packet,dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error!=DYN_TIMEOUT)
- {
- // copy the contents of the status of the packet to the output buffer
- dyn_copy_packet(dyn_master_rx_buffer,status_packet);
- }
-
- return error;
-}
-
-uint8_t dyn_master_bulk_read(uint8_t num,TBulkData *info)
-{
- uint8_t i,error;
-
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_bulk_read_packet=0x01;
- dyn_bulk_read_length=0x00;
- // generate the sync write packet
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_BULK_READ);
- // set the start address
- dyn_set_write_address(dyn_master_tx_buffer,0x00);
- // load the data for each device
- for(i=0;i<num;i++)
- {
- dyn_set_bulk_read_data(dyn_master_tx_buffer,info[i].id,info[i].length,info[i].address);
- dyn_bulk_read_length+=info[i].length+6;
- }
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error!=DYN_NO_ERROR)
- {
- dyn_master_reset();
- return error;
- }
- for(i=0;i<num;i++)
- {
- error=dyn_get_bulk_read_data(dyn_master_rx_buffer,dyn_bulk_read_length,info[i].id,info[i].data);
- if(error!=DYN_SUCCESS)
- return error;
- }
-
- return DYN_SUCCESS;
-}
diff --git a/f1/dynamixel/implementations/dynamixel_master_uart_dma.h b/f1/dynamixel/implementations/dynamixel_master_uart_dma.h
deleted file mode 100755
index cf0e788b23dd0109244c420b1060130aa72876be..0000000000000000000000000000000000000000
--- a/f1/dynamixel/implementations/dynamixel_master_uart_dma.h
+++ /dev/null
@@ -1,46 +0,0 @@
-#ifndef _DYNAMIXEL_MASTER_DMA_H
-#define _DYNAMIXEL_MASTER_DMA_H
-
-#include "stm32f10x.h"
-#include "dynamixel.h"
-#include "dynamixel2.h"
-
-typedef struct{
- uint8_t id;
- uint8_t address;
- uint8_t length;
- uint8_t *data;
-}TBulkData;
-
-// 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);
-void dyn_master_action(void);
-void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data);
-void dyn2_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, uint8_t *data);
-uint8_t dyn_master_bulk_read(uint8_t num,TBulkData *info);
-// repeater functions
-uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet);
-
-#endif
diff --git a/f1/dynamixel/implementations/dynamixel_slave_spi.c b/f1/dynamixel/implementations/dynamixel_slave_spi.c
deleted file mode 100644
index e05447f1ba5e952ad217a1af7497b4d90bec8c29..0000000000000000000000000000000000000000
--- a/f1/dynamixel/implementations/dynamixel_slave_spi.c
+++ /dev/null
@@ -1,249 +0,0 @@
-#include "dynamixel_slave_spi.h"
-
-#define SPI SPI1
-#define SPI_CLK RCC_APB2Periph_SPI1
-#define SPI_CLK_INIT RCC_APB2PeriphClockCmd
-#define SPI_IRQn SPI1_IRQn
-#define SPI_IRQHANDLER SPI1_IRQHandler
-
-#define SPI_SCK_PIN GPIO_Pin_5
-#define SPI_SCK_GPIO_PORT GPIOA
-#define SPI_SCK_GPIO_CLK RCC_AHB1Periph_GPIOA
-#define SPI_SCK_SOURCE GPIO_PinSource5
-#define SPI_SCK_AF GPIO_AF_SPI1
-
-#define SPI_MISO_PIN GPIO_Pin_6
-#define SPI_MISO_GPIO_PORT GPIOA
-#define SPI_MISO_GPIO_CLK RCC_AHB1Periph_GPIOA
-#define SPI_MISO_SOURCE GPIO_PinSource6
-#define SPI_MISO_AF GPIO_AF_SPI1
-
-#define SPI_MOSI_PIN GPIO_Pin_7
-#define SPI_MOSI_GPIO_PORT GPIOA
-#define SPI_MOSI_GPIO_CLK RCC_AHB1Periph_GPIOA
-#define SPI_MOSI_SOURCE GPIO_PinSource7
-#define SPI_MOSI_AF GPIO_AF_SPI1
-
-#define SPI_NSS_PIN GPIO_Pin_4
-#define SPI_NSS_GPIO_PORT GPIOA
-#define SPI_NSS_GPIO_CLK RCC_AHB1Periph_GPIOA
-#define SPI_NSS_SOURCE GPIO_PinSource4
-#define SPI_NSS_AF GPIO_AF_SPI1
-
-#define MAX_BUFFER_LEN 1024
-
-// private varibales
-uint8_t dyn_slave_address;// this module slave address
-// input buffer
-uint8_t dyn_slave_rx_buffer[MAX_BUFFER_LEN];
-uint16_t dyn_slave_rx_num_data;
-// output buffer
-uint8_t dyn_slave_tx_buffer[MAX_BUFFER_LEN];
-uint16_t dyn_slave_tx_num_data;
-uint16_t dyn_slave_tx_ptr;
-// instruction packet ready flag
-volatile uint8_t dyn_slave_packet_ready;
-// sending status packet flag
-volatile uint8_t dyn_slave_sending_packet;
-
-// private functions
-void dyn_parse_inst_packet(void)
-{
- if(dyn_slave_rx_num_data>3)// the length byte has been received
- {
- if(dyn_slave_rx_num_data==(dyn_get_length(dyn_slave_rx_buffer)+4))// payload size plus header size
- {
- dyn_slave_packet_ready=0x01;
- }
- }
-}
-
-void dyn_slave_set_rx_mode(void)
-{
- /* Disable the Tx empty interrupt */
- SPI_I2S_ITConfig(SPI, SPI_I2S_IT_TXE, DISABLE);
- /* Enable the Rx buffer not empty interrupt */
- SPI_I2S_ITConfig(SPI, SPI_I2S_IT_RXNE, ENABLE);
-}
-
-void dyn_slave_set_tx_mode(void)
-{
- /* Disable the Rx buffer not empty interrupt */
- SPI_I2S_ITConfig(SPI, SPI_I2S_IT_RXNE, DISABLE);
- /* Enable the Tx empty interrupt */
- SPI_I2S_ITConfig(SPI, SPI_I2S_IT_TXE, ENABLE);
-}
-
-// interrupt handlers
-void SPI_IRQHANDLER(void)
-{
- /* SPI in Receiver mode */
- if (SPI_I2S_GetITStatus(SPI, SPI_I2S_IT_RXNE) == SET)
- {
- if(dyn_slave_sending_packet==0x00)
- {
- GPIO_ToggleBits(GPIOD, GPIO_Pin_12);
- dyn_slave_rx_buffer[dyn_slave_rx_num_data++] = SPI_I2S_ReceiveData(SPI);
- dyn_parse_inst_packet();
- }
- else
- SPI_I2S_ReceiveData(SPI);
- }
- /* SPI in Tramitter mode */
- if (SPI_I2S_GetITStatus(SPI, SPI_I2S_IT_TXE) == SET)
- {
- if(dyn_slave_sending_packet==0x01)
- {
- if(dyn_slave_tx_num_data==0x00)// there is no more data to be sent
- {
- dyn_slave_tx_ptr=0x00;
- dyn_slave_sending_packet=0x00;
- // disable interrupts
- dyn_slave_set_rx_mode();
- SPI_I2S_ReceiveData(SPI);
- }
- else
- {
- SPI_I2S_SendData(SPI,dyn_slave_tx_buffer[dyn_slave_tx_ptr++]);// send the next_byte
- dyn_slave_tx_num_data--;
- }
- }
- }
-}
-
-// public functions
-void dyn_slave_init(void)
-{
- uint16_t i;
- GPIO_InitTypeDef GPIO_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
- SPI_InitTypeDef SPI_InitStructure;
-
- // initialize the buffers
- for(i=0;i<MAX_BUFFER_LEN;i++)
- {
- dyn_slave_rx_buffer[i]=0x00;
- dyn_slave_tx_buffer[i]=0x00;
- }
- dyn_slave_rx_num_data=0x00;
- dyn_slave_tx_num_data=0x00;
- dyn_slave_tx_ptr=0x00;
- // initialize the flags
- dyn_slave_packet_ready=0x00;
- dyn_slave_sending_packet=0x00;
-
- /* Enable the SPI clock */
- SPI_CLK_INIT(SPI_CLK, ENABLE);
-
- /* Enable GPIO clocks */
- RCC_AHB1PeriphClockCmd(SPI_SCK_GPIO_CLK | SPI_MISO_GPIO_CLK | SPI_MOSI_GPIO_CLK | SPI_NSS_GPIO_CLK, ENABLE);
-
- /* SPI GPIO Configuration --------------------------------------------------*/
-
- /* Connect SPI pins to AF5 */
- GPIO_PinAFConfig(SPI_SCK_GPIO_PORT, SPI_SCK_SOURCE, SPI_SCK_AF);
- GPIO_PinAFConfig(SPI_MISO_GPIO_PORT, SPI_MISO_SOURCE, SPI_MISO_AF);
- GPIO_PinAFConfig(SPI_MOSI_GPIO_PORT, SPI_MOSI_SOURCE, SPI_MOSI_AF);
- GPIO_PinAFConfig(SPI_NSS_GPIO_PORT, SPI_NSS_SOURCE, SPI_NSS_AF);
-
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
-
- /* SPI SCK pin configuration */
- GPIO_InitStructure.GPIO_Pin = SPI_SCK_PIN;
- GPIO_Init(SPI_SCK_GPIO_PORT, &GPIO_InitStructure);
-
- /* SPI MISO pin configuration */
- GPIO_InitStructure.GPIO_Pin = SPI_MISO_PIN;
- GPIO_Init(SPI_MISO_GPIO_PORT, &GPIO_InitStructure);
-
- /* SPI MOSI pin configuration */
- GPIO_InitStructure.GPIO_Pin = SPI_MOSI_PIN;
- GPIO_Init(SPI_MOSI_GPIO_PORT, &GPIO_InitStructure);
-
- /* SPI NSS pin configuration */
- GPIO_InitStructure.GPIO_Pin = SPI_NSS_PIN;
- GPIO_Init(SPI_NSS_GPIO_PORT, &GPIO_InitStructure);
-
- /* SPI configuration -------------------------------------------------------*/
- SPI_I2S_DeInit(SPI);
- SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
- SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
- SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
- SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
- SPI_InitStructure.SPI_NSS = SPI_NSS_Hard;
- SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_32;
- SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
- SPI_InitStructure.SPI_CRCPolynomial = 7;
- SPI_InitStructure.SPI_Mode = SPI_Mode_Slave;
- SPI_Init(SPI, &SPI_InitStructure);
-
- /* Configure the SPI interrupt priority */
- NVIC_InitStructure.NVIC_IRQChannel = SPI_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- /* by default set receive mode */
- dyn_slave_set_rx_mode();
-
- /* Enable the SPI peripheral */
- SPI_Cmd(SPI, ENABLE);
-}
-
-void dyn_slave_set_address(uint8_t id)
-{
- dyn_slave_address=id;
-}
-
-uint8_t dyn_slave_get_address(void)
-{
- return dyn_slave_address;
-}
-
-uint8_t dyn_slave_is_packet_ready(void)
-{
- return dyn_slave_packet_ready;
-}
-
-void dyn_slave_get_inst_packet(uint8_t *packet)
-{
- uint8_t i;
-
- for(i=0;i<dyn_slave_rx_num_data;i++)
- packet[i]=dyn_slave_rx_buffer[i];
- dyn_slave_rx_num_data=0x00;
- dyn_slave_packet_ready=0x00;
-}
-
-void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data)
-{
- // wait until the previous transmission has ended (if any)
- while(dyn_slave_sending_packet==0x01);
- // create the status packet
- dyn_init_status_packet(dyn_slave_tx_buffer);
- dyn_set_status_error(dyn_slave_tx_buffer,error);
- dyn_set_id(dyn_slave_tx_buffer,dyn_slave_address);
- dyn_set_status_data(dyn_slave_tx_buffer,length,data);
- dyn_set_checksum(dyn_slave_tx_buffer);
- // start sending the package
- dyn_slave_tx_num_data=dyn_get_length(dyn_slave_tx_buffer)+4+1;
- dyn_slave_sending_packet=0x01;
- dyn_slave_set_tx_mode();
-}
-
-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_init_status_packet(dyn_slave_tx_buffer);
- dyn_copy_packet(packet,dyn_slave_tx_buffer);
- // start sending the package
- dyn_slave_tx_num_data=dyn_get_length(dyn_slave_tx_buffer)+4+1;
- dyn_slave_sending_packet=0x01;
- dyn_slave_set_tx_mode();
-}
diff --git a/f1/dynamixel/implementations/dynamixel_slave_spi.h b/f1/dynamixel/implementations/dynamixel_slave_spi.h
deleted file mode 100644
index cbf764a92eb7b8cfbf6e296f30f45c366adf8283..0000000000000000000000000000000000000000
--- a/f1/dynamixel/implementations/dynamixel_slave_spi.h
+++ /dev/null
@@ -1,16 +0,0 @@
-#ifndef _DYNAMIXEL_SLAVE_SPI_H
-#define _DYNAMIXEL_SLAVE_SPI_H
-
-#include "stm32f4xx.h"
-#include "dynamixel.h"
-
-// public functions
-void dyn_slave_init(void);
-void dyn_slave_set_address(uint8_t id);
-uint8_t dyn_slave_get_address(void);
-uint8_t dyn_slave_is_packet_ready(void);
-void dyn_slave_get_inst_packet(uint8_t *packet);
-void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data);
-void dyn_slave_resend_status_packet(uint8_t *packet);
-
-#endif
diff --git a/f1/dynamixel/implementations/dynamixel_slave_uart_dma.c b/f1/dynamixel/implementations/dynamixel_slave_uart_dma.c
deleted file mode 100644
index 6363f7e3e9e0f29a2b6e6b7cdd0aed07b0b4bfd0..0000000000000000000000000000000000000000
--- a/f1/dynamixel/implementations/dynamixel_slave_uart_dma.c
+++ /dev/null
@@ -1,316 +0,0 @@
-#include "dynamixel_slave_uart_dma.h"
-#include "gpio.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 GPIOB
-#define DYN_SLAVE_TX_GPIO_CLK RCC_APB2Periph_GPIOB
-#define DYN_SLAVE_TX_SOURCE GPIO_PinSource10
-
-#define DYN_SLAVE_RX_PIN GPIO_Pin_11
-#define DYN_SLAVE_RX_GPIO_PORT GPIOB
-#define DYN_SLAVE_RX_GPIO_CLK RCC_APB2Periph_GPIOB
-#define DYN_SLAVE_RX_SOURCE GPIO_PinSource11
-
-/* DMA configuration */
-#define DYN_SLAVE_DR_ADDRESS ((uint32_t)USART3 + 0x04)
-#define DYN_SLAVE_DMA DMA1
-#define DYN_SLAVE_DMA_CLK RCC_AHBPeriph_DMA1
-
-#define DYN_SLAVE_TX_DMA_CHANNEL DMA1_Channel2
-#define DYN_SLAVE_TX_DMA_FLAG_GLIF DMA1_FLAG_GL2
-#define DYN_SLAVE_TX_DMA_FLAG_TEIF DMA1_FLAG_TE2
-#define DYN_SLAVE_TX_DMA_FLAG_HTIF DMA1_FLAG_HT2
-#define DYN_SLAVE_TX_DMA_FLAG_TCIF DMA1_FLAG_TC2
-
-#define DYN_SLAVE_RX_DMA_CHANNEL DMA1_Channel3
-#define DYN_SLAVE_RX_DMA_FLAG_GLIF DMA1_FLAG_GL3
-#define DYN_SLAVE_RX_DMA_FLAG_TEIF DMA1_FLAG_TE3
-#define DYN_SLAVE_RX_DMA_FLAG_HTIF DMA1_FLAG_HT3
-#define DYN_SLAVE_RX_DMA_FLAG_TCIF DMA1_FLAG_TC3
-
-#define DYN_SLAVE_DMA_TX_IRQn DMA1_Channel2_IRQn
-#define DYN_SLAVE_DMA_RX_IRQn DMA1_Channel3_IRQn
-#define DYN_SLAVE_DMA_TX_IRQHandler DMA1_Channel2_IRQHandler
-#define DYN_SLAVE_DMA_RX_IRQHandler DMA1_Channel3_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 th response
-// input buffer
-uint8_t dyn_slave_rx_buffer[MAX_BUFFER_LEN];
-volatile uint8_t dyn_slave_receiving_header;
-// 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)
-{
- 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;
- // set up the DMA RX transaction
- DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = 4;
- DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_slave_rx_buffer;
- DMA_Init(DYN_SLAVE_RX_DMA_CHANNEL,&DYN_SLAVE_DMA_RX_InitStructure);
- DMA_ITConfig(DYN_SLAVE_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
- DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,ENABLE);
- USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
- dyn_slave_receiving_header=0x01;
- }
-}
-
-void DYN_SLAVE_DMA_TX_IRQHandler(void)
-{
- DMA_Cmd(DYN_SLAVE_TX_DMA_CHANNEL,DISABLE);
- DMA_ClearFlag(DYN_SLAVE_TX_DMA_FLAG_GLIF);
- DMA_ClearITPendingBit(DYN_SLAVE_TX_DMA_FLAG_GLIF);
- USART_ITConfig(DYN_SLAVE, USART_IT_TC, ENABLE);
-}
-
-void DYN_SLAVE_DMA_RX_IRQHandler(void)
-{
- if(dyn_slave_receiving_header==0x01)
- {
- DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,DISABLE);
- if(dyn_get_length(dyn_slave_rx_buffer)==0x00)
- {
- DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,DISABLE);
- DMA_ClearFlag(DYN_SLAVE_RX_DMA_FLAG_GLIF);
- DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_FLAG_GLIF);
- DMA_ITConfig(DYN_SLAVE_RX_DMA_CHANNEL,DMA_IT_TC,DISABLE);
- dyn_slave_packet_ready=0x01;
- }
- else
- {
- DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_slave_rx_buffer);
- DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_slave_rx_buffer[4];
- DMA_Init(DYN_SLAVE_RX_DMA_CHANNEL,&DYN_SLAVE_DMA_RX_InitStructure);
- DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,ENABLE);
- USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
- dyn_slave_receiving_header=0x00;
- DMA_ClearFlag(DYN_SLAVE_RX_DMA_FLAG_GLIF);
- DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_FLAG_GLIF);
- }
- }
- else
- {
- DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,DISABLE);
- DMA_ClearFlag(DYN_SLAVE_RX_DMA_FLAG_GLIF);
- DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_FLAG_GLIF);
- DMA_ITConfig(DYN_SLAVE_RX_DMA_CHANNEL,DMA_IT_TC,DISABLE);
- 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_APB1PeriphClockCmd(DYN_SLAVE_TX_GPIO_CLK | DYN_SLAVE_RX_GPIO_CLK, ENABLE);
- // configure the GPIO pins
-
- /* Configure USART Tx and Rx as alternate function push-pull */
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- 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_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- 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_receiving_header=0x01;
-
- USART_DeInit(DYN_SLAVE);
- USART_StructInit(&USART_InitStructure);
- // configure the serial port
- USART_InitStructure.USART_BaudRate = 1000000;
- 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 = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- 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);
- 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);
- 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_AHBPeriphClockCmd(DYN_SLAVE_DMA_CLK, ENABLE);
- DMA_DeInit(DYN_SLAVE_TX_DMA_CHANNEL);
- 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_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_DIR = DMA_DIR_PeripheralDST;
- DYN_SLAVE_DMA_TX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_slave_tx_buffer;
- DYN_SLAVE_DMA_TX_InitStructure.DMA_M2M = DMA_M2M_Disable;
- DMA_Init(DYN_SLAVE_TX_DMA_CHANNEL,&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_CHANNEL,DMA_IT_TC,ENABLE);
- DMA_ITConfig(DYN_SLAVE_TX_DMA_CHANNEL,DMA_IT_HT | DMA_IT_TE,DISABLE);
-
- /* Configure RX DMA */
- DMA_DeInit(DYN_SLAVE_RX_DMA_CHANNEL);
- DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = 4;// transfer the first 3 bytes
- 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_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_DIR = DMA_DIR_PeripheralSRC;
- DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_slave_rx_buffer;
- DYN_SLAVE_DMA_RX_InitStructure.DMA_M2M = DMA_M2M_Disable;
- DMA_Init(DYN_SLAVE_RX_DMA_CHANNEL,&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_CHANNEL,DMA_IT_TC,ENABLE);
- DMA_ITConfig(DYN_SLAVE_RX_DMA_CHANNEL,DMA_IT_HT | DMA_IT_TE,DISABLE);
-
- DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,ENABLE);
- USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, 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;
-}
-
-void dyn_slave_get_inst_packet(uint8_t *packet)
-{
- uint8_t i;
-
- for(i=0;i<dyn_get_length(dyn_slave_rx_buffer)+4;i++)
- packet[i]=dyn_slave_rx_buffer[i];
- dyn_slave_packet_ready=0x00;
-}
-
-void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data)
-{
- // wait until the previous transmission has ended (if any)
- while(dyn_slave_sending_packet==0x01);
- // create the status packet
- dyn_init_status_packet(dyn_slave_tx_buffer);
- dyn_set_status_error(dyn_slave_tx_buffer,error);
- dyn_set_id(dyn_slave_tx_buffer,dyn_slave_address);
- dyn_set_status_data(dyn_slave_tx_buffer,length,data);
- dyn_set_checksum(dyn_slave_tx_buffer);
- // set the DMA transfer
- DMA_SetCurrDataCounter(DYN_SLAVE_TX_DMA_CHANNEL,dyn_get_length(dyn_slave_tx_buffer)+4);
- DMA_Cmd(DYN_SLAVE_TX_DMA_CHANNEL,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_init_status_packet(dyn_slave_tx_buffer);
- dyn_copy_packet(packet,dyn_slave_tx_buffer);
- // set the DMA transfer
- DMA_SetCurrDataCounter(DYN_SLAVE_TX_DMA_CHANNEL,dyn_get_length(dyn_slave_tx_buffer)+4);
- DMA_Cmd(DYN_SLAVE_TX_DMA_CHANNEL,ENABLE);
- USART_ClearFlag(DYN_SLAVE,USART_FLAG_TC);
- USART_DMACmd(DYN_SLAVE, USART_DMAReq_Tx, ENABLE);
- dyn_slave_sending_packet=0x01;
-}
-
-void dyn_slave_reset(void)
-{
- dyn_slave_sending_packet=0x00;
- // set up the DMA RX transaction
- DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = 4;
- DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_slave_rx_buffer;
- DMA_Init(DYN_SLAVE_RX_DMA_CHANNEL,&DYN_SLAVE_DMA_RX_InitStructure);
- DMA_ITConfig(DYN_SLAVE_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
- DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,ENABLE);
- USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
- dyn_slave_receiving_header=0x01;
-}
diff --git a/f1/dynamixel/implementations/dynamixel_slave_uart_dma.h b/f1/dynamixel/implementations/dynamixel_slave_uart_dma.h
deleted file mode 100644
index 88b0a66f62b065cb3d223c444eece4df1afcefd7..0000000000000000000000000000000000000000
--- a/f1/dynamixel/implementations/dynamixel_slave_uart_dma.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef _DYNAMIXEL_SLAVE_UART_DMA_H
-#define _DYNAXIXEL_SLAVE_UART_DMA_H
-
-#include "dynamixel.h"
-#include "stm32f10x.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);
-void dyn_slave_get_inst_packet(uint8_t *packet);
-void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data);
-void dyn_slave_resend_status_packet(uint8_t *packet);
-void dyn_slave_reset(void);
-
-#endif