From 496dbbfa2fea6901dd5bade1023daac4c26141cd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Sat, 6 Feb 2016 17:06:34 +0000
Subject: [PATCH] Solved a problem with the motion timing.
---
Makefile | 18 +-----------------
src/bioloid_stm32.c | 2 +-
src/eeprom.c | 2 +-
src/motion_manager.c | 10 +++++++---
4 files changed, 10 insertions(+), 22 deletions(-)
diff --git a/Makefile b/Makefile
index ec863f8..19efbc3 100755
--- a/Makefile
+++ b/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
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index b34b9d4..7f63859 100644
--- a/src/bioloid_stm32.c
+++ b/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();
diff --git a/src/eeprom.c b/src/eeprom.c
index 68155b9..2f1c990 100755
--- a/src/eeprom.c
+++ b/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
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 8a172e0..2ea42c2 100644
--- a/src/motion_manager.c
+++ b/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;
--
GitLab