Initial version of the bioloid firmware.

Makefile

+# setup
+# modified by zerom for WinARM 8/2010
+
+PROJECT_NAME=bioloid_firmware
+TARGET_FILES=$(wildcard src/*.c)
+TARGET_PROCESSOR=STM32F407VG
+
+include ../../STM32_processor/libraries/f4/select_processor.mk
+
+STM32_LIB_PATH = ../../STM32_processor/libraries/f4/stm32_lib
+STM32_DSP_LIB_PATH = ../../STM32_processor/libraries/f4/stm32_dsp_lib
+DYNAMIXEL_LIB_PATH = ../../STM32_processor/libraries/dynamixel
+STM32_STARTUP_FILES_PATH = ../../STM32_processor/startup/f4
+STM32_LINKER_SCRIPTS_PATH = ../../STM32_processor/libraries/f4/linker_scripts
+BUILD_PATH=build
+
+COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m4 -mthumb -mthumb-interwork 
+COMPILE_OPTS += -Wall -g -O2 -fno-common 
+COMPILE_OPTS += -mfloat-abi=hard -mfpu=fpv4-sp-d16
+COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) -DARM_MATH_CM4 -D__FPU_PRESENT
+INCLUDE_DIRS = -I$(STM32_LIB_PATH)/include -I$(STM32_LIB_PATH)/include/core -I$(STM32_LIB_PATH)/include/devices -I$(STM32_DSP_LIB_PATH)/include -I${DYNAMIXEL_LIB_PATH}/include -I./include
+LIBRARY_DIRS = -L$(STM32_LIB_PATH)/lib -L$(STM32_DSP_LIB_PATH)/lib -L$(DYNAMIXEL_LIB_PATH)/lib -L/usr/arm-none-eabi/lib/fpu/
+
+TCHAIN_PREFIX=arm-none-eabi-
+
+CC = $(TCHAIN_PREFIX)gcc
+CFLAGS = $(COMPILE_OPTS) $(INCLUDE_DIRS) 
+
+AS = $(TCHAIN_PREFIX)gcc
+ASFLAGS = $(COMPILE_OPTS) -c 
+
+LD = $(TCHAIN_PREFIX)gcc
+LDFLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -Wl,-Map=$@.map,-cref $(INCLUDE_DIRS) $(LIBRARY_DIRS) -T $(STM32_LINKER_SCRIPTS_PATH)/$(LINKER_SCRIPT_FILE) -nostartfiles 
+
+OBJCP = $(TCHAIN_PREFIX)objcopy
+OBJCPFLAGS_HEX = -O ihex
+OBJCPFLAGS_BIN = -O binary
+OBJDUMP = $(TCHAIN_PREFIX)objdump
+OBJDUMPFLAGS = -h -S -C -D
+
+MAIN_OUT_ELF = $(BUILD_PATH)/$(PROJECT_NAME).elf
+MAIN_OUT_HEX = $(BUILD_PATH)/$(PROJECT_NAME).hex
+MAIN_OUT_BIN = $(BUILD_PATH)/$(PROJECT_NAME).bin
+MAIN_OUT_LSS = $(BUILD_PATH)/$(PROJECT_NAME).lss
+
+BIOLOID_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
+BIOLOID_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(BIOLOID_OBJS_TMP))
+
+all: $(MAIN_OUT_ELF) $(MAIN_OUT_HEX) $(MAIN_OUT_BIN) $(MAIN_OUT_LSS)
+
+mkdir_build: 
+	mkdir -p $(BUILD_PATH)
+
+$(BUILD_PATH)/%.o: src/%.c 
+	$(CC) -c $(CFLAGS) -o $@ $<
+
+$(MAIN_OUT_ELF): mkdir_build $(BIOLOID_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o) $(STM32_LIB_PATH)/lib/$(LIBRARY) 
+	$(LD) $(LDFLAGS) $(BIOLOID_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(STM32_LIB_PATH)/lib/$(LIBRARY) $(STM32_DSP_LIB_PATH)/lib/libstm32f4_dsp.a ${DYNAMIXEL_LIB_PATH}/lib/dynamixel.a -lm --output $@
+
+$(MAIN_OUT_HEX): $(MAIN_OUT_ELF)	
+	$(OBJCP) $(OBJCPFLAGS_HEX) $< $@
+
+$(MAIN_OUT_BIN): $(MAIN_OUT_ELF)
+	$(OBJCP) $(OBJCPFLAGS_BIN) $< $@
+
+$(MAIN_OUT_LSS): $(MAIN_OUT_ELF)
+	$(OBJDUMP) $(OBJDUMPFLAGS) $< > $@
+
+$(BUID_PATH)/$(STARTUP_FILE:.s=.o): $(STM32_STARTUP_FILES_PATH)/$(STARTUP_FILE)
+	$(AS) $(ASFLAGS) -o $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(STM32_STARTUP_FILES_PATH)/$(STARTUP_FILE) > $(BUILD_PATH)/$(STARTUP_FILE:.s=.lst)
+clean:
+	-rm -rf $(BUILD_PATH)
+

include/dynamixel_master_uart.h

+#ifndef _DYNAMIXEL_MASTER_H
+#define _DYNAMIXEL_MASTER_H
+
+#include "stm32f4xx.h"
+#include "dynamixel.h"
+
+// interrupt handlers
+void USART2_IRQHandler(void);
+
+// dynamixel master functions
+void dyn_master_init(void);
+void dyn_master_flush(void);
+void dyn_master_set_timeout(uint16_t timeout_ms);
+void dyn_master_scan(uint8_t *num,uint8_t *ids);
+uint8_t dyn_master_ping(uint8_t id);
+uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data);
+uint8_t dyn_master_read_word(uint8_t id,uint8_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 dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data);
+uint8_t dyn_master_write_word(uint8_t id, uint8_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 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);
+uint8_t dyn_master_reset(uint8_t id);
+// repeater functions
+uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet);
+
+#endif

include/system_stm32f4xx.h

+/**
+  ******************************************************************************
+  * @file    system_stm32f4xx.h
+  * @author  MCD Application Team
+  * @version V1.3.0
+  * @date    08-November-2013
+  * @brief   CMSIS Cortex-M4 Device System Source File for STM32F4xx devices.       
+  ******************************************************************************  
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
+  *
+  * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
+  * You may not use this file except in compliance with the License.
+  * You may obtain a copy of the License at:
+  *
+  *        http://www.st.com/software_license_agreement_liberty_v2
+  *
+  * Unless required by applicable law or agreed to in writing, software 
+  * distributed under the License is distributed on an "AS IS" BASIS, 
+  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  * See the License for the specific language governing permissions and
+  * limitations under the License.
+  *
+  ******************************************************************************  
+  */ 
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32f4xx_system
+  * @{
+  */  
+  
+/**
+  * @brief Define to prevent recursive inclusion
+  */
+#ifndef __SYSTEM_STM32F4XX_H
+#define __SYSTEM_STM32F4XX_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif 
+
+/** @addtogroup STM32F4xx_System_Includes
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+
+/** @addtogroup STM32F4xx_System_Exported_types
+  * @{
+  */
+
+extern uint32_t SystemCoreClock;          /*!< System Clock Frequency (Core Clock) */
+
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Exported_Constants
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Exported_Macros
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Exported_Functions
+  * @{
+  */
+  
+extern void SystemInit(void);
+extern void SystemCoreClockUpdate(void);
+/**
+  * @}
+  */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /*__SYSTEM_STM32F4XX_H */
+
+/**
+  * @}
+  */
+  
+/**
+  * @}
+  */  
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

include/time.h

+#ifndef _TIME_H
+#define _TIME_H
+
+#include "stm32f4xx.h"
+#include "system_stm32f4xx.h"
+
+void time_init(void);
+void delay_ms(__IO uint32_t time);
+
+#endif

src/bioloid_stm32.c

+#include "stm32f4xx.h"
+#include "system_stm32f4xx.h"
+
+#define SPIx                           SPI1
+#define SPIx_CLK                       RCC_APB2Periph_SPI1
+#define SPIx_CLK_INIT                  RCC_APB2PeriphClockCmd
+#define SPIx_IRQn                      SPI1_IRQn
+#define SPIx_IRQHANDLER                SPI1_IRQHandler
+
+#define SPIx_SCK_PIN                   GPIO_Pin_5
+#define SPIx_SCK_GPIO_PORT             GPIOA
+#define SPIx_SCK_GPIO_CLK              RCC_AHB1Periph_GPIOA
+#define SPIx_SCK_SOURCE                GPIO_PinSource5
+#define SPIx_SCK_AF                    GPIO_AF_SPI1
+
+#define SPIx_MISO_PIN                  GPIO_Pin_6
+#define SPIx_MISO_GPIO_PORT            GPIOA
+#define SPIx_MISO_GPIO_CLK             RCC_AHB1Periph_GPIOA
+#define SPIx_MISO_SOURCE               GPIO_PinSource6
+#define SPIx_MISO_AF                   GPIO_AF_SPI1
+
+#define SPIx_MOSI_PIN                  GPIO_Pin_7
+#define SPIx_MOSI_GPIO_PORT            GPIOA
+#define SPIx_MOSI_GPIO_CLK             RCC_AHB1Periph_GPIOA
+#define SPIx_MOSI_SOURCE               GPIO_PinSource7
+#define SPIx_MOSI_AF                   GPIO_AF_SPI1
+
+#define SPIx_NSS_PIN                   GPIO_Pin_4
+#define SPIx_NSS_GPIO_PORT             GPIOA
+#define SPIx_NSS_GPIO_CLK              RCC_AHB1Periph_GPIOA
+#define SPIx_NSS_SOURCE                GPIO_PinSource4
+#define SPIx_NSS_AF                    GPIO_AF_SPI1
+
+uint8_t rx_buffer[32];
+uint8_t rx_num;
+uint8_t tx_buffer[32];
+uint8_t tx_num;
+
+void SPIx_IRQHANDLER(void)
+{
+  /* SPI in Receiver mode */
+  if (SPI_I2S_GetITStatus(SPIx, SPI_I2S_IT_RXNE) == SET)
+  {
+    GPIO_ToggleBits(GPIOD, GPIO_Pin_12);
+    rx_buffer[rx_num]=SPI_I2S_ReceiveData(SPIx);
+    tx_buffer[rx_num]=rx_buffer[rx_num];
+    rx_num=(rx_num+1)%32;
+    if(rx_num==4)
+    {
+      SPI_I2S_ITConfig(SPIx, SPI_I2S_IT_TXE, ENABLE);
+      SPI_I2S_ITConfig(SPIx, SPI_I2S_IT_RXNE, DISABLE);
+      rx_num=0;
+    }
+  }
+  /* SPI in Tramitter mode */
+  if (SPI_I2S_GetITStatus(SPIx, SPI_I2S_IT_TXE) == SET)
+  {
+    GPIO_ToggleBits(GPIOD, GPIO_Pin_14);
+    SPI_I2S_SendData(SPIx, tx_buffer[tx_num]);
+    tx_num=(tx_num+1)%32;
+    if(tx_num==4)
+    {
+      SPI_I2S_ITConfig(SPIx, SPI_I2S_IT_TXE, DISABLE);
+      SPI_I2S_ITConfig(SPIx, SPI_I2S_IT_RXNE, ENABLE);
+      tx_num=0;
+    }
+  }
+}
+
+void SPI_Config(void)
+{
+  GPIO_InitTypeDef GPIO_InitStructure;
+  NVIC_InitTypeDef NVIC_InitStructure;
+  SPI_InitTypeDef  SPI_InitStructure;
+
+  /* Peripheral Clock Enable -------------------------------------------------*/
+  /* Enable the SPI clock */
+  SPIx_CLK_INIT(SPIx_CLK, ENABLE);
+
+  /* Enable GPIO clocks */
+  RCC_AHB1PeriphClockCmd(SPIx_SCK_GPIO_CLK | SPIx_MISO_GPIO_CLK | SPIx_MOSI_GPIO_CLK | SPIx_NSS_GPIO_CLK, ENABLE);
+
+  /* SPI GPIO Configuration --------------------------------------------------*/
+  /* GPIO Deinitialisation */
+  GPIO_DeInit(SPIx_SCK_GPIO_PORT);
+  GPIO_DeInit(SPIx_MISO_GPIO_PORT);
+  GPIO_DeInit(SPIx_MOSI_GPIO_PORT);
+  GPIO_DeInit(SPIx_NSS_GPIO_PORT);
+
+  /* Connect SPI pins to AF5 */
+  GPIO_PinAFConfig(SPIx_SCK_GPIO_PORT, SPIx_SCK_SOURCE, SPIx_SCK_AF);
+  GPIO_PinAFConfig(SPIx_MISO_GPIO_PORT, SPIx_MISO_SOURCE, SPIx_MISO_AF);
+  GPIO_PinAFConfig(SPIx_MOSI_GPIO_PORT, SPIx_MOSI_SOURCE, SPIx_MOSI_AF);
+  GPIO_PinAFConfig(SPIx_NSS_GPIO_PORT, SPIx_NSS_SOURCE, SPIx_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 = SPIx_SCK_PIN;
+  GPIO_Init(SPIx_SCK_GPIO_PORT, &GPIO_InitStructure);
+
+  /* SPI  MISO pin configuration */
+  GPIO_InitStructure.GPIO_Pin =  SPIx_MISO_PIN;
+  GPIO_Init(SPIx_MISO_GPIO_PORT, &GPIO_InitStructure);
+
+  /* SPI  MOSI pin configuration */
+  GPIO_InitStructure.GPIO_Pin =  SPIx_MOSI_PIN;
+  GPIO_Init(SPIx_MOSI_GPIO_PORT, &GPIO_InitStructure);
+
+  /* SPI  NSS pin configuration */
+  GPIO_InitStructure.GPIO_Pin =  SPIx_NSS_PIN;
+  GPIO_Init(SPIx_NSS_GPIO_PORT, &GPIO_InitStructure);
+
+  /* SPI configuration -------------------------------------------------------*/
+  SPI_I2S_DeInit(SPIx);
+  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;
+  /* Configure the Priority Group to 1 bit */
+  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
+
+  /* Configure the SPI interrupt priority */
+  NVIC_InitStructure.NVIC_IRQChannel = SPIx_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+
+  /* Slave board configuration */
+  /* Initializes the SPI communication */
+  SPI_InitStructure.SPI_Mode = SPI_Mode_Slave;
+  SPI_Init(SPIx, &SPI_InitStructure);
+
+  /* Enable the Rx buffer not empty interrupt */
+  SPI_I2S_ITConfig(SPIx, SPI_I2S_IT_RXNE, ENABLE);
+
+  /* Enable the Tx empty interrupt */
+  SPI_I2S_ITConfig(SPIx, SPI_I2S_IT_TXE, DISABLE);
+
+  /* Enable the SPI peripheral */
+  SPI_Cmd(SPIx, ENABLE);
+}
+
+int32_t main(void)
+{
+  GPIO_InitTypeDef  GPIO_InitStructure;
+  uint8_t data,i;
+
+  /* GPIOD Periph clock enable */
+  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
+
+  /* Configure PD12, PD13, PD14 and PD15 in output pushpull mode */
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13| GPIO_Pin_14| GPIO_Pin_15;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
+  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+  GPIO_Init(GPIOD, &GPIO_InitStructure);
+
+  rx_num=0;
+  tx_num=0;
+  for(i=0;i<32;i++)
+  {
+    tx_buffer[i]=0x00;
+    rx_buffer[i]=0x00;
+  }
+
+  /* configure the SPI interface */
+  SPI_Config();
+
+  while(1)                             /* main function does not return */
+  {
+    // polling example
+//    GPIO_ToggleBits(GPIOD, GPIO_Pin_13);
+//    while(!SPI_I2S_GetFlagStatus(SPIx, SPI_I2S_FLAG_RXNE));
+//    data=SPI_I2S_ReceiveData(SPIx);
+//    while(!SPI_I2S_GetFlagStatus(SPIx, SPI_I2S_FLAG_TXE));
+//    SPI_I2S_SendData(SPIx,data);
+//    GPIO_ToggleBits(GPIOD, GPIO_Pin_12);
+  }
+}
+

src/dynamixel_master_uart.c

+#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_0
+#define     USART_TX_EN_GPIO_PORT    GPIOA
+#define     USART_TX_EN_GPIO_CLK     RCC_AHB1Periph_GPIOA
+#define     USART_TX_EN_SOURCE       GPIO_PinSource0
+#define     USART_TX_EN_AF           GPIO_AF_OUT
+
+#define     USART_RX_EN_PIN          GPIO_Pin_1
+#define     USART_RX_EN_GPIO_PORT    GPIOA
+#define     USART_RX_EN_GPIO_CLK     RCC_AHB1Periph_GPIOA
+#define     USART_RX_EN_SOURCE       GPIO_PinSource1
+#define     USART_RX_EN_AF           GPIO_AF_OUT
+
+#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_RX_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;
+}
+

src/time.c

+#include "time.h"
+
+static __IO uint32_t timing_delay;
+
+/*******************************************************************************
+* Function Name  : SysTickHandler
+* Description    : This function handles SysTick Handler.
+* Input          : None
+* Output         : None
+* Return         : None
+*******************************************************************************/
+void SysTick_Handler(void)
+{
+  if(timing_delay!=0)
+    timing_delay--;
+}
+
+// public functions
+void time_init(void)
+{
+  // set the time base to 1ms
+  SysTick_Config(SystemCoreClock / 1000);
+}
+
+void delay_ms(__IO uint32_t time)
+{
+  timing_delay=time;
+
+  while(timing_delay!=0);
+}
+
+