Using Limit Switches with the Arm

[iwin2000]

This is a little urgent because the robot needs to be shipped tomorrow and it would be ideal to have a working code before that...
I am coding in EasyC and I'm having some problems with coding the limit switches to limit the arm movements so it doesn't hit the bottom of the robot when extended.

This image will provide you with more information about what we want:


When our Limit Switches equal 1, it is pushed down. Otherwise, they equal 0.
The rotator motor of the arm is a PWM. The extender is a relay switch.

VARIABLES USED:

unsigned char LimitSwitch1;
unsigned char LimitSwitch2; 
unsigned char LimitSwitch3; 
unsigned char LimitSwitch4; 
unsigned char YAxisInputArmRotation;

// Joystick Y-Axis Movement controls Arm Rotation Motor Forward / Backward PWM 
// Joystick Aux1 and Aux2 Movement controls Arm Extender Motor Forward / Backward 
// 
// Get Joystick Input for Aux1 and Aux2 
LimitSwitch1 = GetDigitalInput ( 5 ) ; 
LimitSwitch2 = GetDigitalInput ( 6 ) ; 
LimitSwitch3 = GetDigitalInput ( 7 ) ; 
LimitSwitch4 = GetDigitalInput ( 8 ) ; 

if ( LimitSwitch1 == 1 ) 
{ 
// If the Arm Extention Switch is DOWN (ON HOME POSITION) then only allow EXTRACTION and ROTATION MOVEMENT. 
// Up / Down Arm Rotation Movement -- Get Joystick Y-Axis Input 
OIToPWM ( 3 , 2 , 3 , 1 ) ; // Out Extender Arm Movement
OIToRelay ( 3 , 4 , 1 , 2 ) ; 
PrintToScreen ( "Limit Switch ONE is pushed down -- RETRACTION NOT ALLOWED.\n" ) ; 
} 

if ( LimitSwitch1 == 1 && LimitSwitch2 == 1 ) 
{ 
// If Limit Switch 2 ( ARM ROTATION -- FRONT ) is down and LimitSwitch1 is down ( ARM IS RETRACTED ) then ALLOW ALL MOVEMENT. 
// Up / Down Arm Rotation Movement -- Get Joystick Y-Axis Input 
OIToPWM ( 3 , 2 , 3 , 1 ) ; // In / Out Extender Arm Movement 
OIToRelay ( 3 , 3 , 1 , 1 ) ; 
OIToRelay ( 3 , 4 , 1 , 2 ) ; PrintToScreen ( "Limit Switch ONE and Limit Switch TWO are down -- ARM IS RETRACTED, ALLOW ALL.\n" ) ; 
}

if ( LimitSwitch1 == 0 && LimitSwitch2 == 1 ) 
{ 
// If Limit Switch 2 ( ARM ROTATION -- FRONT ) is down BUT Limit Switch 1 (ARM EXTENTION -- HOME ) IS NOT DOWN then ONLY ALLOW REVERSE ROTATION AND RETRATION 
// Up / Down Arm Rotation Movement -- Get Joystick Y-Axis Input 
YAxisInputArmRotation = GetOIAInput ( 3 , 2 ) ; 

if ( YAxisInputArmRotation <= 127 ) 
{ 
SetPWM ( 3 , YAxisInputArmRotation ) ; 
} 
// In Extender Arm Movement 
OIToRelay ( 3 , 4 , 1 , 2 ) ; 
PrintToScreen ( "Limit Switch TWO is down, ONE is NOT. -- RETRACTION AND REVERSE ROTATION ¬ ALLOWED\n" ) ; 
} 

if ( LimitSwitch1 == 1 && LimitSwitch3 == 1 ) 
{ 
// If Limit Switch 3 ( ARM ROTATION -- BACK ) is down and LimitSwitch1 is down ( ARM IS RETRACTED¬ ) then ALLOW ALL MOVEMENT. 
// Up / Down Arm Rotation Movement -- Get Joystick Y-Axis Input 
OIToPWM ( 3 , 2 , 3 , 1 ) ; 
// In / Out Extender Arm Movement 
OIToRelay ( 3 , 3 , 1 , 1 ) ; 
OIToRelay ( 3 , 4 , 1 , 2 ) ; 
PrintToScreen ( "Limit Switch ONE and THREE are pushed down -- ARM IS RETRACTED, ALLOW ALL.\n" ) ; 
} 

if ( LimitSwitch1 == 0 && LimitSwitch3 == 1 ) { 
// If Limit Switch 3 ( ARM ROTATION -- BACK ) is down and LimitSwitch1 is up ( ARM IS EXTENDED ) then only allow forward rotation and retraction 
// Up Arm Rotation Movement -- Get Joystick Y-Axis Input 
YAxisInputArmRotation = GetOIAInput ( 3 , 2 ) ; 
if ( YAxisInputArmRotation >= 127 ) { 
SetPWM ( 3 , YAxisInputArmRotation ) ; 
} 
// In Extender Arm Movement 
OIToRelay ( 3 , 4 , 1 , 2 ) ; 
PrintToScreen ( "Limit Switch THREE is down, ONE is not -- ALLOW RETRACTION and FORWARD ROTATION.\n" ) ; } 

if ( LimitSwitch4 == 1 ) { 
// If Limit Switch 4 is down then move the opposite direction you were going. 
// Arm Rotation Movement is opposite what it was originally going... 
YAxisInputArmRotation = GetOIAInput ( 3 , 2 ) ; 

if ( YAxisInputArmRotation >= 127 ) 
{

} 

PrintToScreen ( "Limit Switch FOUR is pushed down.\n" ) ; 
} 

if ( LimitSwitch1 == 0 && LimitSwitch2 == 0 && LimitSwitch3 == 0 && LimitSwitch4 == 0 ) 
{
// If Limit Switches are not down then DO WHATEVER YOU WANT. 
// Up / Down Arm Rotation Movement -- Get Joystick Y-Axis Input 
OIToPWM ( 3 , 2 , 3 , 1 ) ; 
// In / Out Extender Arm Movement 
OIToRelay ( 3 , 3 , 1 , 1 ) ; 
OIToRelay ( 3 , 4 , 1 , 2 ) ; 
PrintToScreen ( "NO Limit Switches are PUSHED DOWN. ALL MOVEMENT ALLOWED\n" ) ; 
} 
}

I am just generally very unsure about my code and we've run into a problem while testing it: If the arm is moving in any way and one of the switches gets pressed, it totally ignores all user input and keeps turning that way with the last input that was recieved, until the switch is let go, which is TOTALLY not what we want. Is there any way I can reorganize it so that it works the way I need it to? I need it to make the arm STOP when the switch is toggled. Afterwards, I need it to only be able to turn or retract the OPPOSITE way it came from.

Please, any comments will help me, especially since I need to get this working by tomorrow... Thanks