Solved a problem with the motion timing.

Makefile

@@ -2,23 +2,7 @@
 # modified by zerom for WinARM 8/2010
 
 PROJECT_NAME=bioloid_firmware
-#TARGET_FILES=$(wildcard src/*.c)
-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/eeprom.c
-TARGET_FILES+=src/ram.c
-TARGET_FILES+=src/adc_dma.c
-TARGET_FILES+=src/zigbee.c
-TARGET_FILES+=src/bioloid_time.c
-TARGET_FILES+=src/bioloid_dyn_slave.c
-TARGET_FILES+=src/bioloid_dyn_master_sensors.c
-TARGET_FILES+=src/bioloid_dyn_master_servos.c
-TARGET_FILES+=src/motion_pages.c
-TARGET_FILES+=src/motion_manager.c
-TARGET_FILES+=src/action.c
-TARGET_FILES+=src/bioloid_gyro.c
+TARGET_FILES=$(wildcard src/*.c)
 TARGET_PROCESSOR=STM32F407VG
 
 HAL_PATH=../../STM32_processor/hal/f4

src/bioloid_stm32.c

@@ -27,7 +27,7 @@ int32_t main(void)
   bioloid_time_init();
   /* initialize zigbee module */
   zigbee_init();
-  zigbee_enable_power();
+//  zigbee_enable_power();
   /* initialize the dynamixel slave interface */
   bioloid_dyn_slave_init();
   bioloid_dyn_slave_start();

src/eeprom.c

@@ -52,7 +52,7 @@
 #define    DEFAULT_DEVICE_ID                0x0002
 #define    DEFAULT_BAUDRATE                 0x0001
 #define    DEFAULT_RETURN_DELAY             0x0000
-#define    DEFAULT_MM_PERIOD                0x01FF
+#define    DEFAULT_MM_PERIOD                0x1E78
 #define    DEFAULT_BAL_KNEE_GAIN            0x04BE // 1/54 in fixed point format 0|16
 #define    DEFAULT_BAL_ANKLE_ROLL_GAIN      0x0CCD // 1/20
 #define    DEFAULT_BAL_ANKLE_PITCH_GAIN     0x0E39 // 1/18

src/motion_manager.c

@@ -147,7 +147,7 @@ void MANAGER_TIMER_IRQHandler(void)
     {
       __HAL_TIM_CLEAR_IT(&MANAGER_TIM_Handle, TIM_IT_CC1);
       capture = HAL_TIM_ReadCapturedValue(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
-      __HAL_TIM_SET_COMPARE(&MANAGER_TIM_Handle, TIM_CHANNEL_1, (capture + manager_motion_period));
+      __HAL_TIM_SET_COMPARE(&MANAGER_TIM_Handle, TIM_CHANNEL_1, (capture + manager_motion_period_us));
       // call the action process
       action_process();
       // call the joint motion process
@@ -185,6 +185,10 @@ void manager_init(uint16_t period_us)
   // detect the servos connected 
   dyn_master_scan(&bioloid_dyn_master_servos,&num,servo_ids); 
   ram_data[BIOLOID_MM_NUM_SERVOS]=num;
+  if(num>0)
+    gpio_set_led(TXD_LED);
+  if(num==18)
+    gpio_set_led(RXD_LED);
   manager_num_servos=0;
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
@@ -335,7 +339,7 @@ uint16_t manager_get_period_us(void)
 
 void manager_set_period(uint16_t period_us)
 {
-  manager_motion_period=(period_us*1000000)>>16;
+  manager_motion_period=(period_us<<16)/1000000;
   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;
@@ -350,7 +354,7 @@ inline void manager_enable(void)
   TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
   TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
   capture = HAL_TIM_ReadCapturedValue(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
-  TIM_OCInitStructure.Pulse = capture+manager_motion_period;
+  TIM_OCInitStructure.Pulse = capture+manager_motion_period_us;
   HAL_TIM_OC_ConfigChannel(&MANAGER_TIM_Handle, &TIM_OCInitStructure,TIM_CHANNEL_1);
   HAL_TIM_OC_Start_IT(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
   ram_data[BIOLOID_MM_CNTRL]|=MANAGER_ENABLE;