-
Sergi Hernandez authoredSergi Hernandez authored
action.c 25.14 KiB
#include "action.h"
#include "action_registers.h"
#include "ram.h"
#define SPEED_BASE_SCHEDULE 0x00
#define TIME_BASE_SCHEDULE 0x0A
// private functions
void action_write_cmd(TActionMModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
unsigned int ram_offset;
ram_offset=address-module->ram_base_address;
if(ram_in_range(module->ram_base_address+ACTION_CONTROL_OFFSET,address,length))
{
if(data[ACTION_CONTROL_OFFSET-ram_offset]&ACTION_START)
action_start_page(module);
if(data[ACTION_CONTROL_OFFSET-ram_offset]&ACTION_STOP)
action_stop_page(module);
}
if(ram_in_range(module->ram_base_address+ACTION_PAGE_OFFSET,address,length))// load the page identifier
action_load_page(module,data[ACTION_PAGE_OFFSET-ram_offset]);
}
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);
}
void action_load_next_step(TActionMModule *action)
{
// 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
long long int tmp_angle;// fixed point format 48|16 format
long long int angle;// fixed point format 48|16 format
long long int target_angles;// fixed point 48|16 format
long long int next_angles;// fixed point 48|16 format
long long int action_pre_time_initial_speed_angle;// fixed point 48|16 format
long long int moving_angle;// fixed point 48|16 format
// time variables
long long int accel_time_num;// fixed point 48|16 format
long long int zero_speed_divider;// fixed point 48|16 format
long long int non_zero_speed_divider;// fixed point 48|16 format
// control variables
unsigned char dir_change;
// 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
{
if(action->current_page.header.stepnum==1)
{
if(action->stop)
action->next_index=action->current_page.header.exit;
else
{
action->num_repetitions--;
if(action->num_repetitions>0)
action->next_index=action->current_page_index;
else
action->next_index=action->current_page.header.next;
}
if(action->next_index==0)
action->end=0x01;
else
{
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;
}
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)
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
{
if(action->stop)
action->next_index=action->current_page.header.exit;
else
{
action->num_repetitions--;
if(action->num_repetitions>0)
action->next_index=action->current_page_index;
else
action->next_index=action->current_page.header.next;
}
if(action->next_index==0)
action->end=0x01;
else
{
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;
}
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
max_angle=0;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
angle=action->current_page.steps[action->current_step_index].position[i];
if(angle==0x5A00)// bigger than 180
target_angles=action->mmodule.manager->servo_values[i].target_angle;
else
target_angles=angle<<9;
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];
else
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->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;
else
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
{
// fer el valor absolut
if(action->moving_angles[i]<0)
tmp_angle=-action->moving_angles[i];
else
tmp_angle=action->moving_angles[i];
if(tmp_angle>max_angle)
max_angle=tmp_angle;
}
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;
accel_time_num=accel_time_num<<9;
if(action->step_time<=(accel_time_num<<1))
{
if(action->step_time==0)
accel_time_num=0;
else
{
accel_time_num=(action->step_time-action->period)>>1;
if(accel_time_num==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;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
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;
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);
}
}
}
void action_finish(TActionMModule *action){
// set the internal state machine to the idle state
action->end=0x00;
// change the internal state
action->running=0x00;
action->memory->data[action->ram_base_address+ACTION_CONTROL_OFFSET]&=(~ACTION_RUNNING);
}
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;
}
void action_set_module(void *module,unsigned char servo_id)
{
}
void action_pre_process(void *module)
{
TActionMModule *action=(TActionMModule *)module;
unsigned char i;
if(action->running==0x01)
{
action->current_time+=action->period;
switch(action->state)
{
case ACTION_PRE: if(action->current_time>action->section_time)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
/* the last segment of the pre section */
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->step_time-action->pre_time)==0)
action->main_angles[i]=0;
else
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
{
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->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);
action->state=ACTION_MAIN;
}
else
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION) {
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];
}
}
action->state=ACTION_PRE;
}
break;
case ACTION_MAIN: if(action->current_time>action->section_time)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
/* last segment of the main section */
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->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])
{
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->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;
action->state=ACTION_POST;
}
else
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
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];
}
}
action->state=ACTION_MAIN;
}
break;
case ACTION_POST: if(action->current_time>action->section_time)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
/* last segment of the post section */
if(action->zero_speed_finish[i])
{
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->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->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->end)
{
action->state=ACTION_PAUSE;
action_finish(action);
}
else
{
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->mmodule.manager,i)==MM_ACTION)
{
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;
action->state=ACTION_PRE;
}
}
else
{
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->mmodule.manager,i)==MM_ACTION)
{
if(action->zero_speed_finish[i])
{
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->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];
}
}
}
action->state=ACTION_POST;
}
break;
case ACTION_PAUSE: if(action->current_time>action->section_time)
{
if(action->end)
{
action->state=ACTION_PAUSE;
action_finish(action);
}
else {
// find next page to execute
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->mmodule.manager,i)==MM_ACTION)
{
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;
action->state=ACTION_PRE;
}
}
else// pause section still active
action->state=ACTION_PAUSE;
break;
default: break;
}
}
}
// public functions
unsigned char action_init(TActionMModule *action,TMemory *memory,unsigned short int ram_base_address)
{
unsigned char i;
/* initialize the motion module */
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;
// speed variables
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->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;
// control variables
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->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_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(TActionMModule *action)
{
return &action->mmodule;
}
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))
return 0x00;
if(!pages_check_checksum(&action->next_page))
return 0x00;
action->memory->data[action->ram_base_address+ACTION_PAGE_OFFSET]=page_id;
return 0x01;
}
void action_start_page(TActionMModule *action)
{
unsigned char i;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
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->memory->data[action->ram_base_address+ACTION_CONTROL_OFFSET]|=ACTION_RUNNING;
}
void action_stop_page(TActionMModule *action)
{
action->stop=0x01;
}
unsigned char action_is_running(TActionMModule *action)
{
return action->running;
}
unsigned char action_get_current_page(TActionMModule *action)
{
return action->current_page_index;
}
unsigned char action_get_current_step(TActionMModule *action)
{
return action->current_step_index;}