We're using unsigned ints. The values when we have strange behavior go above 32766. That's just the last number I wrote down when I was testing the arm.
This is most of our arm routine. (Probably more than you wanted to see--sorry.)
Code:
static unsigned char enable = 0;
void arm_positioning(void)
{
static short use_manual_control_shoulder = 1, use_manual_control_elbow = 1;
if(arm_sw_freeze == 1) //Make the arm stop moving
{
enable = 0;
use_manual_control_shoulder = use_manual_control_elbow = 0;
}
if(oi_shoulder_sw_fwd)
{
cylinder_fwd = 0;
cylinder_rev = 1;
shoulder_target = Get_Shoulder_Pos();
use_manual_control_shoulder = 1;
}
else if(oi_shoulder_sw_rev)
{
cylinder_fwd = 1;
cylinder_rev = 0;
shoulder_target = Get_Shoulder_Pos();
use_manual_control_shoulder = 1;
}
if(oi_elbow_sw_fwd)
{
arm_motor = 127 + arm_speed;
elbow_target = Get_Elbow_Pos();
use_manual_control_elbow = 1;
enable = 1; //Allow arm to move again, if it was turned off.
}
else if(oi_elbow_sw_rev)
{
arm_motor = 127 - arm_speed;
elbow_target = Get_Elbow_Pos();
use_manual_control_elbow = 1;
enable = 1; //Allow arm to move again, if it was turned off.
}
// manual positioning, but no movement commanded; hold cylinder in place
if (!oi_shoulder_sw_fwd && !oi_shoulder_sw_rev
&& use_manual_control_shoulder)
{
cylinder_fwd = 0;
cylinder_rev = 0;
enable = 1;
}
if (!oi_elbow_sw_fwd && !oi_elbow_sw_rev
&& use_manual_control_elbow)
{
arm_motor = 127;
enable = 1;
}
if(!use_manual_control_shoulder && !use_manual_control_elbow)
position_arm(shoulder_target, elbow_target);
printf("Manual FWD: %d REV: %d, Shldr Trgt: %u, Elb Trgt: %u, Shldr: 0x%04x, Elbow: 0x%04x\n",
oi_shoulder_sw_fwd, oi_shoulder_sw_rev, shoulder_target, elbow_target, Get_Shoulder_Pos(), Get_Elbow_Pos());
printf("Shoulder Pot: %u, Elbow Pot: %u, Cylinder Values FWD: %d REV: %d\n",
Get_Shoulder_Pos(), Get_Elbow_Pos(), cylinder_fwd, cylinder_rev);
}
void position_arm(unsigned int shoulder_target, unsigned int elbow_target)
{
//This gets changed a few lines later if enable is on.
arm_motor = 127;
cylinder_fwd = cylinder_rev = 0;
if (enable)
{
if(Get_Shoulder_Pos() < (shoulder_target - SHOULDER_DEADBAND))
{
cylinder_fwd = 1;
cylinder_rev = 0;
}
else if(Get_Shoulder_Pos() > (shoulder_target + SHOULDER_DEADBAND))
{
cylinder_fwd = 0;
cylinder_rev = 1;
}
else
{
cylinder_fwd = cylinder_rev = 0;
}
if(Get_Elbow_Pos() < (elbow_target - ELBOW_DEADBAND))
{
arm_motor = 127 + arm_speed;
}
else if(Get_Elbow_Pos() > (elbow_target + ELBOW_DEADBAND))
{
arm_motor = 127 - arm_speed;
}
}
}
unsigned int Get_Shoulder_Pos()
{
return Get_Analog_Value(robot_shoulder_pot);
}
unsigned int Get_Elbow_Pos()
{
return Get_Analog_Value(robot_elbow_pot);
}