diff --git a/include/dynamixel_master_uart_dma.h b/include/dynamixel_master_uart_dma.h
deleted file mode 100755
index e07789ed1722735f0f6e2ad4272de497e602df56..0000000000000000000000000000000000000000
--- a/include/dynamixel_master_uart_dma.h
+++ /dev/null
@@ -1,49 +0,0 @@
-#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
diff --git a/include/dynamixel_slave_uart_dma.h b/include/dynamixel_slave_uart_dma.h
deleted file mode 100644
index 65cb4be5fb5ccfe4893d169a872259a830292d0e..0000000000000000000000000000000000000000
--- a/include/dynamixel_slave_uart_dma.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#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
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 53867960691ae5878f479b8aab349b44fddb64e6..24441227d7f133f7cadb49a9356d8acbaeb9c736 100644
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -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);
diff --git a/src/bioloid_dyn_master_sensors.c b/src/bioloid_dyn_master_sensors.c
index 6c0a87e50496688facbc03e05ee497c648530250..afdee1c8d987ba420e1fab456f69e2af438eb718 100755
--- a/src/bioloid_dyn_master_sensors.c
+++ b/src/bioloid_dyn_master_sensors.c
@@ -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;
diff --git a/src/bioloid_dyn_master_servos.c b/src/bioloid_dyn_master_servos.c
index b4ac7e7c1cf85ce92e139801c2d3d9108b7f40b4..c4c3d4f3800923c595d7e5a75206a98ee247f190 100755
--- a/src/bioloid_dyn_master_servos.c
+++ b/src/bioloid_dyn_master_servos.c
@@ -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;
diff --git a/src/bioloid_dyn_slave.c b/src/bioloid_dyn_slave.c
index 42305946679fd3738154e7f88622b3eede4c5207..d117a2655d1259dd54b17bf820aac020303a5c94 100644
--- a/src/bioloid_dyn_slave.c
+++ b/src/bioloid_dyn_slave.c
@@ -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]);
diff --git a/src/dynamixel_master_uart_dma.c b/src/dynamixel_master_uart_dma.c
deleted file mode 100755
index a5b515e0225544235f3ea87265bcada689bbfc85..0000000000000000000000000000000000000000
--- a/src/dynamixel_master_uart_dma.c
+++ /dev/null
@@ -1,979 +0,0 @@
-#include "dynamixel_master_uart_dma.h"
-#include "motion_manager.h"
-#include "stm32_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
-
-/* 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 POWER_GPIO_CLK RCC_AHB1Periph_GPIOB
-#define POWER_PIN GPIO_Pin_2
-#define POWER_GPIO_PORT GPIOB
-#define POWER_SOURCE GPIO_PinSource2
-
-
-#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];
-// 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;
-// number of packets to receive
-uint8_t dyn_master_num_read_packets;
-uint8_t dyn_master_packet_length;
-// DMA initialization data structures
-DMA_InitTypeDef DMA_TX_InitStructure;
-DMA_InitTypeDef DMA_RX_InitStructure;
-
-// private functions
-void dyn_master_reset(void)
-{
- /* Reset reception */
- USART_DMACmd(USART, USART_DMAReq_Rx, DISABLE);
- DMA_Cmd(USART_RX_DMA_STREAM,DISABLE);
- /* wait until the transaction ends */
- DMA_ClearFlag(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
- DMA_ClearITPendingBit(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
- /* clear any pending data */
- USART_ReceiveData(USART);
- /* Reset transmission */
- USART_DMACmd(USART, USART_DMAReq_Tx, DISABLE);
- DMA_Cmd(USART_TX_DMA_STREAM,DISABLE);
- 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, DISABLE);
- /* Reset variables */
- dyn_master_packet_ready=0x00;
- dyn_master_sending_packet=0x00;
-}
-
-uint8_t dyn_master_wait_sending(void)
-{
- uint16_t timeout_left=dyn_master_timeout*1000;
-
- while(dyn_master_sending_packet)
- {
- delay_us(100);
- timeout_left-=100;
- if(timeout_left<=0)
- {
- dyn_master_reset();
- return DYN_TIMEOUT;
- }
- }
-
- return DYN_SUCCESS;
-}
-
-uint8_t dyn_master_send(void)
-{
- uint8_t error;
-
- dyn_master_version=1;
- // wait until any previous transmission ends
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- return error;
- // 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_ClearITPendingBit(USART,USART_FLAG_TC);
- USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE);
- dyn_master_sending_packet=0x01;
- return error;
-
-}
-
-uint8_t dyn2_master_send(void)
-{
- uint8_t error;
-
- dyn_master_version=2;
- // wait until any previous transmission ends
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- return error;
- // set the DMA transfer
- DMA_SetCurrDataCounter(USART_TX_DMA_STREAM,dyn2_get_length(dyn_master_tx_buffer)+7);
- DMA_Cmd(USART_TX_DMA_STREAM,ENABLE);
- USART_ClearFlag(USART,USART_FLAG_TC);
- USART_ClearITPendingBit(USART,USART_FLAG_TC);
- USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE);
- dyn_master_sending_packet=0x01;
- return error;
-}
-
-uint8_t dyn_master_receive(void)
-{
- uint16_t timeout_left=dyn_master_timeout*1000;
-
- // wait for the status packet
- while(!dyn_master_packet_ready)
- {
- delay_us(100);
- timeout_left-=100;
- 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)
-{
- uint16_t timeout_left=dyn_master_timeout*1000;
-
- // wait for the status packet
- while(!dyn_master_packet_ready)
- {
- delay_us(100);
- timeout_left-=100;
- if(timeout_left<=0)
- {
- dyn_master_reset();
- return DYN_TIMEOUT;
- }
- }
- dyn_master_packet_ready=0x00;
- // check the input packet checksum
- if(dyn2_check_checksum(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;
-
- /* generate the read packet */
- dyn_init_read_packet(dyn_master_tx_buffer,id,address,length);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error==DYN_NO_ERROR)
- dyn_get_read_status_data(dyn_master_rx_buffer,data);// read the data from the status packet
- else
- dyn_master_reset();
- return error;
-}
-
-uint8_t dyn2_master_read(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
-{
- uint8_t error;
-
- /* generate the read packet */
- dyn2_init_read_packet(dyn_master_tx_buffer,id,address,length);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn2_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn2_master_receive();
- if(error==DYN_NO_ERROR)
- dyn2_get_read_status_data(dyn_master_rx_buffer,data);// read the data from the status packet
- else
- dyn_master_reset();
- 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_write_packet(dyn_master_tx_buffer,id,address,length,data);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- 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;
-}
-
-uint8_t dyn2_master_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
-{
- uint8_t error;
-
- // generate the read packet for the desired device
- dyn2_init_write_packet(dyn_master_tx_buffer,id,address,length,data);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn2_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- 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;
-}
-
-// interrupt handlers
-void USART_IRQHandler(void)
-{
- static uint8_t num_bytes=0;
- uint8_t data;
-
- if(USART_GetITStatus(USART, USART_IT_RXNE) != RESET)
- {
- USART_ClearFlag(USART,USART_FLAG_RXNE);
- data=USART_ReceiveData(USART);
- switch(num_bytes)
- {
- case 0: if(data==0xFF)
- {
- dyn_master_rx_buffer[num_bytes]=data;
- num_bytes++;
- }
- break;
- case 1: if(data==0xFF)
- {
- dyn_master_rx_buffer[num_bytes]=data;
- num_bytes++;
- }
- else num_bytes--;
- break;
- case 2: if(data!=0xFF)
- {
- dyn_master_rx_buffer[num_bytes]=data;
- num_bytes++;
- }
- break;
- case 3: dyn_master_rx_buffer[num_bytes]=data;
- num_bytes=0;
- /* disable USART RX interrupts */
- USART_ITConfig(USART,USART_IT_RXNE,DISABLE);
- /* enable DMA RX */
- DMA_RX_InitStructure.DMA_BufferSize = data;
- DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_master_rx_buffer[dyn_master_packet_length+4];
- dyn_master_packet_length+=data+4;
- DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
- DMA_Cmd(USART_RX_DMA_STREAM,ENABLE);
- USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
- break;
- default: break;
- }
- }
- else 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
- {
- /* enable RX interrupts */
- USART_ITConfig(USART, USART_IT_RXNE, ENABLE);
- }
- }
-}
-
-void USART_DMA_TX_IRQHandler(void)
-{
- DMA_Cmd(USART_TX_DMA_STREAM,DISABLE);
- DMA_ClearFlag(USART_TX_DMA_STREAM,USART_TX_DMA_FLAG_TCIF);
- DMA_ClearITPendingBit(USART_TX_DMA_STREAM,USART_TX_DMA_FLAG_TCIF);
- USART_DMACmd(USART, USART_DMAReq_Tx, DISABLE);
- USART_ITConfig(USART, USART_IT_TC, ENABLE);
-}
-
-void USART_DMA_RX_IRQHandler(void)
-{
- DMA_Cmd(USART_RX_DMA_STREAM,DISABLE);
- DMA_ClearFlag(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
- DMA_ClearITPendingBit(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
- USART_DMACmd(USART, USART_DMAReq_Rx, DISABLE);
- dyn_master_num_read_packets--;
- if(dyn_master_num_read_packets==0x00)
- {
- dyn_master_packet_length=0x00;
- dyn_master_packet_ready=0x01;
- }
- else
- {
- /* enable RX interrupts */
- USART_ITConfig(USART, USART_IT_RXNE, ENABLE);
- }
-}
-
-// 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);
- RCC_AHB1PeriphClockCmd(POWER_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;
- }
- // initialize the flags
- dyn_master_packet_ready=0x00;
- dyn_master_sending_packet=0x00;
- dyn_master_no_answer=0x00;
- dyn_master_num_read_packets=0x00;
- dyn_master_packet_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 = 0;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
- 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);
-
- /* Clear all USART interrupt flags */
- USART_ClearFlag(USART,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(USART, USART_FLAG_RXNE |
- USART_FLAG_ORE |
- USART_FLAG_TXE |
- USART_FLAG_CTS |
- USART_FLAG_LBD |
- USART_FLAG_IDLE |
- USART_FLAG_PE |
- USART_FLAG_TC);
-
- // 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 = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
- 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_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[4];
- DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
- /* initialize DMA interrupts */
- NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_RX_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC,ENABLE);
- DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,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;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
- 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_ping_packet(dyn_master_tx_buffer,id);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- return error;
-}
-
-uint8_t dyn2_master_ping(uint8_t id)
-{
- uint8_t error;
-
- // generate the ping packet for the desired device
- dyn2_init_ping_packet(dyn_master_tx_buffer,id);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn2_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn2_master_receive();
- 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)
-{
- uint8_t error;
-
- // generate the ping packet for the desired device
- dyn_init_reg_write_packet(dyn_master_tx_buffer,id,address,length,data);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- return error;
-}
-
-uint8_t dyn2_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
-{
- uint8_t error;
-
- // generate the ping packet for the desired device
- dyn2_init_reg_write_packet(dyn_master_tx_buffer,id,address,length,data);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn2_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn2_master_receive();
- return error;
-}
-
-uint8_t dyn_master_action(void)
-{
- dyn_master_no_answer=0x01;
- // generate the ping packet for the desired device
- dyn_init_action_packet(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- return dyn_master_send();
-}
-
-uint8_t dyn2_master_action(void)
-{
- dyn_master_no_answer=0x01;
- // generate the ping packet for the desired device
- dyn2_init_action_packet(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- return dyn2_master_send();
-}
-
-uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, TWriteData *data)
-{
- dyn_master_no_answer=0x01;
- // generate the sync write packet
- dyn_init_sync_write_packet(dyn_master_tx_buffer,num,ids,address,length,data);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- return dyn_master_send();
-}
-
-uint8_t dyn2_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, TWriteData *data)
-{
- dyn_master_no_answer=0x01;
- // generate the sync write packet
- dyn2_init_sync_write_packet(dyn_master_tx_buffer,num,ids,address,length,data);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- return dyn2_master_send();
-}
-
-// repeater functions
-uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet)
-{
- uint8_t error;
-
- // copy the contents of the instruction packet to the output buffer
- dyn_copy_packet(inst_packet,dyn_master_tx_buffer);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- 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,uint8_t *ids,uint8_t *address,uint8_t *length,TWriteData *data)
-{
- uint16_t offset=0;
- uint8_t i,error;
-
- dyn_master_num_read_packets=num;
- // generate the sync write packet
- dyn_init_bulk_read_packet(dyn_master_tx_buffer,num,ids,address,length);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- 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_status_error(&dyn_master_rx_buffer[offset]);
- if(error!=DYN_NO_ERROR)
- return error;
- offset+=dyn_get_read_status_data(&dyn_master_rx_buffer[offset],data[i].data_addr)+4;
- }
-
- return DYN_SUCCESS;
-}
-
-
diff --git a/src/dynamixel_slave_uart_dma.c b/src/dynamixel_slave_uart_dma.c
deleted file mode 100644
index 3fb16778cc9fb565b29d476ed72e241754166ddc..0000000000000000000000000000000000000000
--- a/src/dynamixel_slave_uart_dma.c
+++ /dev/null
@@ -1,404 +0,0 @@
-#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;
-}
-
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 869d4d9e96ddf3ddce620461c8925297f82874d4..9f1f39a303bc3391b8480de073a42374aacc23b3 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -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;
}