diff --git a/dynamixel_manager/include/modules/joint_motion.h b/dynamixel_manager/include/modules/joint_motion.h
index bd92ebf2b1a5cf856c9980857a8fc64d19b22d5e..ac6daed767a268936383f21732232b588db27d8b 100644
--- a/dynamixel_manager/include/modules/joint_motion.h
+++ b/dynamixel_manager/include/modules/joint_motion.h
@@ -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];
diff --git a/dynamixel_manager/src/modules/joint_motion.c b/dynamixel_manager/src/modules/joint_motion.c
index ffc40f9cb843704cc0f24e722987db1b0dd6fb75..e3fb1a847d0924f9ed7e2f8c74b6d1f1c1c2fe22 100644
--- a/dynamixel_manager/src/modules/joint_motion.c
+++ b/dynamixel_manager/src/modules/joint_motion.c
@@ -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;