Commit 4ff0e7fa authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added the slope control to the action execution.

parent f230093e
......@@ -52,6 +52,8 @@ typedef struct{
uint16_t center_angle;
uint16_t max_speed;
int16_t current_angle;
uint8_t cw_comp;
uint8_t ccw_comp;
uint16_t current_value;
TModules module;
uint8_t enabled;
......@@ -64,6 +66,7 @@ typedef struct{
// public variables
extern int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
extern int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
// public functions
void manager_init(uint16_t period_us);
......
......@@ -124,10 +124,8 @@ void action_load_next_step(void)
}
}
// compute trajectory
// 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)
......@@ -178,6 +176,7 @@ void action_load_next_step(void)
if(tmp_angle>max_angle)
max_angle=tmp_angle;
}
manager_current_slopes[i]=action_current_page.header.slope[i];
}
}
if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)
......
......@@ -20,6 +20,7 @@ uint8_t manager_num_servos;
TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
// current angles used for all motion modules
int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
// balance values and configuration
int64_t manager_balance_offset[MANAGER_MAX_NUM_SERVOS];
uint8_t manager_balance_enabled;
......@@ -36,12 +37,14 @@ void manager_send_motion_command(void)
if(manager_servos[i].enabled && manager_servos[i].model!=0x0000)
{
servo_ids[num]=manager_servos[i].id;
write_data[num].data_addr=(uint8_t *)&(manager_servos[i].current_value);
manager_servos[i].cw_comp=(1<<manager_current_slopes[i]);
manager_servos[i].ccw_comp=(1<<manager_current_slopes[i]);
write_data[num].data_addr=(uint8_t *)&(manager_servos[i].cw_comp);
num++;
}
}
if(num>0)
dyn_master_sync_write(&bioloid_dyn_master_servos,num,servo_ids,P_GOAL_POSITION_L,2,write_data);
dyn_master_sync_write(&bioloid_dyn_master_servos,num,servo_ids,P_GOAL_POSITION_L,4,write_data);
}
inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
......
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