DEWBOT IV Control Software

From DEW Robotics
Jump to: navigation, search

This is the main file for DEWBOT IV control system. A full file set (code, headers, etc can be found at DEWBOT_IV_Control#Control_System_Files.

/*******************************************************************************
* FILE NAME: 1640.c
*
* DESCRIPTION:
*  This file contains the Team 1640 Robot Routines -- DEWBOT IV - 2008 FRC
*
* Revision History                                                                                             
*   Initial Coding - FAS		2/4/2008                                                               
*   Released - FAS			2/22/2008		Version: 1.0.0
*   Added IR_Ramp()		2/24/2008		Version: 1.1.0
*		Helps prevent wheelies in IR mode by ramping up/dn drive motors
*   Added runIRMode()		3/19/2008		Version: 1.2.0
*   Added runAutoMode()
*		These routines were added to facilitate using IR and Full Autonomous functionality
*		Either mode is choosen by the Manual Arm Switch position
*		IR = Manual and Autonomous = Auto
*******************************************************************************/

#include <stdio.h>

#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "user_Serialdrv.h"
#include "encoder.h"
#include "adc.h"
#include "gyro.h"
#include "pid.h"
#include "1640.h"

// Start of 1640 Routines

//Pid Structure for PID Routines
extern Pid_Config_Data_Type Pid_Config_Data[];
Pid_Config_Data_Type *arm_pid = &Pid_Config_Data[ARM];
Pid_Config_Data_Type *drive_pid = &Pid_Config_Data[DRIVE];


//Set default conditions
unsigned char armed_enabled = 0;
unsigned char capture_enabled = 0;
unsigned char op_mode = RESET;
unsigned char armMotion = NEUTRAL;

#define FULL 0
#define IR 1
#define GYRO_90_LEFT -900

#define DEFAULT_IR_SPEED 48
#define FULL_IR_SPEED 100
#define LOW_DEADBAND -35 // was -7,
#define HIGH_DEADBAND 35 // was 7
// the dead band is pretty high with the way the floor runs
// with 7 there was lots of slow movements because of the deadband
// 25 worked well, minimal adjustments


unsigned char motion_state = INITIAL_SEGMENT; 

//This should represent 
//  536" - 20.68 wheel rotations
//  162" - 6.25 wheel rotations
//  486" - 18.75 wheel rotations
//  162" - 6.25 wheel rotations
//  Full Encoder Revolution = 128 Ticks

//long const seg_len[] = {2647,800,2400,800}; // on drive shaft
long const seg_len[] = {7400,2200,6700,2200}; // with small VEX wheel
//For Gyro
unsigned char gyro_biased = 0;

int foster_wait = 0;
	

/*
*****************************************************
*                               a r m p o s i t i o n                               *
*****************************************************
*      Uses PID motion or Manual Joystick to move Arm into Position
*      Position Constants are set using Manual Mode and Reading Encoder Output
*      PID Error is current Encoder Position - Position Constant for op_mode
*      PID will constanly adjust motors to keep arm in op_mode position
*      Called from Default_Routine
*
*      Initial Coding - FAS		2/4/2008
*/
void armPosition (unsigned char arm_position, unsigned char over_ride)
{
	long encoder_pos;
	long pos_error;
	char speed;
	unsigned char reset_condition = 0;

	//Get Current Arm Position
	encoder_pos = Get_Encoder_1_Count();
	printf("Arm Position: %d\r", (int) encoder_pos); 
	
	//Check for Manual Arm Over Ride
	if (over_ride == MANUAL_MODE)	
	{
		//Manual Arm Indicator
		Pwm2_green = 0;
		
		//Manual Arm Movement using Joystick Y Axis Only
		ArmMotor1 = p4_y ;
		printf("Arm Joystick: %d\r", p4_y);
		
		//Output Encoder Position
		//printf("Arm Position: %d\r", (int) encoder_pos); 

		if (top_limit == ON)
			if (ArmMotor1 > 127)
				ArmMotor1 = 127;

		if (bot_limit == ON)
		{
			if (ArmMotor1 < 127)
			{
				Reset_Encoder_1_Count(BOTTOM_LIMIT);
				ArmMotor1 = 127;
				reset_condition = 0;
			}
		}
			
	}
	else	
	{
		//Auto Arm Mode Indicator
		Pwm2_green = 1;

		//Calculuate distance to arm_position
		switch (arm_position)
		{
			// RESET is called before any other arm_position can be set
			// Can be reset if the OI reset button is pushed.
			// Drives arm to bottom limit and resets arm encoder to ZERO.
			case RESET:
				//Move Arm Downward, continue until Bottom Limit is tripped
				reset_condition = 1;
				break;
							
			case PARKED:
				//Calculate distance from PARKED POSITION
				//pos_error = encoder_pos - CATCH_POSITION;
				pos_error = PARKED_POSITION  - encoder_pos;
				break;
	
			case CATCH:
				//Calculate distance from CATCH POSITION
				//pos_error = encoder_pos - CATCH_POSITION;
				pos_error = CATCH_POSITION - encoder_pos;
				break;
	
			case CAPTURE:
				//Calculate distance from CAPTURE POSITION
				//pos_error = encoder_pos - CAPTURE_POSITION;
				pos_error = CAPTURE_POSITION - encoder_pos;
				break;
	
			case POSSESS:
				//Calculate distance from POSSESS POSITION
				//pos_error = encoder_pos - POSSESS_POSITION;
				pos_error = POSSESS_POSITION - encoder_pos;
				break;
	
			case ARMED:
				//Calculate distance from ARMED POSITION
				//pos_error = encoder_pos - ARMED_POSITION;
				pos_error = ARMED_POSITION -  encoder_pos;
				break;
	
			case HURDLE:
				//Calculate distance from HURDLE POSITION
				//pos_error = encoder_pos - HURDLE_POSITION;
				pos_error = HURDLE_POSITION - encoder_pos;
				break;
	
			case PTP:
				//Calculate distance from PTP POSITION
				//pos_error = encoder_pos - PTP_POSITION;
				pos_error = PTP_POSITION  -  encoder_pos;
				break;
	
			case PLACE:
				//Calculate distance from PLACE POSITION
				//pos_error = encoder_pos - PLACE_POSITION;
				pos_error = PLACE_POSITION -  encoder_pos;
				break;
		}

		//Look for Limit Trip
		if (top_limit == ON && armMotion == UP)
		{
			//Reset Arm to Hurdle Position
			//Calculate distance from "Prepare To Place" POSITION
			//pos_error = encoder_pos - PTP;
			pos_error = PTP -  encoder_pos;
			op_mode = PTP;
			
			//Set Arm Motors to Downward Speed
			ArmMotor1 = 100;
	
			//Don't do a reset
			reset_condition = 0;
		}	
		// Arm motion is Neutral or Trying to move down.
		//Or in a RESET Condition moving towards bottom and hits the limit
		else if (bot_limit  == ON && armMotion != UP) 
		{
			//Set Encoder to ZERO
			Reset_Encoder_1_Count(BOTTOM_LIMIT);
			
			//Now Position Arm to CAPTURE Position
			//Calculate distance from CAPTURE POSITION
			//pos_error = encoder_pos - CAPTURE_POSITION;
			//op_mode = CAPTURE_POSITION;
			if (autonomous_mode && (manual_toggle == MANUAL_MODE))
			{
				pos_error = HURDLE_POSITION - encoder_pos;
				op_mode = HURDLE;
			}
			else
			{
				pos_error = CAPTURE_POSITION - encoder_pos;
				op_mode = CAPTURE;
			}
						
			//Set Arm Motors to Upward Speed so that CAPTURE/HURDLE position can be reached
			//after a RESET
			ArmMotor1 = 200;

			//Turn OFF Reset
			reset_condition = 0;
		}
		else if (reset_condition == 1)
		{
			//RESET keeps arm moving downward until limit is reached
			//Move Downward
			ArmMotor1 = 20;
		}
		else
		{
			//Everthing is ok to move to or keep at op_mode POSITION
			//Calculate PID Speed
			speed = UpdatePID (arm_pid, pos_error, encoder_pos);
			//printf("Speed: %d  ",  (int) speed);
			//printf("Encoder Pos: %d   pos_error: %d\r", (int) encoder_pos, (int) pos_error);
		
			//Set Arm Motors to PID Speed
			ArmMotor1 = 127 + speed;
			reset_condition = 0;
		}
	}
	

	//Check for "in-range" to op_mode position
	//Arm is in position when Encoder = a range of +-ARM_RANGE from POSITION CONSTANT
	
	
	//POSITION #4 (PTP and Place) - Set LED's
	if ((encoder_pos > (POSITION_4 - ARM_RANGE))  && (encoder_pos < (POSITION_4 + ARM_RANGE)))
		Relay2_green = 1;
	else
		Relay2_green = 0;


	//POSITION #3 (ARMED and HURDLE) - Set LED's and enable/disable ARMED Mode
	if ((encoder_pos > (POSITION_3 - ARM_RANGE -5))  && (encoder_pos < (POSITION_3 + ARM_RANGE + 5)))
	{
		//Only ENABLE ARMED when arm is in position for ARMED
		armed_enabled = 1;
		Switch1_LED = 1;
	}
	else
	{
		armed_enabled = 0;
		Switch1_LED = 0;
	}

	//POSITION #2  (PARKED and POSSES) - Set LED's
	if ((encoder_pos > (POSITION_2 - ARM_RANGE))  && (encoder_pos < (POSITION_2 + ARM_RANGE)))
		Switch2_LED = 1;
	else
		Switch2_LED = 0;
			
	//POSITION #1 (CATCH and CAPTURE) - Set LED's and consider Sonar only if capture_enabled is true,
	//which means the ball is in CAPTURE POSITION.
	if ((encoder_pos > (POSITION_1 - ARM_RANGE-5))  && (encoder_pos < (POSITION_1 + ARM_RANGE+5)))
	{
		//Auto capture if Sonar Sensed Ball @ programmed distance
		if (capture_enabled)
		{
			//Set op_mode to CAPTURE so forks stay UP
			op_mode = CAPTURE;
			 Relay1_green = 0;
		}
		Switch3_LED = 1;
	}
	else
		Switch3_LED = 0;

}

/*
******************************************************
*                     c h e c k C O M P R E S S O R                         *
******************************************************
*      Turns compressor on/off depending on digital input from Pressure Switch      
*       Called From Default_Routine and User_Initialization                                     
*                                                                                                                            
*      Revision History                                                                                             
*      Initial Coding - FAS		2/4/2008                                                               
*/  
void checkCompressor (void)
{
	if (PressureSwitch == ON)
	{
		//Turn Compressor ON
		Compressor_fwd = TRUE;
		Compressor_rev = FALSE;
	}
	else
	{
		//Turn Compressor OFF
		Compressor_fwd = FALSE;
		Compressor_rev = FALSE;
	}
}
	
/*
******************************************************
*                                 f o r k _ 8                                          *
******************************************************
*      retracts/extends 8" Fork Cylinder based on op_mode
*      Called from Default_Routine
*
*      Revision History                                                                                             
*      Initial Coding - FAS		2/4/2008
*/
void fork_8 (unsigned char position)
{
	if (position == RETRACTED)
	{
		//Retract 8" Fork Cylinder
		Fork_8_fwd = FALSE;  
		Fork_8_rev = TRUE;  
	}
	else
	{
		//Extend 8" Fork Cylinder
		Fork_8_fwd = TRUE;  
		Fork_8_rev = FALSE;  
	}
}	

/*
*****************************************************
*                                  f o r k _ 1_5                                    *
*****************************************************
*      retracts/extends 1.5" Fork Cylinder based on op_mode
*      Called from Default_Routine
*
*      Revision History                                                                                             
*      Initial Coding - FAS		2/4/2008
*/  
void fork_1_5 (unsigned char position)
{
	if (position == RETRACTED)
	{
		//Retract 1 1/2" Fork Cylinder
		Fork_1_5_fwd = FALSE;  
		Fork_1_5_rev = TRUE;  
	}
	else
	{
		//Extend 1 1/2" Fork Cylinder
		Fork_1_5_fwd = TRUE;  
		Fork_1_5_rev = FALSE;  
	}
}	

/*
*****************************************************
*                                c a t a p u l t                                      *
*****************************************************
*      retracts/extends Catapult Cylinder based on op_mode
*      Called from Default_Routine
*
*      Revision History                                                                                             
*      Initial Coding - FAS		2/4/2008
*/
void catapult (unsigned char position)
{
	if (position == RETRACTED)
	{
		//Retract Catapult 1
		Catapult_1_fwd = FALSE;  
		Catapult_1_rev = TRUE;  

		//Retract Catapult 2
		Catapult_2_fwd = FALSE;  
		Catapult_2_rev = TRUE;  
	}
	else
	{
		//Extend Catapult 1
		Catapult_1_fwd = TRUE;  
		Catapult_1_rev = FALSE;  

		//Extend Catapult 2
		Catapult_2_fwd = TRUE;  
		Catapult_2_rev = FALSE;  
	}
}	

/*
*****************************************************
*                                   s h i f t e r                                      *
*****************************************************
*      retracts/extends Shifter Cylinder based on shifter_Toggle  (High/Low Gear) 
*      Called from Default_Routine
*
*      Initial Coding - FAS		2/4/2008
*/
void shifter (unsigned char gear)
{
	if (gear == HIGH_GEAR)
	{
		//Set High Gear
		Shifter_fwd = FALSE;
		Shifter_rev = TRUE;
		Pwm1_green = 1;
	}
	else
	{
		//Set Low Gear
		Shifter_fwd = TRUE;
		Shifter_rev = FALSE;
		Pwm1_green = 0;
	}	
}	



/*
*****************************************************
*                     g e t O p e r a t i o n M o d e                          *
*****************************************************
*      Determines op_mode based on Push Button condition
*      Called from Default_Routine
*
*      Initial Coding - FAS		2/4/2008
*/
unsigned char getOperationMode(unsigned char current_op_mode)
{
	unsigned char which_button;	
	
	//printf("Button: %d\r", current_op_mode);
	
	//Set which_button to Operation MODE
	if (RESET_button)
		which_button = RESET;
	else if (catch_button)
		which_button = CATCH;
	else if (capture_button)
		which_button = CAPTURE;
	else if (park_button)
		which_button = PARKED;
	else if (possess_button)
		which_button = POSSESS;
	else if (armed_button) 
		which_button = ARMED;
	else if (hurdle_button)
		which_button = HURDLE;
	else if (ptp_button)
		which_button = PTP;
	else if (place_button)
		which_button = PLACE;
	else
	{
		//No New Button Pressed, so keep last 
		which_button = current_op_mode;
	}
	
	//Return which_button got pressed
	return (which_button);
}

/*
******************************************************
*                                  r a m p i n g                                       *
******************************************************
*      Joystick / PWM ramping function found on ChiefDelphi
*       Called From Default_Routine
*                                                                                                                            
*      Revision History                                                                                             
*/  

long ramping (unsigned char pwmIN)
{
	long pwmOUT = 0;

	pwmOUT =  ((long)pwmIN - 127);
	pwmOUT = ((pwmOUT) * (pwmOUT) * (pwmOUT));
	pwmOUT = ((pwmOUT) / (128 * 128));
	pwmOUT = (pwmOUT) + (127);
	
	return pwmOUT;
}
//  init_Gyro -- called from auto mode and master startup
void init_Gyro(void)
{
	static unsigned int i;
	static unsigned int j;

	// GYRO
	i++;
	j++; // this will rollover every ~1000 seconds

	if(j == 10)
	{
		printf("\rCalculating Gyro Bias...");
	}

	if(j == 60)
	{
		// start a gyro bias calculation
		Start_Gyro_Bias_Calc();
	}

	if(j == 150)
	{
		// terminate the gyro bias calculation
		Stop_Gyro_Bias_Calc();

		// reset the gyro heading angle
		Reset_Gyro_Angle();

		printf("Done\r");
		j = 0;
		i = 0; 
		gyro_biased = 1;
	}
}
/* End of init_Gyro
*/
/*
******************************************************
*                            Initialize_1640                                        *
******************************************************
*      Initialize Routines for 1640.c
*       Called From Default_Routine
*                                                                                                                            
*      Revision History                                                                                             
*/  

void Initialize_1640(void)
{
	Initialize_Gyro();
	Initialize_ADC();
	Initialize_Encoders();
 
	Initialize_Arm_PID();
	Initialize_Drive_PID();
}	

/*
******************************************************
*                       s e t A r m D i r e c t i o n                              *
******************************************************
*      Determine direction of Arm Travel
*       Called From Default_Routine
*                                                                                                                            
*      Revision History                                                                                             
*/  

void setArmDirection(void)
{
	if (ArmMotor1 > 134)
		armMotion = UP;
	else if (ArmMotor1 < 120)
		armMotion = DN;
	else
		armMotion = NEUTRAL;
}		

/*
******************************************************
*                            m o v e R o b o t                                       *
******************************************************
*      Set Drive Motors in motion
*       Called From Default_Routine
*                                                                                                                            
*   Revision History 
*   Added Y and X parameters		3/19/2008		Version: 1.2.0
*   to make this routine more generic
*	                                                                                            
*/  

void moveRobot(unsigned char Y, unsigned char X)
{
		//Arcade Driving Mode
	//pwm03
		//with Ramping
	// RightDriveMotor = ramping(Limit_Mix(2000 + Y - X + 127)); 
		//without Ramping
	RightDriveMotor = Limit_Mix(2000 + Y - X + 127); 
	
	//Tank Mode - Need second Joystick on PORT3
	//RightDriveMotor = ramping(Y); 
	
	//pwm04
		//with Ramping
	// LeftDriveMotor = ramping(Limit_Mix(2000 + Y + X - 127));
		//without Ramping
	LeftDriveMotor = Limit_Mix(2000 + Y + X - 127);
	
	//Tank Mode - Need second Joystick on PORT3
	//LeftDriveMotor = ramping(Y);
	
	//Tank Mode using XBOX Controller and Chicklet
	//Aux = x
	//Wheel = y
	//pwm03 = p1_aux;
	//pwm04 = p1_wheel;
	
}

/*
******************************************************
*                            I R _ R a m p                                           *
******************************************************
*      Increment/Decrement Drive Motors to prevent Wheelies
*       Called From User_Autonomous_Code()
*                                                                                                                            
*      Revision History                                   
*		Initial Coding by FAS	on	02/24/2008                                                          
*/  

void IR_Ramp (unsigned char direction, unsigned char leftDrive, unsigned rightDrive)
{
	
	//See 1640.h IR Section for IR_RAMP constant
	
	switch (direction)
	{
		case IR_FORWARD:
			//Increment by IR_RAMP (1 will cause about 3.3 second ramp to top Speed)
			leftDrive = leftDrive + IR_RAMP;
			rightDrive = rightDrive + IR_RAMP;
			break;
		case IR_REVERSE:
			//Decrement by IR_RAMP (1 will cause about 3.3 second ramp to top Speed)
			leftDrive = leftDrive - IR_RAMP;
			rightDrive = rightDrive - IR_RAMP;
			break;
	}
	
	//Limits for Victor Speed Controllers - Don' go above 254 or below 0
	
	//Right Drive
	if (rightDrive > 254)
		rightDrive = 254;
	if (rightDrive < 0)
		rightDrive = 0;
	
	//Left Drive
	if (leftDrive > 254)
		leftDrive = 254;
	if (leftDrive < 0)
		leftDrive = 0;
		
}

void runAutoMode(void)
{
	// for GYRO
	static unsigned int i = 0;
	static unsigned int j = 0;
	long heading;
	long pos_error;
	char speed;
	
	
	static long current_gyro_target = 0;
	static unsigned int seg_pos = 0;
	
	static long gyro_target = 0;

	// for Encoder
	long drive_tick_cnt;
    static long last_drive_tick_cnt;

	//Get Current Gyro Heading
	//heading = Get_Gyro_Angle()/10L;
	heading = Get_Gyro_Angle();
	pos_error = heading - current_gyro_target;
	printf("Heading: %d  Error: %d\r", (int) heading, (int) pos_error);
	//printf("Heading: %ld  Error: %ld\r", heading, pos_error);

	//Get Encoder #3 tick count
	drive_tick_cnt = Get_Encoder_3_Count();

	printf("Drive Count: %ld   Segment Len: %d\r", drive_tick_cnt, (unsigned int) seg_len[seg_pos]);

	//printf("P:%d I:%d D:%d  Speed: %d\r", (int)(pid->pTerm*100), (int)(pid->iTerm*100), (int)(pid->dTerm*100), (int)pid->pwmSpeed);

	switch (motion_state)
	{
		//Just in case we want to do something before we begin to move
		case INITIAL_SEGMENT:
			motion_state = MOVE_SEGMENT;
			//printf("INITIAL_SEGMENT\r");
			break;

		case MOVE_SEGMENT:
			//printf("MOVE_SEGMENT\r");
			
			// Move Robot Straight With Motor Error Correction
			//Position Error comes from GYRO

			speed = UpdatePID (drive_pid, pos_error, heading);
			printf("Speed: %d  ",  (int) speed);

			//Set Drive Motors to PID Speed
			if ((speed > LOW_DEADBAND) && (speed < HIGH_DEADBAND))
				//Full Forward (actually reverse in Autononmous mode)
				pwm03 = pwm04 = 254;
			else
			{
				/*  PID Adjustment Commentary
                   	The PID returns a single value, this is how much the speed
					should change.  The PID is really set up to do a single motor,
					which works well on the lifting arm. 
					When we get a negative speed we should subtract it from the
					right motor, postive speed subtract from the left
                
				if ( speed > 0 ) 
					pwm03 = 254 - speed;
				if ( speed < 0) 
					pwm04 = 254 + speed; 
                */

				//Use Correction Speed Factor from PID (Forward robot direction)
				//pwm03 = 127 + speed;
				//pwm04 = 127 - speed;

			//Use Correction Speed Factor from PID (Frank - reverse robot direction) 
				pwm03 = 127 - speed;
				pwm04 = 127 + speed;


			}

			if (drive_tick_cnt >= seg_len[seg_pos])
			{
				motion_state = QTR_TURN_LEFT;
				seg_pos++;
				
				//After one round, only use the 3rd and forth segment distances
				if (seg_pos > 3)
					seg_pos = 2;
					
				pwm03 = pwm04 = 127;
			}
				
			break;

		case QTR_TURN_LEFT:
			//Turn Robot Left
			printf("QTR_TURN_LEFT\r");
			
			//Set motors for Turning Speed
            //Forks are the front
			LeftDriveMotor = 160; // 160;
			RightDriveMotor = 32; //32;
			
			//Update for next Gyro heading
			//current_gyro_target += GYRO_90_LEFT;
			current_gyro_target = GYRO_90_LEFT;

			//gyro_target = GYRO_90_LEFT + current_gyro_target; 
			//printf("Target: %d  Heading: %d\r", (int)gyro_target, (int) heading);

			motion_state = TURNING;

			//Reset PID State
			Initialize_Drive_PID();
			break;

		case TURNING:
			//printf("TURNING\r");

			//Are we at GYRO TARGET Heading?
			//Only checking for when we hit +??? to target position
			//Even if we carry over we should be ready to move straight
			if (pos_error < 50)
			{
				printf("TURNed\r");
				motion_state = STOP;
				// gyro_biased = 0;		
				// was -- motion_state = MOVE_SEGMENT;
			}
			else
				Reset_Encoder_3_Count();
		
			break;

		case STOP:
			if ( last_drive_tick_cnt != drive_tick_cnt )
			{
				printf("Motor Stopped\r");
				pwm03 = pwm04 = 127; // Stop
			}
			else
			{	
				motion_state = MOVE_SEGMENT;
				printf("Starting moving again\r");
				//Reset Gyro Angle
				Reset_Gyro_Angle();
	
				//Reset Target Angle
				current_gyro_target = 0;
				 
				//Reset Encoder for next segment
				Reset_Encoder_3_Count();
	        }
			break;

	}
	last_drive_tick_cnt = drive_tick_cnt; 
}
	

void runIRMode(void)
{
	static unsigned char j=0;
	static unsigned char k=0;
	static unsigned char ir_startup=1;

	//IR Mode - Start out going Forward which is really Reverse on the Robot
	static unsigned char ir_cmd = CMD3;

	//IR Command Mode
	if (CMD0_IN)		//IR Command #0 Active (FORWARD)
	{
		Pwm1_red = 1;
		Pwm2_red = Relay1_red = Relay2_red = 0;
		j = 0;
		//LeftDriveMotor = 127;
		//RightDriveMotor = 127;
		ir_cmd = CMD0;
	}
	else if 	(CMD1_IN)	//IR Command #1 Active (RIGHT JOG)
	{
		Pwm2_red = 1;
		Pwm1_red = Relay1_red = Relay2_red = 0;
		ir_cmd = CMD1;		
		j = 0;
	}
	else if 	(CMD2_IN)	//IR Command #2 Active (LEFT JOG)
	{
		Relay1_red = 1;
		Pwm1_red = Pwm2_red = Relay2_red = 0;
		ir_cmd = CMD2;		
		j = 0;
	}
	else if 	(CMD3_IN)	//IR Command #3 Active (REVERSE)
	{
		Relay2_red = 1;
		Pwm1_red = Pwm2_red = Relay1_red = 0;
		j = 0;
		//LeftDriveMotor = 127;
		//RightDriveMotor = 127;
		ir_cmd = CMD3;
	}
	
	if ((CMD0_IN + CMD1_IN + CMD2_IN + CMD3_IN) > 1)
	{
		//If no commands are active set IR command Mode to #0 - FORWARD					
		ir_cmd = CMD3;
	}
	
	printf("IR COMMAND: %d\r", ir_cmd);
	
	//IR State
	switch (ir_cmd)
	{
		case CMD0:		//Forward
			LeftDriveMotor = 127 + DEFAULT_IR_SPEED;
			RightDriveMotor = 127 + DEFAULT_IR_SPEED;
			//printf("IR COMMAND 0\r");
			///IR_Ramp(IR_FORWARD, LeftDriveMotor, RightDriveMotor);
			break;

		case CMD1:		//Turn Left for (j * 26.2 ms))
			//NOTE: Maybe turn 90 using GYRO instead of Timing JOG
			if (j++ < 5)
			{
				LeftDriveMotor = 32;
				RightDriveMotor = 160;
			}
			else
			{
				//LeftDriveMotor =  127 + DEFAULT_IR_SPEED;
				//RightDriveMotor =  127 + DEFAULT_IR_SPEED;
				LeftDriveMotor = 127;
				RightDriveMotor = 127;
				ir_cmd = CMD0;
			}
			break;
			
		case CMD2:		//Turn Right for (j * 26.2 ms))
			//NOTE: Maybe turn 90 using GYRO instead of Timing JOG
			if (j++ < 5)
			{
				LeftDriveMotor = 160;
				RightDriveMotor = 32;
			}
			else
			{
				LeftDriveMotor =  127 + DEFAULT_IR_SPEED;
				RightDriveMotor =  127 + DEFAULT_IR_SPEED;
			}
			break;
			
		case CMD3:		//Reverse
			if (k++ < 40 && ir_startup)
			{
				LeftDriveMotor = 127; //- DEFAULT_IR_SPEED;
				RightDriveMotor = 127; //- DEFAULT_IR_SPEED;
			}
			else
			{
				ir_startup = 0;
				LeftDriveMotor = 127 - DEFAULT_IR_SPEED; //FULL_IR_SPEED;
				RightDriveMotor = 127 - DEFAULT_IR_SPEED; //FULL_IR_SPEED;
			}
			//IR_Ramp(IR_REVERSE, LeftDriveMotor, RightDriveMotor);
			break;
	}
}

See our other robots at FRC Team 1640