Skip to content

GitLab

Commit fb208d0c authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Changed the long int variables to long long int to avoid undesired overflow.

parent 1a306833
Hide whitespace changes
Inline Side-by-side
dynamixel_manager/include/modules/joint_motion.h
View file @ fb208d0c
......@@ -18,15 +18,15 @@ typedef struct{
TMemory *memory;
TMemModule mem_module;
unsigned short int ram_base_address;
long int target_angles[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
long int target_speeds[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
long int target_accels[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
long int target_stop_angles[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
long int current_angles[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
long int current_speeds[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
char dir[DYN_MANAGER_MAX_NUM_DEVICES];// joint_direction
long int motion_period;
short int motion_period_ms;
signed long long int target_angles[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
signed long long int target_speeds[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
signed long long int target_accels[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
signed long long int target_stop_angles[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
signed long long int current_angles[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
signed long long int current_speeds[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
signed char dir[DYN_MANAGER_MAX_NUM_DEVICES];// joint_direction
unsigned long long int motion_period;
unsigned short int motion_period_ms;
unsigned char stop[NUM_JOINT_GROUPS];
unsigned char moving[NUM_JOINT_GROUPS];
unsigned int group_servos[NUM_JOINT_GROUPS];
......
dynamixel_manager/src/modules/joint_motion.c
View file @ fb208d0c
......@@ -88,48 +88,48 @@ void joint_motion_pre_process(void *module)
if(abs(joint->target_angles[i]-joint->current_angles[i])>=65)
{
moving=0x01;
if(joint->current_speeds[i]!=joint->dir[i]*joint->target_speeds[i])// it is necessary to change the current speed
if(joint->current_speeds[i]!=((signed char)joint->dir[i])*joint->target_speeds[i])// it is necessary to change the current speed
{
joint->current_angles[i]+=((joint->current_speeds[i]*joint->motion_period)>>17);
if(joint->current_speeds[i]>joint->dir[i]*joint->target_speeds[i])
joint->current_angles[i]+=(((signed long long int)(joint->current_speeds[i]*joint->motion_period))>>17);
if(joint->current_speeds[i]>((signed char)joint->dir[i])*joint->target_speeds[i])
{
joint->current_speeds[i]-=((joint->target_accels[i]*joint->motion_period)>>16);// reduce speed
if(joint->current_speeds[i]<joint->dir[i]*joint->target_speeds[i])
joint->current_speeds[i]=joint->dir[i]*joint->target_speeds[i];
joint->current_speeds[i]-=(((signed long long int)(joint->target_accels[i]*joint->motion_period))>>16);// reduce speed
if(joint->current_speeds[i]<((signed char)joint->dir[i])*joint->target_speeds[i])
joint->current_speeds[i]=((signed char)joint->dir[i])*joint->target_speeds[i];
}
else
{
joint->current_speeds[i]+=((joint->target_accels[i]*joint->motion_period)>>16);// increase speed
if(joint->current_speeds[i]>joint->dir[i]*joint->target_speeds[i])
joint->current_speeds[i]=joint->dir[i]*joint->target_speeds[i];
joint->current_speeds[i]+=(((signed long long int)(joint->target_accels[i]*joint->motion_period))>>16);// increase speed
if(joint->current_speeds[i]>((signed char)joint->dir[i])*joint->target_speeds[i])
joint->current_speeds[i]=((signed char)joint->dir[i])*joint->target_speeds[i];
}
joint->current_angles[i]+=((joint->current_speeds[i]*joint->motion_period)>>17);
if(joint->dir[i]==1 && joint->current_angles[i]>joint->target_angles[i])
joint->current_angles[i]+=(((signed long long int)(joint->current_speeds[i]*joint->motion_period))>>17);
if(((signed char)joint->dir[i])==1 && joint->current_angles[i]>joint->target_angles[i])
joint->current_angles[i]=joint->target_angles[i];
if(joint->dir[i]==-1 && joint->current_angles[i]<joint->target_angles[i])
if(((signed char)joint->dir[i])==-1 && joint->current_angles[i]<joint->target_angles[i])
joint->current_angles[i]=joint->target_angles[i];
}
else
{
if(abs(joint->target_angles[i]-joint->current_angles[i])>joint->target_stop_angles[i])// constant speed phase
{
joint->current_angles[i]+=((joint->current_speeds[i]*joint->motion_period)>>16);
if(joint->dir[i]==1 && joint->current_angles[i]>joint->target_angles[i])
joint->current_angles[i]+=(((signed long long int)(joint->current_speeds[i]*joint->motion_period))>>16);
if(((signed char)joint->dir[i])==1 && joint->current_angles[i]>joint->target_angles[i])
joint->current_angles[i]=joint->target_angles[i];
if(joint->dir[i]==-1 && joint->current_angles[i]<joint->target_angles[i])
if(((signed char)joint->dir[i])==-1 && joint->current_angles[i]<joint->target_angles[i])
joint->current_angles[i]=joint->target_angles[i];
}
else// deceleration phase
{
joint->current_angles[i]=joint->current_angles[i]+((joint->current_speeds[i]*joint->motion_period)>>17);
joint->target_speeds[i]=joint->target_speeds[i]-((joint->target_accels[i]*joint->motion_period)>>16);
joint->current_angles[i]=joint->current_angles[i]+(((signed long long int)(joint->current_speeds[i]*joint->motion_period))>>17);
joint->target_speeds[i]=joint->target_speeds[i]-(((signed long long int)(joint->target_accels[i]*joint->motion_period))>>16);
if(joint->target_speeds[i]<0)
joint->target_speeds[i]=0;
joint->current_speeds[i]=joint->dir[i]*joint->target_speeds[i];
joint->current_angles[i]=joint->current_angles[i]+((joint->current_speeds[i]*joint->motion_period)>>17);
if(joint->dir[i]==1 && joint->current_angles[i]>joint->target_angles[i])
joint->current_speeds[i]=((signed char)joint->dir[i])*joint->target_speeds[i];
joint->current_angles[i]=joint->current_angles[i]+(((signed long long int)(joint->current_speeds[i]*joint->motion_period))>>17);
if(((signed char)joint->dir[i])==1 && joint->current_angles[i]>joint->target_angles[i])
joint->current_angles[i]=joint->target_angles[i];
if(joint->dir[i]==-1 && joint->current_angles[i]<joint->target_angles[i])
if(((signed char)joint->dir[i])==-1 && joint->current_angles[i]<joint->target_angles[i])
joint->current_angles[i]=joint->target_angles[i];
}
}
......@@ -139,14 +139,6 @@ void joint_motion_pre_process(void *module)
joint->current_speeds[i]=0;
}
joint->mmodule.manager->servo_values[i].target_angle=joint->current_angles[i];
if(joint->mmodule.manager->servo_configs[i]->pid)
{
}
else
{
joint->mmodule.manager->servo_values[i].cw_compliance=5;
joint->mmodule.manager->servo_values[i].ccw_compliance=5;
}
}
}
}
......@@ -224,7 +216,7 @@ unsigned int joint_motion_get_group_servos(TJointMModule *joint,unsigned char gr
void joint_motion_start(TJointMModule *joint,unsigned char group_id)
{
unsigned char i;
short int angle;
signed long long int angle;
unsigned int servo_index;
/* initialize the internal variables */
......@@ -245,12 +237,13 @@ void joint_motion_start(TJointMModule *joint,unsigned char group_id)
// current values
joint->current_angles[i]=joint->mmodule.manager->servo_values[i].target_angle;
// target angle
angle=joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_ANGLES_OFFSET+i*2]+(joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_ANGLES_OFFSET+i*2+1]<<8);
angle=(signed long long int)((signed short int)(joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_ANGLES_OFFSET+i*2]+(joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_ANGLES_OFFSET+i*2+1]<<8)));
angle=angle<<9;
if(angle>joint->mmodule.manager->servo_configs[i]->ccw_angle_limit)
angle=joint->mmodule.manager->servo_configs[i]->ccw_angle_limit;
else if(angle<joint->mmodule.manager->servo_configs[i]->cw_angle_limit)
angle=joint->mmodule.manager->servo_configs[i]->ccw_angle_limit;
joint->target_angles[i]=angle<<9;
angle=joint->mmodule.manager->servo_configs[i]->cw_angle_limit;
joint->target_angles[i]=angle;
// target speed
joint->target_speeds[i]=joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_SPEEDS_OFFSET+i]<<16;
// target accel
......@@ -263,14 +256,14 @@ void joint_motion_start(TJointMModule *joint,unsigned char group_id)
// check the parameters
if(abs(joint->target_angles[i]-joint->current_angles[i])>=65)
{
if((joint->target_speeds[i]*joint->target_speeds[i])/joint->target_accels[i]>abs(joint->target_angles[i]-joint->current_angles[i]))
if(((unsigned long long int)joint->target_speeds[i]*joint->target_speeds[i])/joint->target_accels[i]>abs(joint->target_angles[i]-joint->current_angles[i]))
{
joint->target_accels[i]=(joint->target_speeds[i]*joint->target_speeds[i])/abs(joint->target_angles[i]-joint->current_angles[i]);
joint->target_accels[i]=((unsigned long long int)joint->target_speeds[i]*joint->target_speeds[i])/abs(joint->target_angles[i]-joint->current_angles[i]);
joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_ACCELS_OFFSET+i]=joint->target_accels[i]>>16;
}
}
// stop angles
joint->target_stop_angles[i]=joint->target_speeds[i]*joint->target_speeds[i]/(2*joint->target_accels[i]);
joint->target_stop_angles[i]=((signed long long int)joint->target_speeds[i]*joint->target_speeds[i])/(2*joint->target_accels[i]);
// the current angles, speeds and accelerations are in RAM
if(joint->target_angles[i]>=joint->current_angles[i])
joint->dir[i]=1;
......
Markdown is supported
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