Skip to content
Snippets Groups Projects
Commit 2fc21611 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Used a global variable for the resulting angles in the action module.

parent a4ec99c7
No related branches found
No related tags found
No related merge requests found
#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;
......
......@@ -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);
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment