|
Re: I am going crazy
Limit switches are the preferred way to do this. Using simple state machine code is a good way to code the behavior. Check out this simple code from last year, the RunArm function is called each time through the slow loop.
...
typedef enum
{
ARM_UNKNOWN,
ARM_EXTENDING,
ARM_RETRACTING,
ARM_EXTENDED,
ARM_RETRACTED,
ARM_LAST
} ARM_STATES;
typedef struct
{
CLAW_STATES eClawState;
ARM_STATES eArmState;
MAST_STATES eMastState;
} ARM_DATA;
#define ARM_MOTOR_EXTEND_DRIVE 192
#define ARM_MOTOR_RETRACT_DRIVE 64
#define ARM_MOTOR_OFF_DRIVE 128
#define RHS_ARMEXTEND_IN p1_sw_top
#define RHS_ARMRETRACT_IN p1_sw_top
#define RHS_ARM_MOTOR pwm04
#define RHS_ARM_EXTEND_LIMIT rc_dig_in06
#define RHS_ARM_RETRACT_LIMIT rc_dig_in07
....
static void RunArm(ARM_DATA *pData)
{
// if we are in an out of range state
if (pData->eArmState >= ARM_LAST)
{
pData->eArmState = ARM_UNKNOWN;
}
switch (pData->eArmState)
{
case ARM_UNKNOWN:
// always extend the arm if in an unknown state
pData->eArmState = ARM_EXTENDING;
break;
case ARM_EXTENDING:
if (RHS_ARM_EXTEND_LIMIT)
{
// still opening
RHS_ARM_MOTOR = ARM_MOTOR_EXTEND_DRIVE;
}
else
{
// we are open
pData->eArmState = ARM_EXTENDED;
}
break;
case ARM_RETRACTING:
if (RHS_ARM_EXTEND_LIMIT)
{
// still closing
RHS_ARM_MOTOR = ARM_MOTOR_RETRACT_DRIVE;
}
else
{
// we are closed
pData->eArmState = ARM_RETRACTED;
}
break;
case ARM_EXTENDED:
RHS_ARM_MOTOR = ARM_MOTOR_OFF_DRIVE;
if (RHS_ARMRETRACT_IN)
{
// follow command to close
pData->eArmState = ARM_RETRACTING;
}
break;
case ARM_RETRACTED:
RHS_ARM_MOTOR = ARM_MOTOR_OFF_DRIVE;
if (RHS_ARMEXTEND_IN)
{
// follow command to open
pData->eArmState = ARM_EXTENDING;
}
break;
break;
default:
RHS_ARM_MOTOR = ARM_MOTOR_OFF_DRIVE;
pData->eArmState = ARM_UNKNOWN;
break;
}
}
HTH
Last edited by wireties : 06-02-2008 at 20:00.
|