diff --git a/Makefile b/Makefile
index 2aef96b8b25cc361a0faa761004b3dd138c45b44..5933af0bc649d05ee2b5115417a501e47bffdca5 100755
--- a/Makefile
+++ b/Makefile
@@ -7,6 +7,7 @@ TARGET_FILES=src/bioloid_stm32.c
TARGET_FILES+=src/system_stm32f4xx.c
TARGET_FILES+=src/stm32f4xx_hal_msp.c
TARGET_FILES+=src/gpio.c
+TARGET_FILES+=src/bioloid_time.c
TARGET_PROCESSOR=STM32F407VG
HAL_PATH=../../STM32_processor/hal/f4
diff --git a/include/bioloid_time.h b/include/bioloid_time.h
new file mode 100755
index 0000000000000000000000000000000000000000..13ba01bbe4fd47210059324744657d0fd5f9314f
--- /dev/null
+++ b/include/bioloid_time.h
@@ -0,0 +1,18 @@
+#ifndef _BIOLOID_TIME_H
+#define _BIOLOID_TIME_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f4xx_hal.h"
+
+void bioloid_time_init(void);
+unsigned long long int bioloid_time_get_counts(void);
+inline unsigned int bioloid_time_get_counts_per_us(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/include/stm32_time.h b/include/stm32_time.h
deleted file mode 100755
index 6b47829c0ff8debc66edf1391df3f3729ce898ef..0000000000000000000000000000000000000000
--- a/include/stm32_time.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#ifndef _STM32_TIME_H
-#define _STM32_TIME_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f4xx.h"
-#include "system_stm32f4xx.h"
-
-void time_init(void);
-void delay_ms(__IO uint32_t time);
-void delay_us(uint32_t us);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index 03f4ab761bccbdff692b7ba22a5520c65afba00f..471649eef3dd21ee4388e7a7fd360b72f13be2fb 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -1,43 +1,26 @@
#include "stm32f4xx_hal.h"
#include "gpio.h"
+#include "bioloid_time.h"
+#include "stm32_time.h"
int32_t main(void)
{
-// uint16_t eeprom_data,period;
+ TTime timer;
+ int i;
- /* initialize EEPROM */
-// EE_Init();
- // initialize the Dynamixel RAM memory space
-// ram_init();
- /* initialize the 1ms system timer */
-// time_init();
+ HAL_Init();
/* initialize the gpio */
gpio_init();
- /* initialize the dynamixel master interface */
-// dyn_master_init();
-// dyn_master_set_timeout(20);
-// /* initialize the dynamixel slave interface*/
-// dyn_slave_init();
-// EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data);
-// dyn_slave_set_address((uint8_t)eeprom_data);
-// EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data);
-// dyn_slave_set_return_delay((uint8_t)eeprom_data);
-// EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data);
-// dyn_slave_set_return_level((uint8_t)eeprom_data);
- /* initialize the IMU */
-// imu_init();
- // initialize the Analog to digital converter
-// adc_init();
- // initialize motion manager
-// EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
-// period=eeprom_data&0x00FF;
-// EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data);
-// period+=((eeprom_data&0x00FF)<<8);
-// manager_init(period);
- /* initialize communications module */
-// comm_init();
-// comm_start();
+ /* initialize the time module */
+ bioloid_time_init();
- while(1);
+ time_init(&timer, bioloid_time_get_counts_per_us(), bioloid_time_get_counts);
+
+ while(1)
+ {
+ for(i=0;i<10000;i++)
+ time_delay_us(&timer,100);
+ gpio_toggle_led(USER1_LED);
+ }
}
diff --git a/src/bioloid_time.c b/src/bioloid_time.c
new file mode 100644
index 0000000000000000000000000000000000000000..2473255b92b8d4fdb6dc01c087a847f0c59b0a90
--- /dev/null
+++ b/src/bioloid_time.c
@@ -0,0 +1,62 @@
+#include "bioloid_time.h"
+
+TIM_HandleTypeDef us_timer_Handle;
+unsigned long long int timer_counts;
+unsigned short int timer_counts_per_us;
+
+// interrupt service routines
+void SysTick_Handler(void)
+{
+ HAL_IncTick();
+ HAL_SYSTICK_IRQHandler();
+}
+
+void TIM6_DAC_IRQHandler(void)
+{
+ /* TIM Update event */
+ if(__HAL_TIM_GET_FLAG(&us_timer_Handle, TIM_FLAG_UPDATE) != RESET)
+ {
+ if(__HAL_TIM_GET_IT_SOURCE(&us_timer_Handle, TIM_IT_UPDATE) !=RESET)
+ {
+ __HAL_TIM_CLEAR_IT(&us_timer_Handle, TIM_IT_UPDATE);
+ timer_counts++;
+ }
+ }
+}
+
+// public functions
+void bioloid_time_init(void)
+{
+ TIM_MasterConfigTypeDef sMasterConfig;
+
+ /* initialize internal variables */
+ timer_counts=0;
+ timer_counts_per_us=(SystemCoreClock/2000000)+1;
+
+ /* configure timer */
+ __TIM6_CLK_ENABLE();
+ us_timer_Handle.Instance = TIM6;
+ us_timer_Handle.Init.Prescaler = 0;
+ us_timer_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+ us_timer_Handle.Init.Period = 0xFFFF;
+ HAL_TIM_Base_Init(&us_timer_Handle);
+
+ sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+ sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+ HAL_TIMEx_MasterConfigSynchronization(&us_timer_Handle, &sMasterConfig);
+
+ /* configure timer interrupts */
+ HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 0, 0);
+ HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);
+ HAL_TIM_Base_Start_IT(&us_timer_Handle);
+}
+
+unsigned long long int bioloid_time_get_counts(void)
+{
+ return (timer_counts<<16)+__HAL_TIM_GetCounter(&us_timer_Handle);
+}
+
+inline unsigned int bioloid_time_get_counts_per_us(void)
+{
+ return timer_counts_per_us;
+}
diff --git a/src/gpio.c b/src/gpio.c
index d5da9fc35178958c4a94afc41e4eb9099ff2efcc..7e541ea51753318c560f464a7117119ad0dca99b 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -241,7 +241,7 @@ void gpio_init(void)
ENABLE_GPO_TIMER_CLK;
GPO_TIM_Handle.Instance=GPO_TIMER;
- GPO_TIM_Handle.Init.Period = 0xFFFF;
+ GPO_TIM_Handle.Init.Period = 0xFFFFFFFF;
GPO_TIM_Handle.Init.Prescaler = 42000;
GPO_TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV2;
GPO_TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
diff --git a/src/stm32_time.c b/src/stm32_time.c
deleted file mode 100644
index f2f8044ae17fb7cd4803453dfc07431fdd5c568c..0000000000000000000000000000000000000000
--- a/src/stm32_time.c
+++ /dev/null
@@ -1,60 +0,0 @@
-#include "stm32_time.h"
-
-static __IO uint32_t timing_delay;
-uint32_t clocks_per_us;
-
-/*******************************************************************************
-* 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)
-{
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
-
- // set the time base to 1ms
- SysTick_Config(SystemCoreClock / 1000);
- NVIC_SetPriority(SysTick_IRQn,0);
-
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE);
-
- TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
- TIM_TimeBaseStructure.TIM_Prescaler = 0;
- TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseStructure.TIM_RepetitionCounter=0x0000;
- TIM_TimeBaseInit(TIM1,&TIM_TimeBaseStructure);
- TIM_SetCounter(TIM1,0);
-
- clocks_per_us=(SystemCoreClock/1000000)+1;
-}
-
-void delay_ms(__IO uint32_t time)
-{
- timing_delay=time;
-
- while(timing_delay!=0);
-}
-
-void delay_us(uint32_t us) // microseconds
-{
- uint32_t clocks,current;
-
- clocks=clocks_per_us*us;
- TIM_SetCounter(TIM1,0);
- TIM_Cmd(TIM1, ENABLE);
- do{
- current=TIM_GetCounter(TIM1);
- }while(current<clocks);
- TIM_Cmd(TIM1, DISABLE);
-}
-
diff --git a/src/stm32f4xx_hal_msp.c b/src/stm32f4xx_hal_msp.c
index 97d79d839202a0afaa3e6bdad3815a6ab921cfaf..e40b1342b7c2ffdf5a7b7312be1c19fc310ca291 100755
--- a/src/stm32f4xx_hal_msp.c
+++ b/src/stm32f4xx_hal_msp.c
@@ -98,6 +98,9 @@ void HAL_MspInit(void)
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
+
+ /* update the SystemCoreClock variable*/
+ SystemCoreClockUpdate();
}
/**
diff --git a/src/system_stm32f4xx.c b/src/system_stm32f4xx.c
index 318a217adf28a83a5e60427b9e31da9c0061f43f..45380618e5863c0aa51ba2b7143b6bb1451b9c62 100644
--- a/src/system_stm32f4xx.c
+++ b/src/system_stm32f4xx.c
@@ -135,7 +135,7 @@
is no need to call the 2 first functions listed above, since SystemCoreClock
variable is updated automatically.
*/
- uint32_t SystemCoreClock = 16000000;
+ uint32_t SystemCoreClock = 168000000;
__IO const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
/**