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

Solved a problem in the timing of the action process.

Changed the stored value of speed ratio in the motion pages.
parent 4426c4ba
No related branches found
No related tags found
No related merge requests found
......@@ -19,6 +19,8 @@ uint8_t action_load_page(uint8_t page_id);
void action_start_page(void);
void action_stop_page(void);
uint8_t action_is_running(void);
uint8_t action_get_current_page(void);
uint8_t action_get_current_step(void);
void action_enable_int(void);
void action_disable_int(void);
uint8_t action_is_int_enabled(void);
......
......@@ -3,6 +3,7 @@
#include "motion_manager.h"
#include "ram.h"
#include "bioloid_dyn_slave.h"
#include <stdio.h>
#define SPEED_BASE_SCHEDULE 0x00
#define TIME_BASE_SCHEDULE 0x0A
......@@ -13,6 +14,7 @@ typedef enum {ACTION_PRE,ACTION_MAIN,ACTION_POST,ACTION_PAUSE} action_states;
TPage action_next_page;
TPage action_current_page;
uint8_t action_current_step_index;
uint8_t action_current_page_index;
// angle variables
int64_t action_moving_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
int64_t action_start_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
......@@ -39,7 +41,6 @@ int16_t action_period_us;
void action_load_next_step(void)
{
// page and step information
static uint8_t current_index=0;
static int8_t num_repetitions=0;
// angle variables
int16_t next_angle;// information from the flash memory (fixed point 9|7 format)
......@@ -70,7 +71,7 @@ void action_load_next_step(void)
{
num_repetitions--;
if(num_repetitions>0)
action_next_index=current_index;
action_next_index=action_current_page_index;
else
action_next_index=action_current_page.header.next;
}
......@@ -78,7 +79,7 @@ void action_load_next_step(void)
action_end=0x01;
else
{
if(action_next_index!=current_index)
if(action_next_index!=action_current_page_index)
{
pages_get_page(action_next_index,&action_next_page);
ram_data[BIOLOID_ACTION_PAGE]=action_next_index;
......@@ -90,10 +91,10 @@ void action_load_next_step(void)
}
}
pages_copy_page(&action_next_page,&action_current_page);// make the next page active
if(current_index!=action_next_index)
if(action_current_page_index!=action_next_index)
num_repetitions=action_current_page.header.repeat;
action_current_step_index=0;
current_index=action_next_index;
action_current_page_index=action_next_index;
}
else if(action_current_step_index==action_current_page.header.stepnum-1)// it is the last step
{
......@@ -103,7 +104,7 @@ void action_load_next_step(void)
{
num_repetitions--;
if(num_repetitions>0)
action_next_index=current_index;
action_next_index=action_current_page_index;
else
action_next_index=action_current_page.header.next;
}
......@@ -111,7 +112,7 @@ void action_load_next_step(void)
action_end=0x01;
else
{
if(action_next_index!=current_index)
if(action_next_index!=action_current_page_index)
{
pages_get_page(action_next_index,&action_next_page);
ram_data[BIOLOID_ACTION_PAGE]=action_next_index;
......@@ -123,8 +124,12 @@ void action_load_next_step(void)
}
}
// compute trajectory
action_pause_time=((action_current_page.steps[action_current_step_index].pause<<14)*action_current_page.header.speed);
action_step_time=((action_current_page.steps[action_current_step_index].time<<14)/action_current_page.header.speed);
// action_pause_time=((action_current_page.steps[action_current_step_index].pause<<4)*action_current_page.header.speed);
action_pause_time=(action_current_page.steps[action_current_step_index].pause*action_current_page.header.speed)>>5;
action_pause_time=action_pause_time<<9;
// action_step_time=((action_current_page.steps[action_current_step_index].time<<4)*action_current_page.header.speed);
action_step_time=(action_current_page.steps[action_current_step_index].time*action_current_page.header.speed)>>5;
action_step_time=action_step_time<<9;
if(action_step_time<action_period)
action_step_time=action_period;// 0.078 in 16|16 format
max_angle=0;
......@@ -252,6 +257,8 @@ void action_init(uint16_t period_us)
// clear the contents of the next page
pages_clear_page(&action_next_page);
pages_clear_page(&action_current_page);
action_current_page_index=0;
action_current_step_index=-1;
// control variables
action_end=0x00;
action_stop=0x00;
......@@ -323,6 +330,16 @@ uint8_t action_is_running(void)
return action_running;
}
uint8_t action_get_current_page(void)
{
return action_current_page_index;
}
uint8_t action_get_current_step(void)
{
return action_current_step_index;
}
void action_enable_int(void)
{
ram_data[BIOLOID_ACTION_CNTRL]|=ACTION_INT_EN;
......@@ -555,7 +572,9 @@ void action_process(void)
}
}
else// pause section still active
{
state=ACTION_PAUSE;
}
break;
default: break;
}
......
This diff is collapsed.
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