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
......@@ -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];
......
......@@ -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;
......
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