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
......@@ -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.
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment