From 2fc21611ce573b0a7c0696a5b3dbbd58a087319f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Tue, 4 Aug 2015 20:29:52 +0000
Subject: [PATCH] Used a global variable for the resulting angles in the action
module.
---
src/action.c | 49 +++++++++++++++++++++++----------------------
src/bioloid_stm32.c | 32 +++++++++++++++++++++++++++++
2 files changed, 57 insertions(+), 24 deletions(-)
diff --git a/src/action.c b/src/action.c
index 365f41e..4089fdd 100644
--- a/src/action.c
+++ b/src/action.c
@@ -1,4 +1,6 @@
#include "action.h"
+#include "motion_pages.h"
+#include "motion_manager.h"
#include "ram.h"
#define SPEED_BASE_SCHEDULE 0x00
@@ -12,7 +14,6 @@ TPage action_current_page;
uint8_t action_current_step_index;
// angle variables
int64_t action_moving_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-int64_t action_current_angles[PAGE_MAX_NUM_SERVOS];
int64_t action_start_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
// speed variables
int64_t action_start_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
@@ -135,10 +136,10 @@ void action_load_next_step(void)
{
angle=action_current_page.steps[action_current_step_index].position[i];
if(angle==0x5A00)// bigger than 180
- target_angles=action_current_angles[i];
+ target_angles=current_angles[i];
else
target_angles=angle<<9;
- action_moving_angles[i]=target_angles-action_current_angles[i];
+ action_moving_angles[i]=target_angles-current_angles[i];
if(action_end)
next_angles=target_angles;
else
@@ -153,8 +154,8 @@ void action_load_next_step(void)
next_angles=next_angle<<9;
}
// check changes in direction
- if(((action_current_angles[i] < target_angles) && (target_angles < next_angles)) ||
- ((action_current_angles[i] > target_angles) && (target_angles > next_angles)))
+ if(((current_angles[i] < target_angles) && (target_angles < next_angles)) ||
+ ((current_angles[i] > target_angles) && (target_angles > next_angles)))
dir_change=0x00;
else
dir_change=0x01;
@@ -224,7 +225,7 @@ void action_init(uint16_t period)
{
// angle variables
action_moving_angles[i]=0;// fixed point 48|16 format
- action_start_angles[i]=action_current_angles[i];
+ action_start_angles[i]=current_angles[i];
// speed variables
action_start_speed[i]=0;// fixed point 48|16 format
action_main_speed[i]=0;// fixed point 48|16 format
@@ -281,7 +282,7 @@ void action_start_page(void)
uint8_t i;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
- action_start_angles[i]=action_current_angles[i];
+ action_start_angles[i]=current_angles[i];
action_stop=0x00;
action_current_time=0;
action_section_time=0;
@@ -326,7 +327,7 @@ void action_process(void)
delta_speed=(action_main_speed[i]-action_start_speed[i]);
current_speed[i]=action_start_speed[i]+delta_speed;
accel_angles[i]=(action_start_speed[i]*action_section_time+((delta_speed*action_section_time)>>1))>>16;
- action_current_angles[i]=action_start_angles[i]+accel_angles[i];
+ current_angles[i]=action_start_angles[i]+accel_angles[i];
/* update of the state */
if(!action_zero_speed_finish[i])
{
@@ -334,15 +335,15 @@ void action_process(void)
main_angles[i]=0;
else
main_angles[i]=((action_moving_angles[i]-accel_angles[i])*(action_step_time-(action_pre_time<<1)))/(action_step_time-action_pre_time);
- action_start_angles[i]=action_current_angles[i];
+ action_start_angles[i]=current_angles[i];
}
else
{
main_angles[i]=action_moving_angles[i]-accel_angles[i]-(((action_main_speed[i]*action_pre_time)>>1)>>16);
- action_start_angles[i]=action_current_angles[i];
+ action_start_angles[i]=current_angles[i];
}
/* the first step of the main section */
- action_current_angles[i]=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1));
+ current_angles[i]=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1));
current_speed[i]=action_main_speed[i];
}
action_current_time=action_current_time-action_section_time;
@@ -356,7 +357,7 @@ void action_process(void)
delta_speed=((action_main_speed[i]-action_start_speed[i])*action_current_time)/action_section_time;
current_speed[i]=action_start_speed[i]+delta_speed;
accel_angles[i]=((action_start_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16;
- action_current_angles[i]=action_start_angles[i]+accel_angles[i];
+ current_angles[i]=action_start_angles[i]+accel_angles[i];
}
state=ACTION_PRE;
}
@@ -366,21 +367,21 @@ void action_process(void)
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
/* last segment of the main section */
- action_current_angles[i]=action_start_angles[i]+main_angles[i];
+ current_angles[i]=action_start_angles[i]+main_angles[i];
current_speed[i]=action_main_speed[i];
/* update state */
- action_start_angles[i]=action_current_angles[i];
+ action_start_angles[i]=current_angles[i];
main_angles[i]=action_moving_angles[i]-main_angles[i]-accel_angles[i];
/* first segment of the post section */
if(action_zero_speed_finish[i])
{
delta_speed=((0.0-action_main_speed[i])*(action_current_time-action_section_time))/action_pre_time;
current_speed[i]=action_main_speed[i]+delta_speed;
- action_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
+ current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
}
else
{
- action_current_angles[i]=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time);
+ current_angles[i]=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time);
current_speed[i]=action_main_speed[i];
}
}
@@ -392,7 +393,7 @@ void action_process(void)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- action_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
+ current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
current_speed[i]=action_main_speed[i];
}
state=ACTION_MAIN;
@@ -407,15 +408,15 @@ void action_process(void)
{
delta_speed=-action_main_speed[i];
current_speed[i]=action_main_speed[i]+delta_speed;
- action_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16);
+ current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16);
}
else
{
- action_current_angles[i]=action_start_angles[i]+main_angles[i];
+ current_angles[i]=action_start_angles[i]+main_angles[i];
current_speed[i]=action_main_speed[i];
}
/* update state */
- action_start_angles[i]=action_current_angles[i];
+ action_start_angles[i]=current_angles[i];
action_start_speed[i]=current_speed[i];
}
/* load the next step */
@@ -437,7 +438,7 @@ void action_process(void)
delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time;
current_speed[i]=action_start_speed[i]+delta_speed;
accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
- action_current_angles[i]=action_start_angles[i]+accel_angles[i];
+ current_angles[i]=action_start_angles[i]+accel_angles[i];
}
action_current_time=action_current_time-action_section_time;
action_section_time=action_pre_time;
@@ -459,11 +460,11 @@ void action_process(void)
{
delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time;
current_speed[i]=action_main_speed[i]+delta_speed;
- action_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16);
+ current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16);
}
else
{
- action_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
+ current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
current_speed[i]=action_main_speed[i];
}
}
@@ -491,7 +492,7 @@ void action_process(void)
delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time;
current_speed[i]=action_start_speed[i]+delta_speed;
accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
- action_current_angles[i]=action_start_angles[i]+accel_angles[i];
+ current_angles[i]=action_start_angles[i]+accel_angles[i];
}
action_current_time=action_current_time-action_section_time;
action_section_time=action_pre_time;
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index db35b7b..03f4ab7 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -3,8 +3,40 @@
int32_t main(void)
{
+// uint16_t eeprom_data,period;
+
+ /* initialize EEPROM */
+// EE_Init();
+ // initialize the Dynamixel RAM memory space
+// ram_init();
+ /* initialize the 1ms system timer */
+// time_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();
while(1);
}
--
GitLab