diff --git a/dynamixel_manager/include/modules/action.h b/dynamixel_manager/include/modules/action.h
index 506b11d5ab837a58f7d08b45041c49cf0994aeb2..309c50ccca35e54327abf3c241ff714a3874ff73 100755
--- a/dynamixel_manager/include/modules/action.h
+++ b/dynamixel_manager/include/modules/action.h
@@ -7,16 +7,62 @@ extern "C" {
#include "motion_pages.h"
#include "motion_module.h"
+#include "motion_pages.h"
+#include "memory.h"
+
+typedef enum {ACTION_PRE,ACTION_MAIN,ACTION_POST,ACTION_PAUSE} action_states;
+
+typedef struct{
+ TMotionModule mmodule;
+ TMemory *memory;
+ TMemModule mem_module;
+ unsigned short int ram_base_address;
+ unsigned short int eeprom_base_address;
+ TPage next_page;
+ TPage current_page;
+ unsigned char current_step_index;
+ unsigned char current_page_index;
+ // angle variables
+ long long int moving_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ long long int start_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ // speed variables
+ long long int start_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ long long int main_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ // control variables
+ unsigned char end;
+ unsigned char stop;
+ unsigned char zero_speed_finish[PAGE_MAX_NUM_SERVOS];
+ unsigned char next_index;
+ unsigned char running;
+ // time variables (in time units (7.8 ms each time unit))
+ long long int total_time;// fixed point 48|16 format
+ long long int pre_time;// fixed point 48|16 format
+ long long int main_time;// fixed point 48|16 format
+ long long int step_time;// fixed point 48|16 format
+ long long int pause_time;// fixed point 48|16 format
+ long long int current_time;// fixed point 48|16 format
+ long long int section_time;// fixed point 48|16 format
+ long long int period;
+ // angle variables
+ long long int accel_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ long long int main_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ // speed variables (in controller units)
+ long long int current_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ long long int delta_speed;
+ char num_repetitions;
+ // control variables
+ action_states state;
+}TActionMModule;
// public functions
-void action_init(void);
-TMotionModule *action_get_module(void);
-unsigned char action_load_page(unsigned char page_id);
-void action_start_page(void);
-void action_stop_page(void);
-unsigned char action_is_running(void);
-unsigned char action_get_current_page(void);
-unsigned char action_get_current_step(void);
+unsigned char action_init(TActionMModule *action,TMemory *memory,unsigned short int ram_base_address);
+TMotionModule *action_get_module(TActionMModule *action);
+unsigned char action_load_page(TActionMModule *action,unsigned char page_id);
+void action_start_page(TActionMModule *action);
+void action_stop_page(TActionMModule *action);
+unsigned char action_is_running(TActionMModule *action);
+unsigned char action_get_current_page(TActionMModule *action);
+unsigned char action_get_current_step(TActionMModule *action);
#ifdef __cplusplus
}
diff --git a/dynamixel_manager/include/modules/action_mm_registers.h b/dynamixel_manager/include/modules/action_mm_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..3dbe77b082bcdf80922d7e85fb3fabe877c2f4c7
--- /dev/null
+++ b/dynamixel_manager/include/modules/action_mm_registers.h
@@ -0,0 +1,10 @@
+#ifndef _ACTION_MM_REGISTERS_H
+#define _ACTION_MM_REGISTERS_H
+
+#define RAM_ACTION_MM_LENGTH 2
+
+#define ACTION_MM_PAGE_OFFSET 0
+#define ACTION_MM_CNTRL_OFFSET 1
+
+#endif
+
diff --git a/dynamixel_manager/src/modules/action.c b/dynamixel_manager/src/modules/action.c
index da86b52a7eef5b178ed98bafd7d6adfdcbeda15c..24eff30c71aba47ebf8ac33f3fb018a36e35132c 100755
--- a/dynamixel_manager/src/modules/action.c
+++ b/dynamixel_manager/src/modules/action.c
@@ -1,43 +1,23 @@
#include "action.h"
-#include "motion_pages.h"
+#include "action_mm_registers.h"
+#include "ram.h"
#define SPEED_BASE_SCHEDULE 0x00
#define TIME_BASE_SCHEDULE 0x0A
-// private variables
-typedef enum {ACTION_PRE,ACTION_MAIN,ACTION_POST,ACTION_PAUSE} action_states;
-TMotionModule action_module;
+// private functions
+void action_write_cmd(TActionMModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_write_table(module->memory,address,length,data);
+}
-TPage action_next_page;
-TPage action_current_page;
-unsigned char action_current_step_index;
-unsigned char action_current_page_index;
-// angle variables
-long long int action_moving_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-long long int action_start_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-// speed variables
-long long int action_start_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-long long int action_main_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-// control variables
-unsigned char action_end,action_stop;
-unsigned char action_zero_speed_finish[PAGE_MAX_NUM_SERVOS];
-unsigned char action_next_index;
-unsigned char action_running;
-// time variables (in time units (7.8 ms each time unit))
-long long int action_total_time;// fixed point 48|16 format
-long long int action_pre_time;// fixed point 48|16 format
-long long int action_main_time;// fixed point 48|16 format
-long long int action_step_time;// fixed point 48|16 format
-long long int action_pause_time;// fixed point 48|16 format
-long long int action_current_time;// fixed point 48|16 format
-long long int action_section_time;// fixed point 48|16 format
-long long int action_period;
+void action_read_cmd(TActionMModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_read_table(module->memory,address,length,data);
+}
-// private functions
-void action_load_next_step(void)
+void action_load_next_step(TActionMModule *action)
{
- // page and step information
- static char num_repetitions=0;
// angle variables
short int next_angle;// information from the flash memory (fixed point 9|7 format)
long long int max_angle;// fixed point format 48|16 format
@@ -56,178 +36,180 @@ void action_load_next_step(void)
// control information
unsigned char i=0;
- action_current_step_index++;
- if(action_current_step_index>=action_current_page.header.stepnum)// the last step has been executed
+ action->current_step_index++;
+ if(action->current_step_index>=action->current_page.header.stepnum)// the last step has been executed
{
- if(action_current_page.header.stepnum==1)
+ if(action->current_page.header.stepnum==1)
{
- if(action_stop)
- action_next_index=action_current_page.header.exit;
+ if(action->stop)
+ action->next_index=action->current_page.header.exit;
else
{
- num_repetitions--;
- if(num_repetitions>0)
- action_next_index=action_current_page_index;
+ action->num_repetitions--;
+ if(action->num_repetitions>0)
+ action->next_index=action->current_page_index;
else
- action_next_index=action_current_page.header.next;
+ action->next_index=action->current_page.header.next;
}
- if(action_next_index==0)
- action_end=0x01;
+ if(action->next_index==0)
+ action->end=0x01;
else
{
- if(action_next_index!=action_current_page_index)
+ if(action->next_index!=action->current_page_index)
{
- pages_get_page(action_next_index,&action_next_page);
- if(!pages_check_checksum(&action_next_page))
- action_end=0x01;
+ pages_get_page(action->next_index,&action->next_page);
+ if(!pages_check_checksum(&action->next_page))
+ action->end=0x01;
}
- if(action_next_page.header.repeat==0 || action_next_page.header.stepnum==0)
- action_end=0x01;
+ if(action->next_page.header.repeat==0 || action->next_page.header.stepnum==0)
+ action->end=0x01;
}
}
- pages_copy_page(&action_next_page,&action_current_page);// make the next page active
- if(action_current_page_index!=action_next_index)
- num_repetitions=action_current_page.header.repeat;
- action_current_step_index=0;
- action_current_page_index=action_next_index;
+ pages_copy_page(&action->next_page,&action->current_page);// make the next page active
+ if(action->current_page_index!=action->next_index)
+ action->num_repetitions=action->current_page.header.repeat;
+ action->current_step_index=0;
+ action->current_page_index=action->next_index;
}
- else if(action_current_step_index==action_current_page.header.stepnum-1)// it is the last step
+ else if(action->current_step_index==action->current_page.header.stepnum-1)// it is the last step
{
- if(action_stop)
- action_next_index=action_current_page.header.exit;
+ if(action->stop)
+ action->next_index=action->current_page.header.exit;
else
{
- num_repetitions--;
- if(num_repetitions>0)
- action_next_index=action_current_page_index;
+ action->num_repetitions--;
+ if(action->num_repetitions>0)
+ action->next_index=action->current_page_index;
else
- action_next_index=action_current_page.header.next;
+ action->next_index=action->current_page.header.next;
}
- if(action_next_index==0)
- action_end=0x01;
+ if(action->next_index==0)
+ action->end=0x01;
else
{
- if(action_next_index!=action_current_page_index)
+ if(action->next_index!=action->current_page_index)
{
- pages_get_page(action_next_index,&action_next_page);
- if(!pages_check_checksum(&action_next_page))
- action_end=0x01;
+ pages_get_page(action->next_index,&action->next_page);
+ if(!pages_check_checksum(&action->next_page))
+ action->end=0x01;
}
- if(action_next_page.header.repeat==0 || action_next_page.header.stepnum==0)
- action_end=0x01;
+ if(action->next_page.header.repeat==0 || action->next_page.header.stepnum==0)
+ action->end=0x01;
}
}
// compute trajectory
- 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*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
+ 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*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;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- angle=action_current_page.steps[action_current_step_index].position[i];
+ angle=action->current_page.steps[action->current_step_index].position[i];
if(angle==0x5A00)// bigger than 180
- target_angles=action_module.manager->servo_values[i].target_angle;
+ target_angles=action->mmodule.manager->servo_values[i].target_angle;
else
target_angles=angle<<9;
- action_moving_angles[i]=target_angles-action_module.manager->servo_values[i].target_angle;
- if(action_end)
+ action->moving_angles[i]=target_angles-action->mmodule.manager->servo_values[i].target_angle;
+ if(action->end)
next_angles=target_angles;
else
{
- if(action_current_step_index==action_current_page.header.stepnum-1)
- next_angle=action_next_page.steps[0].position[i];
+ if(action->current_step_index==action->current_page.header.stepnum-1)
+ next_angle=action->next_page.steps[0].position[i];
else
- next_angle=action_current_page.steps[action_current_step_index+1].position[i];
+ next_angle=action->current_page.steps[action->current_step_index+1].position[i];
if(next_angle==0x5A00)
next_angles=target_angles;
else
next_angles=next_angle<<9;
}
// check changes in direction
- if(((action_module.manager->servo_values[i].target_angle < target_angles) && (target_angles < next_angles)) ||
- ((action_module.manager->servo_values[i].target_angle > target_angles) && (target_angles > next_angles)))
+ if(((action->mmodule.manager->servo_values[i].target_angle < target_angles) && (target_angles < next_angles)) ||
+ ((action->mmodule.manager->servo_values[i].target_angle > target_angles) && (target_angles > next_angles)))
dir_change=0x00;
else
dir_change=0x01;
// check the type of ending
- if(dir_change || action_pause_time>0 || action_end)
- action_zero_speed_finish[i]=0x01;
+ if(dir_change || action->pause_time>0 || action->end)
+ action->zero_speed_finish[i]=0x01;
else
- action_zero_speed_finish[i]=0x00;
+ action->zero_speed_finish[i]=0x00;
// find the maximum angle of motion in speed base schedule
- if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule
+ if(action->current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule
{
// fer el valor absolut
- if(action_moving_angles[i]<0)
- tmp_angle=-action_moving_angles[i];
+ if(action->moving_angles[i]<0)
+ tmp_angle=-action->moving_angles[i];
else
- tmp_angle=action_moving_angles[i];
+ tmp_angle=action->moving_angles[i];
if(tmp_angle>max_angle)
max_angle=tmp_angle;
}
- action_module.manager->servo_values[i].cw_compliance=(1<<(action_current_page.header.slope[i]&0x0F));
- action_module.manager->servo_values[i].ccw_compliance=(1<<((action_current_page.header.slope[i]&0xF0)>>4));
+ action->mmodule.manager->servo_values[i].cw_compliance=(1<<(action->current_page.header.slope[i]&0x0F));
+ action->mmodule.manager->servo_values[i].ccw_compliance=(1<<((action->current_page.header.slope[i]&0xF0)>>4));
}
}
- if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)
- action_step_time=(max_angle*256)/(action_current_page.steps[action_current_step_index].time*720);
- accel_time_num=action_current_page.header.accel;
+ if(action->current_page.header.schedule==SPEED_BASE_SCHEDULE)
+ action->step_time=(max_angle*256)/(action->current_page.steps[action->current_step_index].time*720);
+ accel_time_num=action->current_page.header.accel;
accel_time_num=accel_time_num<<9;
- if(action_step_time<=(accel_time_num<<1))
+ if(action->step_time<=(accel_time_num<<1))
{
- if(action_step_time==0)
+ if(action->step_time==0)
accel_time_num=0;
else
{
- accel_time_num=(action_step_time-action_period)>>1;
+ accel_time_num=(action->step_time-action->period)>>1;
if(accel_time_num==0)
- action_step_time=0;
+ action->step_time=0;
}
}
- action_total_time=action_step_time;
- action_pre_time=accel_time_num;
- action_main_time=action_total_time-action_pre_time;
- non_zero_speed_divider=(action_pre_time>>1)+action_main_time;
- if(non_zero_speed_divider<action_period)
- non_zero_speed_divider=action_period;
- zero_speed_divider=action_main_time;
- if(zero_speed_divider<action_period)
- zero_speed_divider=action_period;
+ action->total_time=action->step_time;
+ action->pre_time=accel_time_num;
+ action->main_time=action->total_time-action->pre_time;
+ non_zero_speed_divider=(action->pre_time>>1)+action->main_time;
+ if(non_zero_speed_divider<action->period)
+ non_zero_speed_divider=action->period;
+ zero_speed_divider=action->main_time;
+ if(zero_speed_divider<action->period)
+ zero_speed_divider=action->period;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- action_pre_time_initial_speed_angle=(action_start_speed[i]*action_pre_time)>>1;
- moving_angle=action_moving_angles[i]<<16;
- if(action_zero_speed_finish[i])
- action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider;
+ action_pre_time_initial_speed_angle=(action->start_speed[i]*action->pre_time)>>1;
+ moving_angle=action->moving_angles[i]<<16;
+ if(action->zero_speed_finish[i])
+ action->main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider;
else
- action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider;
-// if((action_main_speed[i]>>16)>info.max_speed)
-// action_main_speed[i]=(info.max_speed*65536);
-// else if((action_main_speed[i]>>16)<-info.max_speed)
-// action_main_speed[i]=-(info.max_speed*65536);
+ action->main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider;
+// if((action->main_speed[i]>>16)>info.max_speed)
+// action->main_speed[i]=(info.max_speed*65536);
+// else if((action->main_speed[i]>>16)<-info.max_speed)
+// action->main_speed[i]=-(info.max_speed*65536);
}
}
}
-void action_finish(void)
+void action_finish(TActionMModule *action)
{
// set the internal state machine to the idle state
- action_end=0x00;
+ action->end=0x00;
// change the internal state
- action_running=0x00;
+ action->running=0x00;
}
void action_set_period(void *module,unsigned short int period_ms)
{
+ TActionMModule *action=(TActionMModule *)module;
+
// load the current period
- action_period=(period_ms<<16)/1000;
+ action->period=(period_ms<<16)/1000;
}
void action_set_module(void *module,unsigned char servo_id)
@@ -237,221 +219,214 @@ void action_set_module(void *module,unsigned char servo_id)
void action_pre_process(void *module)
{
+ TActionMModule *action=(TActionMModule *)module;
unsigned char i;
- // angle variables
- static long long int accel_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
- static long long int main_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
- // speed variables (in controller units)
- static long long int current_speed[PAGE_MAX_NUM_SERVOS]={0};// fixed point 48|16 format
- long long int delta_speed;
- // control variables
- static action_states state=ACTION_PAUSE;
- if(action_running==0x01)
+ if(action->running==0x01)
{
- action_current_time+=action_period;
- switch(state)
+ action->current_time+=action->period;
+ switch(action->state)
{
- case ACTION_PRE: if(action_current_time>action_section_time)
+ case ACTION_PRE: if(action->current_time>action->section_time)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
/* the last segment of the pre section */
- 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_module.manager->servo_values[i].target_angle=action_start_angles[i]+accel_angles[i];
+ action->delta_speed=(action->main_speed[i]-action->start_speed[i]);
+ action->current_speed[i]=action->start_speed[i]+action->delta_speed;
+ action->accel_angles[i]=(action->start_speed[i]*action->section_time+((action->delta_speed*action->section_time)>>1))>>16;
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+action->accel_angles[i];
/* update of the state */
- if(!action_zero_speed_finish[i])
+ if(!action->zero_speed_finish[i])
{
- if((action_step_time-action_pre_time)==0)
- main_angles[i]=0;
+ if((action->step_time-action->pre_time)==0)
+ action->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_module.manager->servo_values[i].target_angle;
+ action->main_angles[i]=((action->moving_angles[i]-action->accel_angles[i])*(action->step_time-(action->pre_time<<1)))/(action->step_time-action->pre_time);
+ action->start_angles[i]=action->mmodule.manager->servo_values[i].target_angle;
}
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_module.manager->servo_values[i].target_angle;
+ action->main_angles[i]=action->moving_angles[i]-action->accel_angles[i]-(((action->main_speed[i]*action->pre_time)>>1)>>16);
+ action->start_angles[i]=action->mmodule.manager->servo_values[i].target_angle;
}
/* the first step of the main section */
- action_module.manager->servo_values[i].target_angle=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->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+(action->main_angles[i]*(action->current_time-action->section_time))/(action->step_time-(action->pre_time<<1));
+ action->current_speed[i]=action->main_speed[i];
}
}
- action_current_time=action_current_time-action_section_time;
- action_section_time=action_step_time-(action_pre_time<<1);
- state=ACTION_MAIN;
+ action->current_time=action->current_time-action->section_time;
+ action->section_time=action->step_time-(action->pre_time<<1);
+ action->state=ACTION_MAIN;
}
else
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- 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_module.manager->servo_values[i].target_angle=action_start_angles[i]+accel_angles[i];
+ action->delta_speed=((action->main_speed[i]-action->start_speed[i])*action->current_time)/action->section_time;
+ action->current_speed[i]=action->start_speed[i]+action->delta_speed;
+ action->accel_angles[i]=((action->start_speed[i]*action->current_time)+((action->delta_speed*action->current_time)>>1))>>16;
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+action->accel_angles[i];
}
}
- state=ACTION_PRE;
+ action->state=ACTION_PRE;
}
break;
- case ACTION_MAIN: if(action_current_time>action_section_time)
+ case ACTION_MAIN: if(action->current_time>action->section_time)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
/* last segment of the main section */
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+main_angles[i];
- current_speed[i]=action_main_speed[i];
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+action->main_angles[i];
+ action->current_speed[i]=action->main_speed[i];
/* update state */
- action_start_angles[i]=action_module.manager->servo_values[i].target_angle;
- main_angles[i]=action_moving_angles[i]-main_angles[i]-accel_angles[i];
+ action->start_angles[i]=action->mmodule.manager->servo_values[i].target_angle;
+ action->main_angles[i]=action->moving_angles[i]-action->main_angles[i]-action->accel_angles[i];
/* first segment of the post section */
- if(action_zero_speed_finish[i])
+ 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_module.manager->servo_values[i].target_angle=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
+ action->delta_speed=((0.0-action->main_speed[i])*(action->current_time-action->section_time))/action->pre_time;
+ action->current_speed[i]=action->main_speed[i]+action->delta_speed;
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+(((action->main_speed[i]*(action->current_time-action->section_time))+((action->delta_speed*(action->current_time-action->section_time))>>1))>>16);
}
else
{
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time);
- current_speed[i]=action_main_speed[i];
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+((action->main_angles[i]*(action->current_time-action->section_time))/action->pre_time);
+ action->current_speed[i]=action->main_speed[i];
}
}
}
- action_current_time=action_current_time-action_section_time;
- action_section_time=action_pre_time;
- state=ACTION_POST;
+ action->current_time=action->current_time-action->section_time;
+ action->section_time=action->pre_time;
+ action->state=ACTION_POST;
}
else
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
- current_speed[i]=action_main_speed[i];
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+(action->main_angles[i]*action->current_time)/action->section_time;
+ action->current_speed[i]=action->main_speed[i];
}
}
- state=ACTION_MAIN;
+ action->state=ACTION_MAIN;
}
break;
- case ACTION_POST: if(action_current_time>action_section_time)
+ case ACTION_POST: if(action->current_time>action->section_time)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
/* last segment of the post section */
- if(action_zero_speed_finish[i])
+ if(action->zero_speed_finish[i])
{
- delta_speed=-action_main_speed[i];
- current_speed[i]=action_main_speed[i]+delta_speed;
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16);
+ action->delta_speed=-action->main_speed[i];
+ action->current_speed[i]=action->main_speed[i]+action->delta_speed;
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+(((action->main_speed[i]*action->section_time)+((action->delta_speed*action->section_time)>>1))>>16);
}
else
{
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+main_angles[i];
- current_speed[i]=action_main_speed[i];
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+action->main_angles[i];
+ action->current_speed[i]=action->main_speed[i];
}
/* update state */
- action_start_angles[i]=action_module.manager->servo_values[i].target_angle;
- action_start_speed[i]=current_speed[i];
+ action->start_angles[i]=action->mmodule.manager->servo_values[i].target_angle;
+ action->start_speed[i]=action->current_speed[i];
}
}
/* load the next step */
- if(action_pause_time==0)
+ if(action->pause_time==0)
{
- if(action_end)
+ if(action->end)
{
- state=ACTION_PAUSE;
- action_finish();
+ action->state=ACTION_PAUSE;
+ action_finish(action);
}
else
{
- action_load_next_step();
+ action_load_next_step(action);
/* first step of the next section */
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- 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_module.manager->servo_values[i].target_angle=action_start_angles[i]+accel_angles[i];
+ action->delta_speed=((action->main_speed[i]-action->start_speed[i])*(action->current_time-action->section_time))/action->pre_time;
+ action->current_speed[i]=action->start_speed[i]+action->delta_speed;
+ action->accel_angles[i]=(((action->start_speed[i]*(action->current_time-action->section_time))+((action->delta_speed*(action->current_time-action->section_time))>>1))>>16);
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+action->accel_angles[i];
}
}
- action_current_time=action_current_time-action_section_time;
- action_section_time=action_pre_time;
- state=ACTION_PRE;
+ action->current_time=action->current_time-action->section_time;
+ action->section_time=action->pre_time;
+ action->state=ACTION_PRE;
}
}
else
{
- action_current_time=action_current_time-action_section_time;
- action_section_time=action_pause_time;
- state=ACTION_PAUSE;
+ action->current_time=action->current_time-action->section_time;
+ action->section_time=action->pause_time;
+ action->state=ACTION_PAUSE;
}
}
else
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- if(action_zero_speed_finish[i])
+ if(action->zero_speed_finish[i])
{
- delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time;
- current_speed[i]=action_main_speed[i]+delta_speed;
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16);
+ action->delta_speed=((0.0-action->main_speed[i])*action->current_time)/action->section_time;
+ action->current_speed[i]=action->main_speed[i]+action->delta_speed;
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+(((action->main_speed[i]*action->current_time)+((action->delta_speed*action->current_time)>>1))>>16);
}
else
{
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
- current_speed[i]=action_main_speed[i];
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+(action->main_angles[i]*action->current_time)/action->section_time;
+ action->current_speed[i]=action->main_speed[i];
}
}
}
- state=ACTION_POST;
+ action->state=ACTION_POST;
}
break;
- case ACTION_PAUSE: if(action_current_time>action_section_time)
+ case ACTION_PAUSE: if(action->current_time>action->section_time)
{
- if(action_end)
+ if(action->end)
{
- state=ACTION_PAUSE;
- action_finish();
+ action->state=ACTION_PAUSE;
+ action_finish(action);
}
else
{
// find next page to execute
- action_load_next_step();
+ action_load_next_step(action);
/* first step of the next section */
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- 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_module.manager->servo_values[i].target_angle=action_start_angles[i]+accel_angles[i];
+ action->delta_speed=((action->main_speed[i]-action->start_speed[i])*(action->current_time-action->section_time))/action->pre_time;
+ action->current_speed[i]=action->start_speed[i]+action->delta_speed;
+ action->accel_angles[i]=(((action->start_speed[i]*(action->current_time-action->section_time))+((action->delta_speed*(action->current_time-action->section_time))>>1))>>16);
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+action->accel_angles[i];
}
}
- action_current_time=action_current_time-action_section_time;
- action_section_time=action_pre_time;
- state=ACTION_PRE;
+ action->current_time=action->current_time-action->section_time;
+ action->section_time=action->pre_time;
+ action->state=ACTION_PRE;
}
}
else// pause section still active
- state=ACTION_PAUSE;
+ action->state=ACTION_PAUSE;
break;
default: break;
}
@@ -459,98 +434,114 @@ void action_pre_process(void *module)
}
// public functions
-void action_init(void)
+unsigned char action_init(TActionMModule *action,TMemory *memory,unsigned short int ram_base_address)
{
unsigned char i;
/* initialize the motion module */
- mmodule_init(&action_module);
- action_module.id=MM_ACTION;
- action_module.set_period=action_set_period;
- action_module.set_module=action_set_module;
- action_module.pre_process=action_pre_process;
+ mmodule_init(&action->mmodule);
+ action->mmodule.id=MM_ACTION;
+ action->mmodule.set_period=action_set_period;
+ action->mmodule.set_module=action_set_module;
+ action->mmodule.pre_process=action_pre_process;
+ action->mmodule.data=action;
// init manager_current_angles with the current position of the servos
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
// angle variables
- action_moving_angles[i]=0;// fixed point 48|16 format
- action_start_angles[i]=0;
+ action->moving_angles[i]=0;// fixed point 48|16 format
+ action->start_angles[i]=0;
// speed variables
- action_start_speed[i]=0;// fixed point 48|16 format
- action_main_speed[i]=0;// fixed point 48|16 format
+ action->start_speed[i]=0;// fixed point 48|16 format
+ action->main_speed[i]=0;// fixed point 48|16 format
// control variables
- action_zero_speed_finish[i]=0x00;
+ action->zero_speed_finish[i]=0x00;
+ action->current_speed[i]=0;
}
// 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;
+ 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;
- action_running=0x00;
- action_next_index=0;
+ action->end=0x00;
+ action->stop=0x00;
+ action->running=0x00;
+ action->next_index=0;
// time variables (in time units (7.8 ms each time unit))
- action_total_time=0;// fixed point 48|16 format
- action_pre_time=0;// fixed point 48|16 format
- action_main_time=0;// fixed point 48|16 format
- action_step_time=0;// fixed point 48|16 format
- action_pause_time=0;// fixed point 48|16 format
- action_current_time=0;// fixed point 48|16 format
- action_section_time=0;// fixed point 48|16 format
+ action->total_time=0;// fixed point 48|16 format
+ action->pre_time=0;// fixed point 48|16 format
+ action->main_time=0;// fixed point 48|16 format
+ action->step_time=0;// fixed point 48|16 format
+ action->pause_time=0;// fixed point 48|16 format
+ action->current_time=0;// fixed point 48|16 format
+ action->section_time=0;// fixed point 48|16 format
+ action->state=ACTION_PAUSE;
+ action->num_repetitions=0;
//action_period=(DYN_MANAGER_DEFAULT_PERIOD_US<<16)/1000000;
+ mem_module_init(&action->mem_module);
+ action->mem_module.data=action;
+ action->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))action_write_cmd;
+ action->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))action_read_cmd;
+ if(!mem_module_add_ram_segment(&action->mem_module,ram_base_address,RAM_ACTION_MM_LENGTH))
+ return 0x00;
+ if(!mem_add_module(memory,&action->mem_module))
+ return 0x00;
+ action->ram_base_address=ram_base_address;
+ action->memory=memory;
+
+ return 0x01;
}
-TMotionModule *action_get_module(void)
+TMotionModule *action_get_module(TActionMModule *action)
{
- return &action_module;
+ return &action->mmodule;
}
-unsigned char action_load_page(unsigned char page_id)
+unsigned char action_load_page(TActionMModule *action,unsigned char page_id)
{
- action_next_index=page_id;
- pages_get_page(action_next_index,&action_current_page);
- pages_get_page(action_current_page.header.next,&action_next_page);
- if(!pages_check_checksum(&action_current_page))
+ action->next_index=page_id;
+ pages_get_page(action->next_index,&action->current_page);
+ pages_get_page(action->current_page.header.next,&action->next_page);
+ if(!pages_check_checksum(&action->current_page))
return 0x00;
- if(!pages_check_checksum(&action_next_page))
+ if(!pages_check_checksum(&action->next_page))
return 0x00;
return 0x01;
}
-void action_start_page(void)
+void action_start_page(TActionMModule *action)
{
unsigned char i;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
- action_start_angles[i]=action_module.manager->servo_values[i].target_angle;
- action_stop=0x00;
- action_current_time=0;
- action_section_time=0;
- action_current_step_index=-1;
+ action->start_angles[i]=action->mmodule.manager->servo_values[i].target_angle;
+ action->stop=0x00;
+ action->current_time=0;
+ action->section_time=0;
+ action->current_step_index=-1;
/* clear the interrupt flag */
- action_running=0x01;
+ action->running=0x01;
}
-void action_stop_page(void)
+void action_stop_page(TActionMModule *action)
{
- action_stop=0x01;
+ action->stop=0x01;
}
-unsigned char action_is_running(void)
+unsigned char action_is_running(TActionMModule *action)
{
- return action_running;
+ return action->running;
}
-unsigned char action_get_current_page(void)
+unsigned char action_get_current_page(TActionMModule *action)
{
- return action_current_page_index;
+ return action->current_page_index;
}
-unsigned char action_get_current_step(void)
+unsigned char action_get_current_step(TActionMModule *action)
{
- return action_current_step_index;
+ return action->current_step_index;
}