This Year's Code

As I mentioned, the priority this year was speed of writing. Therefore, memory efficiency, processor efficiency, readability, reliability, re-usability, and neatness all took a serious hit. Please don't be alarmed! This year's code will be much neater and easier to follow. I would also like to shift some of the blame onto the other people listed at the top of the code, as some (but very little) of the blame lies on them. I assure you, however, that the code I write for my other programs is never this messy or convoluted.

Also, a joke:
There are 10 kinds of people in this world: Those who understand binary, those who don't, and those who THINK they understand binary

EDIT: The reading is made worse by the lack of proper indentation. This website, like most, seems to hate tabs. Perhaps it doesn't like the number nine...(did anyone see what I did there? )

/******************************************************************************
*
* Sprockets 2011 Code
* Authors: Duane Jeffery
* Matt Aceto
* Brendan van Ryn
* Terry Shi
* Stuart Sullivan
*
* Last Updated: 2/3/2011
*
******************************************************************************/

/* These two values select the stopping distances during autonomous. The first is for straighline, and the second is
* for the Y-branch. Simply replace the number beside the capitalized name and re-download the code. To do so, press
* Ctrl+Shift+A to compile, then select FIRST->Download from the top menu. Finally, make sure to reboot the robot. */
#define STRAIGHTLINEDISTANCE 240
#define BRANCHEDLINEDISTANCE 821

/* Left = back, right = front */

// Header files
#include "WPILib.h"
#include "vision\AxisCamera.h"
#include "math.h"
#include "stdlib.h"

// Drive system definitions. Use these to specify which drive system is in use.
#define MECANUM 32
#define SWERVE 51

// Handles the tedious driverstation output overhead
// Simply pass a number and it will be sent to the driver station
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()
#define DISPLAYMESSAGE(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a); DriverStationLCD::GetInstance()->UpdateLCD()
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Autonomous mode definitions. Use these to specify which autonomous mode to use.
#define STRAIGHT 0
#define BRANCH 1

// Determine which drive system we are using
// If the drive system is defined as MECANUM, the mecanum
// drive code is used. If it is defined as SWERVE, the swerve
// drive code is used. Otherwise, the compiler generates an error
#define DRIVESYSTEM SWERVE
#if (DRIVESYSTEM != MECANUM && DRIVESYSTEM != SWERVE) || !defined DRIVESYSTEM
#error "Drive system not specified"
#endif

// Ultrasonic stopping distance for autonomous
// Branch angle for y-autonomous
#define BRANCHSTOPDISTANCE 400
#define BRANCHANGLE -0.12

// Steering mode macros for the swerve drive function
#define STRAFE 0
#define TANK 1
#define MONSTER 2
#define CAR 3
#define AUTOMONSTER_LEFT 4
#define AUTOMONSTER_RIGHT 5

// Number of pulses per rotation on the steering encoders
// To acheive a pulse value, multiply an angle by this value
// AUTOALIGNSTOP is a time limit before the autoalign function should automatically exit
#define PULSES_PER_STEER (1020)
#define AUTOALIGNSTOP 1000

// Speed of claw rollers
#define CLAWROLLERSPEED 0.6

// Motor macros to be changed depending on whether we use Jaguars or Victors
#define LPDRIVEMOTOR Jaguar*
#define DRIVEMOTOR(a) Jaguar(a)

// Joystick deadbands
#define JOYSTICK_DEADBAND_X 0.0525f
#define JOYSTICK_DEADBAND_Y 0.100f

// Steering synchronization macros
#define STEERINGSYNCDEADBAND 5 // Manimum difference between encoders for synchronization to take effect
#define STEERINGSYNCDISTANCE 100 // Maximum distance between drive units before one steering motor is stopped completely
#define STEERINGSYNCDIVIDER 200 // Resolution of the steering synchronization code (smaller values means more frequent synchronization)

// When the lock button is pressed, multiply the drive speed by this to reduce chances of tipping
#define LOCKBUTTONLIMIT 0.625

// Button Macros
// An x indicates that an imaginary button was specified, such as for an untested feature
#define CLAWREVERSEBUTTON !clawStick->GetRawButton(1)
#define PIVOTPOSITIONHOME clawStick->GetRawButton(2)
#define PIVOTPOSITIONMIDDLE clawStick->GetRawButton(4)
#define PIVOTPOSITIONLOW clawStick->GetRawButton(80) // x
#define PIVOTPOSITIONFULL clawStick->GetRawButton(5)
#define EXTENDBUTTON clawStick->GetRawButton(6)
#define RETRACTBUTTON clawStick->GetRawButton(7)
#define LOCKBUTTONPRESSED leftStick->GetRawButton(2)
#define PIVOTDOWNBUTTON clawStick->GetRawButton(10)
#define PIVOTUPBUTTON clawStick->GetRawButton(11)
#define PIVOTPOSITIONHIGH clawStick->GetRawButton(30) // x
#define STEERALIGNBUTTON leftStick->GetRawButton(10)
#define MONSTERENABLEBUTTON leftStick->GetRawButton(1)
#define DEPLOYMINIBOT leftStick->GetRawButton(6)
#define RETRACTMINIBOT leftStick->GetRawButton(7)
#define SWERVEENABLEBUTTON rightStick->GetRawButton(1)
#define CARENABLEBUTTON rightStick->GetRawButton(20) // x
#define EXITSTEERALIGN leftStick->GetRawButton(11)
#define PICKUPBUTTON clawStick->GetRawButton(9)
#define DRIVEREVERSEOFF (leftStick->GetRawButton(4) || leftStick->GetRawButton(5) || leftStick->GetRawButton(3))
#define DRIVEREVERSEON (rightStick->GetRawButton(4) || rightStick->GetRawButton(5) || rightStick->GetRawButton(3))
#define PREPICKUPBUTTON clawStick->GetRawButton(

// Slot for second digital sidecar
#define DIGITALSIDECARTWO 6

// Autonomous travel time (how long should it strafe after the branch?)
#define AUTONOMOUSTRAVELTIME 3300

// Autonomous straight section time
#define AUTONOMOUSSTRAIGHT 1600

// Autonomous manipulation time
#define MANIPULATIONTIME 6000

// Delay between arriving at rack and deploying tube in autonomous
#define RETRACTTIME 3000

// Amount of time for the tube to be manipulated when the claw switch is triggered
#define AUTOMANIPULATETIME 1750

// Travel speed an angle along branch
#define BRANCHSPEED 0.5

// Port number macros in case things get moved around
#define RIGHTJOYSTICKPORT 1
#define LEFTJOYSTICKPORT 4
#define CLAWJOYSTICKPORT 3

#define LIGHTSENSORPORT1 1
#define LIGHTSENSORPORT2 2
#define LIGHTSENSORPORT3 3
#define LIGHTSENSORPORT4 13

#define ARMENCCHANNELA 7
#define ARMENCCHANNELB 6
#define LEFTSTEERINGENCODERA 12
#define LEFTSTEERINGENCODERB 11
#define RIGHTSTEERINGENCODERA 14
#define RIGHTSTEERINGENCODERB 13

#define EXTENDLIMITSWITCHPORT 4
#define RETRACTLIMITSWITCHPORT 8
#define PIVOTLIMITSWITCHUPPORT 10
#define PIVOTLIMITSWITCHDOWNPORT 9
#define MINIBOTLIMITSWITCHPORT 1

#define STEERINGLIMITFRONTRIGHT 2
#define STEERINGLIMITFRONTLEFT 3
#define STEERINGLIMITBACKRIGHT 4
#define STEERINGLIMITBACKLEFT 5

#define LFMOTORPORT 1
#define LRMOTORPORT 2
#define RFMOTORPORT 3
#define RRMOTORPORT 4
#define LEFTSTEERINGPORT 5
#define RIGHTSTEERINGPORT 6

#define EXTENDMOTORPORT 9
#define PIVOTMOTORPORT 10
#define CLAWTOPMOTORPORT 7
#define CLAWBOTTOMMOTORPORT 8

#define MINIBOTDEPLOYPORT 1
#define MINIBOTRETRACTPORT 2
#define BRAKEPORT 3
#define MINIBOTDEPLOYPORT2 4

#define COMPRESSORSWITCHPORT 12
#define COMPRESSORSPIKEPORT 8
#define CLAWSWITCHPORT 9

#define ULTRAPORT 3
#define ARMLIGHTSENSOR 10

#define SWERVEBLOCKENCODER1 8
#define SWERVEBLOCKENCODER2 6

#define AUTOSWITCHPORT 12


#define PREPICKUPEXTEND 35


// Arm positions
#define ARM_FULLUP 140
#define ARM_RACKTOP 120
#define ARM_RACKMIDDLE 30
#define ARM_RACKBOTTOM 12
#define ARM_HOME 0

// Arm extension limits
#define ARMEXTENDHOME 33
#define ARMEXTENDRACK 57

// Movement speed limits
#define ARMLIMITHEIGHT 25
#define ARMLIMITFACTOR 0.8

// Arm approach distances and speeds
#define ARMAPPROACHUP 25
#define ARMAPPROACHDOWN 25
#define ARMUPSPEED 1.0
#define ARMDOWNSPEED -1.0
#define ARMEXTENDSPEED 1
#define ARMDEADBAND 6

// Steering approach distance
#define STEERING_APPROACH_DISTANCE 100.0f
#define STEERING_DEADBAND 0.02f

// Function prototypes
float Abs(float); // Absolute value function

class MecanumDrive
{
public:
LPDRIVEMOTOR lfDrive; // Create a pointer to a Jag/Victor for the LF Motor
LPDRIVEMOTOR lrDrive; // Create a pointer to a Jag/Victor for the LR Motor
LPDRIVEMOTOR rfDrive; // Create a pointer to a Jag/Victor for the RF Motor
LPDRIVEMOTOR rrDrive; // Create a pointer to a Jag/Victor for the RR Motor
Victor* leftSteer; // Create a pointer to a Jag/Victor for the left steering motor
Victor* rightSteer; // Create a pointer to a Jag/Victor for the right steering motor
Victor* extendMotor; // Create a pointer to a victor for the arm extend motor
Victor* pivotMotor; // Create a pointer to a victor for the arm pivot motor
Victor* topclawmotor; // Create a pointer to a victor for the top claw motor
Victor* bottomclawmotor; // Create a pointer to a victor for the bottom claw motor
Encoder *leftSteeringEncoder; // Create a pointer to an encoder for the left steering motor
Encoder *rightSteeringEncoder; // Create a pointer to an encoder for the right steering motor
RobotDrive *robotDrive; // Create a pointer to a default drive system
DigitalInput *frontleftSteerLimit; // These limit switches prevent wheel rotation beyond limits
DigitalInput *frontrightSteerLimit; //
DigitalInput *backleftSteerLimit; //
DigitalInput *backrightSteerLimit; //
DigitalInput *ClawSwitch; // Limit switch on claw to detect tube presence
float output_speed; // Used by swerve drive
bool flag2, flag3; // Various flags used by swerve drive
int DriveReverse; // Either 1 or -1 for forward or reversed drive, respectively
float y_sign; // Sign of joystick y-movement for the drive reverse feature
int oldLeftEncoderValue, oldRightEncoderValue; // Hold the values of the encoders since the last time the swerve-synchronizing code was called
int trip; // Increment each time third sensor is tripped, to check for consistency and prevent noise from being a problem

public:
MecanumDrive(UINT32 lfMotor, UINT32 lrMotor, UINT32 rfMotor, UINT32 rrMotor)
{
lfDrive = new DRIVEMOTOR(lfMotor); // Create drive motor pointers
lrDrive = new DRIVEMOTOR(lrMotor);
rfDrive = new DRIVEMOTOR(rfMotor);
rrDrive = new DRIVEMOTOR(rrMotor);
extendMotor = new Victor(EXTENDMOTORPORT); // Create pointer to arm extend motor
pivotMotor = new Victor(PIVOTMOTORPORT); // Create pointer to arm pivot motor
topclawmotor = new Victor(CLAWTOPMOTORPORT); //Create a pointer to the top claw motor
bottomclawmotor = new Victor(CLAWBOTTOMMOTORPORT); // Create a pointer to the bottom claw motor
robotDrive = new RobotDrive(lfMotor, lrMotor, rfMotor, rrMotor); // The mecanum function assumes that a robot drive with this setup has been created
frontrightSteerLimit = new DigitalInput(DIGITALSIDECARTWO, STEERINGLIMITFRONTRIGHT); // Create steering limit switches
frontleftSteerLimit = new DigitalInput(DIGITALSIDECARTWO, STEERINGLIMITFRONTLEFT);
backrightSteerLimit = new DigitalInput(DIGITALSIDECARTWO, STEERINGLIMITBACKRIGHT);
backleftSteerLimit = new DigitalInput(DIGITALSIDECARTWO, STEERINGLIMITBACKLEFT);
ClawSwitch = new DigitalInput(DIGITALSIDECARTWO, CLAWSWITCHPORT); // Create a limit switch to detect tube presence in claw

leftSteeringEncoder = new Encoder(LEFTSTEERINGENCODERA, LEFTSTEERINGENCODERB); // Encoders to track rotation positions of motors
rightSteeringEncoder = new Encoder(RIGHTSTEERINGENCODERA, RIGHTSTEERINGENCODERB);
leftSteer = new Victor(LEFTSTEERINGPORT); // Motor controller for steering movement
rightSteer = new Victor(RIGHTSTEERINGPORT);
DriveReverse = 1; // Store the state of the drive reverse feature (1 = forward)
reset_variables(); // Reset and start all encoders and clear various state variables

}//End of MecanumDrive constructor

// Track the line and check the ultrasonic sensor distance for autonomous
int MoveSwerve(int leftSensor, int rightSensor, int endOfLineSensor, int armPivot, AnalogChannel *ultraDistance, int AutoMode)
{
int AUTOSTOPDISTANCE; // Stores the distance to stop (as read by the ultrasonic sensor)

if(AutoMode) AUTOSTOPDISTANCE = STRAIGHTLINEDISTANCE; // Straightline stopping distance
else AUTOSTOPDISTANCE = BRANCHEDLINEDISTANCE; // Y-branch stopping distance

// If we are within the decided distance from the wall
if(ultraDistance->GetVoltage() * 1000 < AUTOSTOPDISTANCE)
{
// Increment a trip counter
trip++;

// If we have been within the given distance for long enough, we can hopefully
// Rule out error on the part of the sensor, and stop, returning our state
if(trip > 320)
{
lfDrive->Set(0);
lrDrive->Set(0);
rfDrive->Set(0);
rrDrive->Set(0);

// Robot should stop moving
return 1;
}
}

// Anything too short to reach 320 was probably just interference from debris, so lower the trip value again
else if(trip > 0) trip--;

// If the right sensor hits the line
if (rightSensor)
{
// Stop the left motors
lfDrive->Set(0);
lrDrive->Set(0);
rfDrive->Set(0.9);
rrDrive->Set(0.9);
}

// If the right sensor has not hit the line, but the left one has,
// set the motors to turn right
else if (leftSensor)
{
lfDrive->Set(0.9);
lrDrive->Set(0.9);
rfDrive->Set(0);
rrDrive->Set(0);
}

// If neither sensor has hit the line, move forward normally
else
{
lfDrive->Set(0.4);
lrDrive->Set(0.4);
rfDrive->Set(0.4);
rrDrive->Set(0.4);
}

// Robot should continue moving
return 0;
}

// Swerve drive function
void DriveSwerve(Joystick *leftStick, Joystick *rightStick, Joystick *clawStick, DigitalInput *ArmPivotLimitDown, DigitalInput *ArmPivotLimitUp, DigitalInput *ArmRetractLimit, DigitalInput *ArmExtendLimit, Encoder *ArmEncoder, int ArmTarget, int ArmPosition, char AutonomousOverride, int ArmPickup, int ArmDeploy, int ArmExtendTarget)
{
static int monsterenabled = 0, carenabled = 0, align = 0, rotateSwitch = 0; // Steering modes and alignment state variables
static int syncDivider = 0; // We only call run the synchronization code periodically
static int aligntime = 0; // We need to give the wheels a little extra time after they reach their goal
float right_x, right_y, left_x, left_y, clawy; // Displacements of joystick axes
int SteerMode; // Between TANK, MONSTER, STRAFE, AUTOMONSTER(RIGHT/LEFT), and CAR

if(DRIVEREVERSEON) DriveReverse = -1;
if(DRIVEREVERSEOFF) DriveReverse = 1;

if (AutonomousOverride == 1)
{
right_x = left_x = -0.32;
right_y = left_y = 0.45;
}

else if (AutonomousOverride == 3)
{
right_x = left_x = 0.32;
right_y = left_y = 0.45;
}

else if (AutonomousOverride == 2)
{
// Return wheels to home and stop
right_x = 0.01;
right_y = 0.01;
left_x = 0.01;
left_y = 0.01;
}

// If the wheels are turned full left, indicating the end of the first stage of auto-align,
// reset the encoders so that this new position is considered center. Continue to stage three
else if (align == 2)
{
align = 3;
DriverStationLCD::GetInstance()->Clear();
DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line1, 1, "mode 2");
DriverStationLCD::GetInstance()->UpdateLCD();
rightSteeringEncoder->Reset();
leftSteeringEncoder->Reset();
}

// If we are in the final stage of wheel alignment, we set the goal to "full-right", which should translate to centre
// because the wheels are turned full-left.
// Once the wheels are centred, we reset the encoders again and exit wheel-alignment mode
else if (align >= 3)
{
DriverStationLCD::GetInstance()->Clear();
DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line1, 1, "mode 3");
DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line2, 1, "%d",
leftSteeringEncoder->Get());
DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line3, 1, "%d",
rightSteeringEncoder->Get());
DriverStationLCD::GetInstance()->UpdateLCD();

right_x = 1.0;
left_x = 1.0;
right_y = 0.01;
left_y = 0.01;

// If the steering motors have finished centering, reset them to center
// and exit auto-align mode
if(leftSteeringEncoder->Get() > AUTOALIGNSTOP && rightSteeringEncoder->Get() < -AUTOALIGNSTOP)
{
aligntime--;
if(aligntime < 1)
{
leftSteeringEncoder->Reset();
rightSteeringEncoder->Reset();
align = 0;
}
}

else aligntime++;
}

else
{
// Read position of left and right drive joysticks
right_x = rightStick->GetRawAxis(1);
right_y = rightStick->GetRawAxis(2);
left_x = leftStick->GetRawAxis(1);
left_y = leftStick->GetRawAxis(2);
}

// Get position of claw y-axis
clawy = clawStick->GetRawAxis(2);

// If the claw stick is less than the deadband, ignore it
if (Abs(clawy) <= JOYSTICK_DEADBAND_Y)
clawy = 0.0f;

// Check the monster/car/swerve buttons
if (SWERVEENABLEBUTTON)
monsterenabled = 0, carenabled = 0;
else if (MONSTERENABLEBUTTON)
monsterenabled = 1, carenabled = 0;
else if (CARENABLEBUTTON)
monsterenabled = 0, carenabled = 1;

// If the auto-align button is pressed, set the align flag and enter strafe mode
if (STEERALIGNBUTTON)
{
align = 1;
SteerMode = STRAFE;
monsterenabled = 0;
carenabled = 0;
}

// In case something goes wrong, this button exits auto-align mode prematurely
if (EXITSTEERALIGN && align > 0)
{
leftSteeringEncoder->Reset();
rightSteeringEncoder->Reset();
align = 0;
}

// If the arm should extend, set the motor forward
// If the arm should retract, set the motor backward
// Otherwise, stop any previous motion
if ((EXTENDBUTTON || ArmPickup == 1) && ArmExtendLimit->Get())
extendMotor->Set(1);
else if ((RETRACTBUTTON || ArmDeploy || ArmPickup == 2) && ArmRetractLimit->Get())
extendMotor->Set(-1);
else if(ArmExtendTarget == -1) // *&^%$#@!~`
extendMotor->Set(0);

// If the arm should pivot up, and the arm has not hit the limit switch, set the motor forward
// If the arm should pivot down, and the arm has not hit the limit switch, set the motor backward
// Otherwise, stop any previous motion
if (PIVOTUPBUTTON && ArmPivotLimitUp->Get())
pivotMotor->Set(ARMUPSPEED);
else if (PIVOTDOWNBUTTON && ArmPivotLimitDown->Get())
pivotMotor->Set(ARMDOWNSPEED);
else if (ArmTarget == -1)
pivotMotor->Set(0);

// If the arm hits the lower limit switch, reset the encoder
if (!ArmPivotLimitDown->Get())
ArmEncoder->Reset();

if(!ClawSwitch->Get() && clawy > 0 && !CLAWREVERSEBUTTON) clawy = 0;

/*// If the tube triggers the claw switch, enter rotate mode
// Otherwise, exit and reset the time counter
if(!ClawSwitch->Get()) rotateSwitch = 1;
else rotateSwitch = 0, rotateTime = 0;*/

// If it's in auto-pickup mode, the rollers should suck in
// Unless the switch has been hit
if(ArmPickup == 1 && !rotateSwitch)
{
topclawmotor->Set(-1);
bottomclawmotor->Set(1);
}

/*// If the switch has been hit, manipulate the tube for a period of time
// to point it upward
else if(ArmPickup && rotateTime < AUTOMANIPULATETIME)
{
rotateTime++;
topclawmotor->Set(-1);
bottomclawmotor->Set(-1);
}*/

// If it's in auto-deploy mode, the rollers should eject
else if(ArmDeploy)
{
topclawmotor->Set(1);
bottomclawmotor->Set(-1);
}

// If the trigger on the claw stick is pressed, set both motors in the same
// direction, according to the joystick axis
else if(!CLAWREVERSEBUTTON)
{
topclawmotor->Set(-clawy);
bottomclawmotor->Set(clawy);
}

else
{
// Otherwise, spin the motors inverse to each other.
// The motor that spins outward travels more slowly to
// prevent the tube from sliding out.
if(clawy > 0)
{
topclawmotor->Set(-clawy);
bottomclawmotor->Set(-clawy / 2);
}

else
{
topclawmotor->Set(-clawy / 2);
bottomclawmotor->Set(-clawy);
}
}

// If both joysticks have been tilted sideways by more than the dead band amount and monster mode is enabled, set to monster mode
// Otherwise, if both joysticks have been tilted sideways by more than the dead band amount, set to strafe mode
// Otherwise, set to tank mode
if ((Abs(right_x) > JOYSTICK_DEADBAND_X && Abs(left_x) > JOYSTICK_DEADBAND_X) || LOCKBUTTONPRESSED)
{
if (monsterenabled)
SteerMode = MONSTER;
else if (carenabled)
SteerMode = CAR;
else
SteerMode = STRAFE;
}

// If the left joystick points forward and the right points backward, enter full-angle monster clockwise
else if(left_y > JOYSTICK_DEADBAND_Y && right_y < -JOYSTICK_DEADBAND_Y)
{
SteerMode = AUTOMONSTER_RIGHT;
}

// If the right joystick points backward and the left joystick points backward, enter full-angle monster counter-clockwise
else if(left_y < -JOYSTICK_DEADBAND_Y && right_y > JOYSTICK_DEADBAND_Y)
{
SteerMode = AUTOMONSTER_LEFT;
}

else SteerMode = TANK;

//*******************************************************************************************
//STEERING ALIGNMENT
//*******************************************************************************************
// If steering alignment buttons are held on joystick and the auto-align mode is not running
if (rightStick->GetRawButton( && rightStick->GetRawButton(9) && !align)
{

if (rightStick->GetRawButton(6))
{ //move left steering to right
if (backrightSteerLimit->Get())
leftSteer->Set(-0.4);
else
leftSteer->Set(0);

leftSteeringEncoder->Stop();
leftSteeringEncoder->Reset();
flag2 = true; //***j
}

else if (rightStick->GetRawButton(7))
{ //move lrft steering to left
if (backleftSteerLimit->Get())
leftSteer->Set(0.4);
else
leftSteer->Set(0);

leftSteeringEncoder->Stop();
leftSteeringEncoder->Reset();
flag2 = true; //***j
}

else if (flag2)
{
leftSteer->Set(0);
leftSteeringEncoder->Start();
flag2 = false; //***j
}

if (rightStick->GetRawButton(10))
{ //move right steering to right
if (frontrightSteerLimit->Get())
rightSteer->Set(-0.4);
else
rightSteer->Set(0);

rightSteeringEncoder->Stop();
flag3 = true;
rightSteeringEncoder->Reset();
} else if (rightStick->GetRawButton(11))
{ //move right steering to left
if (frontleftSteerLimit->Get())
rightSteer->Set(0.4);
else
rightSteer->Set(0);

rightSteeringEncoder->Stop();
flag3 = true;
rightSteeringEncoder->Reset();
} else if (flag3)
{
rightSteer->Set(0);
rightSteeringEncoder->Start();
flag3 = false;
}
} //end of if steering alignment buttons are held on joystick

// If auto-align has been initiated, move until both limit switches are hit
else if (align == 1)
{

DriverStationLCD::GetInstance()->Clear();
DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line1, 1, "mode 1");
DriverStationLCD::GetInstance()->UpdateLCD();

if (frontleftSteerLimit->Get())
rightSteer->Set(0.5);
else
rightSteer->Set(0);

if (backrightSteerLimit->Get())
leftSteer->Set(-0.5);
else
leftSteer->Set(0);

if (!rightSteer->Get() && !leftSteer->Get())
align = 2;
}

//if steering position must come from drive joystick position (x,y)
else
{
//read position of each steering encoder
float leftSteeringEncoderVal = leftSteeringEncoder->Get();
float rightSteeringEncoderVal = rightSteeringEncoder->Get();
float leftSyncPercent = 1.0f; // These are used to scale the steering motor speeds
float rightSyncPercent = 1.0f; // to synchronize them

//CLEARMESSAGE;
//DISPLAYMESSAGE(2, (int)(leftSteeringEncoderVal));
//DISPLAYMESSAGE(3, (int)(rightSteeringEncoderVal));

//normalize the values (convert from counted integer >> real number from +1 to -1)
leftSteeringEncoderVal = leftSteeringEncoderVal / PULSES_PER_STEER;
rightSteeringEncoderVal = rightSteeringEncoderVal / PULSES_PER_STEER;

float leftgoal = 0;
float rightgoal = 0;

syncDivider++; // Count another call

// If enough of a delay has been incurred already, perform the synchronization code
if(syncDivider > STEERINGSYNCDIVIDER)
{
// Reset the divider
syncDivider = 0;

// Determine which steering unit is turning faster
int deltaLeft;
int deltaRight;

deltaLeft = oldLeftEncoderValue - (int)leftSteeringEncoderVal;
deltaRight = oldRightEncoderValue - (int)rightSteeringEncoderVal;
oldLeftEncoderValue = (int)leftSteeringEncoderVal;
oldRightEncoderValue = (int)rightSteeringEncoderVal;

if(abs(deltaLeft) > abs(deltaRight) + STEERINGSYNCDEADBAND && abs(deltaRight) > STEERINGSYNCDEADBAND)
{
// Left is moving too fast, so adjust
leftSyncPercent = (float)abs(oldLeftEncoderValue - oldRightEncoderValue) / STEERINGSYNCDISTANCE;

if(leftSyncPercent > 1.0f) leftSyncPercent = 1.0f;
else if(leftSyncPercent < 0.0f) leftSyncPercent = 0.0f;

leftSyncPercent = 1.0f - leftSyncPercent;
}

else if(abs(deltaRight) > abs(deltaLeft) + STEERINGSYNCDEADBAND && abs(deltaLeft) > STEERINGSYNCDEADBAND)
{
// Right is moving too fast, so adjust
rightSyncPercent = (float)abs(oldLeftEncoderValue - oldRightEncoderValue) / STEERINGSYNCDISTANCE;

if(rightSyncPercent > 1.0f) rightSyncPercent = 1.0f;
else if(rightSyncPercent < 0.0f) rightSyncPercent = 0.0f;

rightSyncPercent = 1.0f - rightSyncPercent;
}
}

if (SteerMode == TANK)
{
//point left wheels straignt towards the wide front
leftgoal = 0;
rightgoal = 0;

// Reverse the drive if necessary (DriveReverse is either 1 or -1). This is only for tank
left_y *= DriveReverse;
right_y *= DriveReverse;

// Swap the left and right drive settings if the drive has been reversed
if(DriveReverse == -1)
{
float temp;

temp = left_x;
left_x = right_x;
right_x = temp;
}

// If the arm is above a certain height, reduce movement sensitivity
if(ArmEncoder->Get() > ARMLIMITHEIGHT)
{
lfDrive->Set(left_y * ARMLIMITFACTOR);
lrDrive->Set(left_y * ARMLIMITFACTOR);
rfDrive->Set(right_y * ARMLIMITFACTOR);
rrDrive->Set(right_y * ARMLIMITFACTOR);
}

// Otherwise, don't limit speed
else
{
lfDrive->Set(left_y);
lrDrive->Set(left_y);
rfDrive->Set(right_y);
rrDrive->Set(right_y);
}
}

else if (SteerMode == STRAFE || SteerMode == MONSTER || SteerMode
== CAR || SteerMode == AUTOMONSTER_LEFT || SteerMode == AUTOMONSTER_RIGHT)
{
float deflection = 0;
float steer_angle = 0;
short quadrant = 0;
float y_avg = 0;

y_avg = (right_y + left_y) / 2;
if(y_avg == 0.0f) y_avg = JOYSTICK_DEADBAND_Y / 100;
y_sign = y_avg / Abs(y_avg); y_sign *= -1;

float x_avg;
x_avg = (left_x + right_x) / 2;

if(Abs(y_avg) > 0.8 && SteerMode == MONSTER)
{
SteerMode = CAR;
}

//determine output speed based on net joyustick deflection
deflection = sqrt((y_avg * y_avg) + (x_avg * x_avg)); //Pythagorean Theorem

//keep number within range. Ignore corners of joystick motion
if (deflection > 1)
{
deflection = 1;
}

else if (deflection < -1)
{
deflection = -1;
}

// If we are in auto-monster, the deflection should be set to the difference between the two joystick positions
if(SteerMode == AUTOMONSTER_LEFT || SteerMode == AUTOMONSTER_RIGHT) deflection = Abs(left_y - right_y) / 2;

if (align >= 1)
{ //no drive during alignment mode
deflection = 0;
}

// If the arm is above a certain height, reduce drive sensitivity
if(ArmEncoder->Get() > ARMLIMITHEIGHT) deflection *= ARMLIMITFACTOR;

//determine steering position based on joystick angle (measured in radians)
steer_angle = atan(Abs(x_avg) / Abs(y_avg)); //trigonometry Tan-1 = Opp / Adj

//conversion from radians to degrees
steer_angle = steer_angle * (180 / 3.14159);

//convert to normalized value (between forward >> 0.85 and sideways >> 0)
steer_angle = (steer_angle * 0.85) / 90;

// Reverse drive direction if necessary
deflection *= DriveReverse;

// If the lock button is pressed, we use a special quadrant
if(LOCKBUTTONPRESSED)
{
quadrant = 7;
SteerMode = STRAFE;
deflection *= LOCKBUTTONLIMIT;
}

//if joysticks are pointed in the right straight deadband
else if(x_avg > 0 && Abs(y_avg) <= JOYSTICK_DEADBAND_Y)
{
quadrant = 6;
}

//if joysticks are pointed in the left straight deadband
else if (x_avg < 0 && Abs(y_avg) <= JOYSTICK_DEADBAND_Y)
{
quadrant = 5;
}

//if joysticks are pointed in 4th quadrant
else if (x_avg > 0 && y_avg >= 0)
{
//to make driving easier, when the arm is up, the arm side becomes the "front"
//when the arm is down, the kicker side becomes the "front"
quadrant = 4;
}

//if joysticks are pointed in 3rd quardant
else if (x_avg < 0 && y_avg > 0)
{
quadrant = 3;
}

//if joysticks are pointed in 2nd quardant
else if (x_avg < 0 && y_avg <= 0)
{
quadrant = 2;
}

//if joysticks are pointed in 1st quadrant
else
{
quadrant = 1;
}

//set direction to drive and angle to steering based on quardant of motion
if (quadrant == 4)
{
rfDrive->Set(deflection);
rrDrive->Set(deflection);
lfDrive->Set(deflection);
lrDrive->Set(deflection);
leftgoal = steer_angle * -1;
rightgoal = steer_angle;
}

else if (quadrant == 3)
{
rfDrive->Set(deflection);
rrDrive->Set(deflection);
lfDrive->Set(deflection);
lrDrive->Set(deflection);
leftgoal = steer_angle;
rightgoal = steer_angle * -1;
}

else if (quadrant == 2)
{
rfDrive->Set(deflection * -1);
rrDrive->Set(deflection * -1);
lfDrive->Set(deflection * -1);
lrDrive->Set(deflection * -1);
leftgoal = steer_angle * -1;
rightgoal = steer_angle;
}

else if (quadrant == 1)
{
rfDrive->Set(deflection * -1);
rrDrive->Set(deflection * -1);
lfDrive->Set(deflection * -1);
lrDrive->Set(deflection * -1);
leftgoal = steer_angle;
rightgoal = steer_angle * -1;
}

else if (quadrant == 5)
{
if (align >= 1 || Abs(leftSteer->Get()) > 0.05)
{
rfDrive->Set(0);
rrDrive->Set(0);
lfDrive->Set(0);
lrDrive->Set(0);
DISPLAYSTRING(4, "Wheels turning");
}

else
{
rfDrive->Set(deflection * -1);
rrDrive->Set(deflection * -1);
lfDrive->Set(deflection * -1);
lrDrive->Set(deflection * -1);
DISPLAYSTRING(4, "Wheels stopped");
}

leftgoal = -0.85;
rightgoal = 0.85;
}

else if (quadrant == 6)
{
if(align >= 1 || Abs(leftSteer->Get()) > 0.05)
{
rfDrive->Set(0);
rrDrive->Set(0);
lfDrive->Set(0);
lrDrive->Set(0);
DISPLAYSTRING(4, "Wheels turning");
}
else
{
rfDrive->Set(deflection * -1);
rrDrive->Set(deflection * -1);
lfDrive->Set(deflection * -1);
lrDrive->Set(deflection * -1);
DISPLAYSTRING(4, "Wheels stopped");
}

leftgoal = 0.85;
rightgoal = -0.85;
}

// If we are in lock-mode
else if(quadrant == 7)
{
// If auto-align is enabled or the steering is still turning
// stop the drive motors
if(align >= 1 || Abs(leftSteer->Get()) > 0.05)
{
rfDrive->Set(0);
rrDrive->Set(0);
lfDrive->Set(0);
lrDrive->Set(0);
DISPLAYSTRING(4, "Wheels turning");
}

// Otherwise, use the direct joystick x-values to set the motor speed
else
{
rfDrive->Set(-x_avg);
rrDrive->Set(-x_avg);
lfDrive->Set(-x_avg);
lrDrive->Set(-x_avg);
DISPLAYSTRING(4, "Wheels stopped");
}

// Set goal position for steering to full right
leftgoal = 0.85;
rightgoal = -0.85;
}
} //end of if steering in strafe mode

//if MONSTER is steermode, invert the leftsteer goal
if(SteerMode == MONSTER)
{
leftgoal *= -1;
leftgoal *= DriveReverse;
rightgoal *= DriveReverse;
leftgoal *= y_sign;
rightgoal *= y_sign;
leftgoal *= 0.75;
rightgoal *= 0.75;
}

// In the case of either auto-monster, the deflection has already been calculated,
// so simply set the wheels to point either left and right or right and left in
// monster style
else if(SteerMode == AUTOMONSTER_LEFT)
{
leftgoal = -0.58;
rightgoal = -0.58;
leftgoal *= DriveReverse;
rightgoal *= DriveReverse;
}

else if(SteerMode == AUTOMONSTER_RIGHT)
{
leftgoal = 0.58;
rightgoal = 0.58;
leftgoal *= DriveReverse;
rightgoal *= DriveReverse;
}

// Otherwise, if car is enabled, centre the rear wheels
else if(SteerMode == CAR)
{
if(lfDrive->Get() > 0)
{
rightgoal = 0.0f;
leftgoal = leftgoal /2;
}
else
{
leftgoal = 0.0f;
rightgoal = rightgoal /2;
}
}

// If the drive has been reversed, the angle of the strafe should also be inverted
//leftgoal *= DriveReverse;
//rightgoal *= DriveReverse;

//if left steering has reached it's target position
if(Abs(leftSteeringEncoderVal - leftgoal) < STEERING_DEADBAND)
{
//turn left steering motor off
leftSteer->Set(0);
}

//if left steering motor is expected to move
else
{
float speedincrement = 1/STEERING_APPROACH_DISTANCE;
//if left steering motor must move left to reach target
if(leftSteeringEncoderVal < leftgoal)
{
// If the left-rear limit switch is hit and we're travelling left, stop
if(!backleftSteerLimit->Get()) leftSteer->Set(0);

//if left steering motor is within approach distance
else if(leftSteeringEncoderVal + (STEERING_APPROACH_DISTANCE/PULSES_PER_STEER) >= leftgoal)
{
//adjust speed proportinal to distance from target
//printf("Close... Going Right...\r\n");
leftSteer->Set(speedincrement * Abs((PULSES_PER_STEER*leftgoal) - (PULSES_PER_STEER*leftSteeringEncoderVal)));
}
//if left steering motor is far from target
else
{
//go top speed
//printf("Far... Going Right...\r\n");
leftSteer->Set(0.9 * leftSyncPercent);
}
}

//if left steering motor must move right to reach target
else
{
// If the steering motor must move right and the right-rear limit switch it hit, stop
if(!backrightSteerLimit->Get()) leftSteer->Set(0);

//if left steering motor is within approach distance
else if(leftSteeringEncoderVal - (STEERING_APPROACH_DISTANCE/PULSES_PER_STEER) <= leftgoal)
{
//adjust speed proportinal to distance from target
//printf("Close... Going Left...\r\n");
leftSteer->Set(-1 * speedincrement * Abs((PULSES_PER_STEER*leftgoal) - (PULSES_PER_STEER*leftSteeringEncoderVal)));
}
//if left steering motor is far from target
else
{
//go top speed
//printf("Far... Going Left...\r\n");
leftSteer->Set(-0.9 * leftSyncPercent);
}
}
} //end of if left steering motor is expected to move


//if right steering has reached it's target position
if(Abs(rightSteeringEncoderVal - rightgoal) < STEERING_DEADBAND)
{
//turn right steering motor off
rightSteer->Set(0);
}

//if right steering motor is expected to move
else
{
float speedincrement = 1/STEERING_APPROACH_DISTANCE;

//if right steering motor must move left to reach target
if(rightSteeringEncoderVal < rightgoal)
{
// If the front steering motor must move left and the front-left limit switch is triggered, stop
if(!frontleftSteerLimit->Get()) rightSteer->Set(0);

//if right steering motor is within approach distance
else if(rightSteeringEncoderVal + STEERING_APPROACH_DISTANCE >= rightgoal)
{
//adjust speed proportinal to distance from target
rightSteer->Set(speedincrement * Abs((PULSES_PER_STEER*rightgoal) - (PULSES_PER_STEER*rightSteeringEncoderVal)));
}
//if right steering motor is far from target
else
{
//go top speed
rightSteer->Set(0.9 * rightSyncPercent);
}

}
//if right steering motor must move right to reach target
else
{
// If the front steering motor must move right and the front-right limit switch is triggered, stop
if(!frontrightSteerLimit->Get()) rightSteer->Set(0);

//if right steering motor is within approach distance
if(rightSteeringEncoderVal - STEERING_APPROACH_DISTANCE <= rightgoal)
{
//adjust speed proportinal to distance from target
rightSteer->Set(-1 * speedincrement * Abs((PULSES_PER_STEER*rightgoal) - (PULSES_PER_STEER*rightSteeringEncoderVal)));
}
//if right steering motor is far from target
else
{
//go top speed
rightSteer->Set(-0.9 * rightSyncPercent);
}
}
} //end of if right steering motor is expected to move
// Display steering information
/*CLEARMESSAGE;
DISPLAYMESSAGE(1, (int)leftgoal * 100);
DISPLAYMESSAGE(2, (int)rightgoal * 100);
DISPLAYMESSAGE(3, (int)(lfDrive->Get()*100.0f));
DISPLAYMESSAGE(4, (int)(rrDrive->Get()*100.0f));*/
//DISPLAYMESSAGE(3, SteerMode);
//DISPLAYMESSAGE(4, monsterenabled && carenabled);
} //end of if steering position must come from joystick position (x,y)
} // End of DriveSwerve function

// Reset swerve-drive related variables
void reset_variables(void)
{
output_speed = 0;
oldLeftEncoderValue = 0;
oldRightEncoderValue = 0;
flag2 = false;
flag3 = false;
leftSteeringEncoder->Reset(); //***i
leftSteeringEncoder->Start(); //***i
rightSteeringEncoder->Reset(); //***i
rightSteeringEncoder->Start(); //***i
} // End of reset_variables

// End of "which drive system are we using?"
}; // End of MecanumDrive class

class SimpleTracker : public SimpleRobot
{
MecanumDrive *myRobot; // Pointer to drive system
Joystick *rightStick; // Right joystick
Joystick *leftStick; // Left joystick
Joystick *clawStick; // Claw control joystick
DigitalInput *LightSensor1; // Light sensors - Left sensor
DigitalInput *LightSensor2; // - End of line sensor
DigitalInput *LightSensor3; // - Right sensor
DigitalInput *LightSensor4; // Reverse sensor
Encoder *ArmEncoder; // Encoder for arm
Encoder *BlockEncoder;// Encoder for the steering block
DigitalInput *ArmRetractLimit; // Telescoping retraction limit switch
DigitalInput *ArmExtendLimit; // Telescoping extention limit switch
DigitalInput *ArmPivotLimitDown; // Pivoting limit switch (home)
DigitalInput *ArmPivotLimitUp; // Pivoting limit switch (full up)
DigitalInput *MinibotLimit; // Minibot deployment limit switch
DigitalInput *autonomousSetting; // A jumper to select autonomous modes
DigitalInput *ArmLightSensor; //The light sensor used for the arm (a getto encoder)
Victor *ArmExtend; // Arm Extend Motor
Victor *ArmPivot; // Arm pivot motor
Victor* topclawmotor; // Create a pointer to a victor for the top claw motor
Victor* bottomclawmotor; // Create a pointer to a victor for the bottom claw motor
Solenoid *minibotDeploy; // Pointer to solenoid to deploy minibot
Solenoid *minibotRetract; // Pointer to solenoid to retract minibot
Solenoid *engageBrake; // Pointer to solenoid to engage the brake
Solenoid *minibotDeploy2; // Pointer to second solenoid to deploy minibot
AnalogChannel *autoSwitch; // Pointer to autonomous analog switch
Compressor *theCompressor; // Pointer to compressor
Relay *compressor; // Compressor spike
DigitalInput *pressureSwitch; // Pressure switch to control compressor
int ArmEncoderValue; // Current arm encoder position
int BlockEncoderValue;
int ExtendEncoderValue; // Current arm extension position
DigitalInput *ClawSwitch; // Limit switch in claw motor
DigitalInput *AutonomousSwitch; // Autonomous mode switch
AnalogChannel *ultraDistance; // Ultrasonic distance sensor for autonomous

public:
SimpleTracker(void)
{
myRobot = new MecanumDrive(LFMOTORPORT, LRMOTORPORT, RFMOTORPORT, RRMOTORPORT); // Instantiate drive system
rightStick = new Joystick(RIGHTJOYSTICKPORT); // Create the joysticks
leftStick = new Joystick(LEFTJOYSTICKPORT);
clawStick = new Joystick(CLAWJOYSTICKPORT);
LightSensor1 = new DigitalInput(LIGHTSENSORPORT1); // Create the light sensors
LightSensor2 = new DigitalInput(LIGHTSENSORPORT2);
LightSensor3 = new DigitalInput(LIGHTSENSORPORT3);
LightSensor4 = new DigitalInput(DIGITALSIDECARTWO, LIGHTSENSORPORT4);
ArmEncoder = new Encoder(ARMENCCHANNELA, ARMENCCHANNELB); // Create encoder to gauge arm position
BlockEncoder = new Encoder(DIGITALSIDECARTWO, SWERVEBLOCKENCODER1, DIGITALSIDECARTWO, SWERVEBLOCKENCODER2);
ArmRetractLimit = new DigitalInput(RETRACTLIMITSWITCHPORT); // Create limit switch for arm retraction
ArmExtendLimit = new DigitalInput(EXTENDLIMITSWITCHPORT); //Create a limit switch for arm extention
ArmPivotLimitUp = new DigitalInput(PIVOTLIMITSWITCHUPPORT); // Create limit switch for arm pivoting (full up)
ArmPivotLimitDown = new DigitalInput(PIVOTLIMITSWITCHDOWNPORT); // Create limit switch for arm pivoting (full down)
MinibotLimit = new DigitalInput(DIGITALSIDECARTWO, MINIBOTLIMITSWITCHPORT); // Create limit switch for minibot deployment
ArmLightSensor = new DigitalInput(DIGITALSIDECARTWO, ARMLIGHTSENSOR); // Creates a lightsensor fo the arm "encoder"
ArmPivot = new Victor(PIVOTMOTORPORT); // This class needs access to the motor as well
ArmExtend = new Victor(EXTENDMOTORPORT); // This class needs access to the extending motor as well
topclawmotor = new Victor(CLAWTOPMOTORPORT); //Create a pointer to the top claw motor
bottomclawmotor = new Victor(CLAWBOTTOMMOTORPORT); // Create a pointer to the bottom claw motor
minibotDeploy = new Solenoid(MINIBOTDEPLOYPORT);
minibotDeploy2 = new Solenoid(MINIBOTDEPLOYPORT2);
minibotRetract = new Solenoid(MINIBOTRETRACTPORT);
engageBrake = new Solenoid(BRAKEPORT);
//autoSwitch = new AnalogChannel(AUTOSWITCHPORT);
autonomousSetting = new DigitalInput(DIGITALSIDECARTWO, AUTOSWITCHPORT);
compressor = new Relay(DIGITALSIDECARTWO, COMPRESSORSPIKEPORT);
pressureSwitch = new DigitalInput(DIGITALSIDECARTWO, COMPRESSORSWITCHPORT);
ClawSwitch = new DigitalInput(DIGITALSIDECARTWO, CLAWSWITCHPORT); // Create a limit switch to detect tube presence in claw
ultraDistance = new AnalogChannel(ULTRAPORT);
AutonomousSwitch = new DigitalInput(DIGITALSIDECARTWO, AUTOSWITCHPORT);


//theCompressor = new Compressor(DIGITALSIDECARTWO, COMPRESSORSWITCHPORT, DIGITALSIDECARTWO, COMPRESSORSPIKEPORT);

// Start recording pulses
ArmEncoder->Start();
ArmEncoder->Reset();

BlockEncoder->Start();
BlockEncoder->Reset();

ExtendEncoderValue = 0; // Initialize extension encoder to start position
// Enable the compressor
//theCompressor->Start();
//Image* cameraImage = frcCreateImage(IMAQ_IMAGE_HSL);
} // End of SimpleTracker constructor

void OperatorControl(void)
{
int minibotDeployed = 0;
int ArmTarget = -1; // Target pivot position for arm
int ExtendTarget = -1; // Target extension position for arm
int ArmPickup = 0; // Pickup enabled?
int ArmDeploy = 0; // Auto-deploy enabled?
int prevlight = 0, curlight = 0; // Previous and current values of arm extension light sensor
float prevarmext = 0; // Store whether the arm was moving forward or backward before it stopped

// Reset encoder on arm pivot
//ArmEncoder->Reset();
//ArmTarget = -1;

BlockEncoder->Start();
BlockEncoder->Reset();

// While in operator control
while (IsOperatorControl())
{
// Reset the "watchdog" timer to show that the processor hasn't frozen
GetWatchdog().Feed();

float voltage = ultraDistance->GetVoltage();
CLEARMESSAGE;
DISPLAYMESSAGE(5, (int)(voltage * 1000.0f));

BlockEncoderValue = BlockEncoder->Get();
//DISPLAYMESSAGE(1, BlockEncoderValue);
//DISPLAYMESSAGE(2, ArmEncoderValue);

//DISPLAYMESSAGE(1, LightSensor1->Get());
//DISPLAYMESSAGE(2, LightSensor2->Get());
//DISPLAYMESSAGE(3, LightSensor3->Get());
//DISPLAYMESSAGE(4, LightSensor4->Get());
//DISPLAYMESSAGE(6, ExtendEncoderValue);

// If we are not at the desired pressure, turn on the compressor
if(!pressureSwitch->Get()) compressor->Set(Relay::kForward);
else compressor->Set(Relay::kOff);

// Get the current encoder position
ArmEncoderValue = ArmEncoder->Get();

curlight = ArmLightSensor->Get();
if(ArmExtend->Get() != 0) prevarmext = ArmExtend->Get();

if(curlight != prevlight)
{
if(prevarmext < 0) ExtendEncoderValue--;
else if(prevarmext && ArmExtendLimit->Get()) ExtendEncoderValue++;
}

prevlight = curlight;

if(!ArmRetractLimit->Get()) ExtendEncoderValue = 0;

//DISPLAYMESSAGE(2, ExtendEncoderValue);

// Enter auto-pickup upon button command and set arm to pivot to home position
if(PICKUPBUTTON)
{
ArmPickup = 1;
ExtendTarget = -1;
ArmTarget = ARM_HOME;
}

if(!ClawSwitch->Get() && ArmPickup) ArmPickup = 2;

//CLEARMESSAGE;
//DISPLAYMESSAGE(1, ArmEncoderValue);
//DISPLAYMESSAGE(2, pressureSwitch->Get());

// Send joystick data to drive system as well as limit switch pointers
// Depending on which drive system we're using (according to the DRIVESYSTEM definition),
// call the appropriate function
myRobot->DriveSwerve(leftStick, rightStick, clawStick,
ArmPivotLimitDown, ArmPivotLimitUp, ArmRetractLimit,
ArmExtendLimit, ArmEncoder, ArmTarget, ArmEncoderValue, 0, ArmPickup, ArmDeploy, ExtendTarget);

// If the deploy button is pressed, close all retract solenoids and
// open the extend solenoids
if(DEPLOYMINIBOT)
{
minibotDeploy->Set(1);
minibotDeploy2->Set(1);
minibotRetract->Set(0);
minibotDeployed = 1;
}

// If the retract button is pressed, close all deploy solenoids and
// open the retract solenoids
else if(RETRACTMINIBOT)
{
minibotDeploy->Set(0);
minibotDeploy2->Set(0);
minibotRetract->Set(1);
minibotDeployed = 0;
}

// If any arm button is pressed, exit auto-pickup
if(EXTENDBUTTON || RETRACTBUTTON || PIVOTPOSITIONHOME || PIVOTPOSITIONLOW || PIVOTPOSITIONMIDDLE || PIVOTPOSITIONHIGH || PIVOTPOSITIONFULL || PIVOTDOWNBUTTON || PIVOTUPBUTTON || PREPICKUPBUTTON) ArmPickup = 0, ArmDeploy = 0;
if(ArmDeploy == 1 && ArmPickup != 0) ArmPickup = 0;
if(!ArmRetractLimit->Get() && ArmPickup != 2) ArmPickup = 0;

// Read the joystick buttons and set the arm-pivot
// target position to the corresponding encoder value
if (PIVOTPOSITIONHOME)
{
// If the trigger is pressed, enable auto-deploy
if(!CLAWREVERSEBUTTON)
{
ArmTarget = -1;
ExtendTarget = -1;
ArmDeploy = 1;
ArmPickup = 0;
}

// Otherwise, go to home position
else ArmTarget = ARM_HOME, ExtendTarget = 0;
}

else if (PIVOTPOSITIONLOW)
ArmTarget = ARM_RACKBOTTOM, ExtendTarget = 0;
else if (PIVOTPOSITIONMIDDLE)
ArmTarget = ARM_RACKMIDDLE, ExtendTarget = 0;
else if (PIVOTPOSITIONHIGH)
ArmTarget = ARM_RACKTOP, ExtendTarget = 0;
else if (PIVOTPOSITIONFULL)
ArmTarget = ARM_FULLUP, ExtendTarget = ARMEXTENDRACK;
else if(PREPICKUPBUTTON)
ArmTarget = ARM_HOME, ExtendTarget = PREPICKUPEXTEND;

// If the manual arm buttons are pressed, disable auto-movement
if (PIVOTDOWNBUTTON || PIVOTUPBUTTON)
ArmTarget = -1;

if (EXTENDBUTTON || RETRACTBUTTON)
ExtendTarget = -1;

// Negative one indicates that the arm position is being set
// manually. Otherwise, it is being moved to a preset position
// and we need to use a PID loop to approach the correct position
if (ArmTarget >= 0)
{
// If the arm pivot has hit the home position limit switch, reset the encoder and stop
if (!ArmPivotLimitDown->Get() && ArmTarget < ArmEncoderValue)
{
ArmEncoder->Reset();
ArmPivot->Set(0);
ArmTarget = -1;
}

// If the arm pivot has hit the full up limit switch, stop
if (!ArmPivotLimitUp->Get() && ArmTarget > ArmEncoderValue)
{
ArmPivot->Set(0);
ArmTarget = -1;
}

// If the arm needs to move down
if (ArmEncoderValue > ArmTarget + ARMDEADBAND)
{
if (ArmEncoderValue - ArmTarget < ARMAPPROACHDOWN)
ArmPivot->Set(ARMDOWNSPEED + Abs(((ARMAPPROACHDOWN - (ArmEncoderValue - ArmTarget)) / ARMAPPROACHDOWN)));
else
ArmPivot->Set(ARMDOWNSPEED);
}

// If the arm needs to move up
else if (ArmEncoderValue < ArmTarget - ARMDEADBAND)
{
if (ArmTarget - ArmEncoderValue < ARMAPPROACHUP)
ArmPivot->Set(ARMUPSPEED * 0.4 - Abs(((ARMAPPROACHUP - (ArmTarget - ArmEncoderValue)) / ARMAPPROACHUP)));
else
ArmPivot->Set(ARMUPSPEED);
}

else ArmPivot->Set(0);
}

DISPLAYMESSAGE(1, ArmEncoderValue);

if(ExtendTarget >= 0)
{
if(ExtendEncoderValue > ExtendTarget + 1)
{
if(ArmRetractLimit->Get()) ArmExtend->Set(-1);
else ArmExtend->Set(0);
}

else if(ExtendEncoderValue < ExtendTarget - 1)
{
if((ArmExtendLimit->Get() && ArmEncoderValue > 30) || ExtendTarget == PREPICKUPEXTEND) ArmExtend->Set(1);
else ArmExtend->Set(0);
}

else ArmExtend->Set(0);
}

// If auto-deploy is enabled
if(ArmDeploy)
{
// If the arm has not finished retracting
if(ArmRetractLimit->Get())
{
// Eject the tube and retract the arm
myRobot->topclawmotor->Set(0.5); //@
myRobot->bottomclawmotor->Set(-0.5); //@
ArmExtend->Set(-1);
}

// Otherwise
else
{
// Stop the motors and exit auto-deploy
myRobot->topclawmotor->Set(0);
myRobot->bottomclawmotor->Set(0);
ArmExtend->Set(0);
//ArmDeploy = 0;
ArmTarget = ARM_HOME;
}
}

if(!ArmPivotLimitDown->Get()) ArmDeploy = 0;

// The arm is moving, disengage the brake. Otherwise, re-engage the brake
if (ArmPivot->Get())
engageBrake->Set(0);
else
engageBrake->Set(1);
} // End of loop while in operator control
} // End of OperatorControl function

void Autonomous(void)
{
// Has the robot reached the end of the line ("T")?
int stop = 0; // Has the robot reached the end of the line?
//int strafe = 0; // Is the robot strafing along the branch?
//int ArmTarget = -1; // Target pivot position for arm
int ExtendTarget = ARMEXTENDRACK; // Target extension position for arm
unsigned int timesetting = 0; // Used to track timing of various scripted events
int prevlight, curlight; // Previous and current states of light sensor on arm extension
float prevarmext = 0; // Previous setting of arm extension motor
int trip = 0;

BlockEncoder->Start();
BlockEncoder->Reset();
myRobot->trip = 0;

// While in autonomous mode
while (IsAutonomous())
{
// Feed "watchdog" to prevent an assumed lockup
GetWatchdog().Feed();
CLEARMESSAGE;
DISPLAYMESSAGE(6, (int)(ultraDistance->GetVoltage() * 1000.0f));


// Arm encoder overhead
// If the light sensor detects a different colour than last time, it has moved
curlight = ArmLightSensor->Get();
if(ArmExtend->Get() != 0) prevarmext = ArmExtend->Get(); // Only change the stored motor direction if the motor is running
if(curlight != prevlight)
{
// Increment or decrement encoder value based on direction of extension motor
if(prevarmext < 0) ExtendEncoderValue--;
// Stop adding if the exension limit has been hit
else if(prevarmext > 0 && ArmExtendLimit->Get()) ExtendEncoderValue++;
}

// Set previous state for next time
prevlight = curlight;

// If the arm retracts fully, reset value to zero, just in case
if(!ArmRetractLimit->Get()) ExtendEncoderValue = 0;
ArmEncoderValue = ArmEncoder->Get();
//DISPLAYMESSAGE(2, ExtendEncoderValue);
// End of arm encoder overhead

// If the robot has not reached the end, track the line
// When the return value becomes one, the robot should begin executing
// The scripted events below. 0 == first stage of autonomous (line tracking)
if (stop == 0) stop = myRobot->MoveSwerve(LightSensor1->Get(), LightSensor3->Get(), LightSensor2->Get(), ArmPivotLimitUp->Get(), ultraDistance, AutonomousSwitch->Get());

// Extend the arm to the desired position, if one exists (-1 means neutral)
if(ExtendTarget >= 0)
{
if(ExtendEncoderValue > ExtendTarget + 1)
{
if(ArmRetractLimit->Get()) ArmExtend->Set(-1);
else ArmExtend->Set(0);
}

else if(ExtendEncoderValue < ExtendTarget - 1)
{
if(ArmExtendLimit->Get() && ArmEncoderValue > 80) ArmExtend->Set(1);
else ArmExtend->Set(0);
}

else ArmExtend->Set(0);
}

// Once we've reached the end of the first line, stop and wait for the arm to finish
// pivoting and extending
if(stop == 1)
{
// Make sure to stop
myRobot->lfDrive->Set(0);
myRobot->lrDrive->Set(0);
myRobot->rfDrive->Set(0);
myRobot->rrDrive->Set(0);

if(Abs(ArmPivot->Get()) < 0.05 && Abs(ArmExtend->Get()) < 0.05) stop = 2;
}

// Once we're prepared to deploy the first tube, do so
else if(stop == 2 && AutonomousSwitch->Get())
{
// Neutralize the arm extension for later
ExtendTarget = -1;

// Allow the arm to retract some before deploying the tube, while also moving back
// slightly from the rack
if(timesetting < RETRACTTIME)
{
timesetting++;
myRobot->lfDrive->Set(-0.2);
myRobot->lrDrive->Set(-0.2);
myRobot->rfDrive->Set(-0.2);
myRobot->rrDrive->Set(-0.2);
}

// Then, rotate the tube briefly
else if(timesetting < MANIPULATIONTIME)
{
myRobot->lfDrive->Set(0);
myRobot->lrDrive->Set(0);
myRobot->rfDrive->Set(0);
myRobot->rrDrive->Set(0);
myRobot->bottomclawmotor->Set(-CLAWROLLERSPEED);
myRobot->topclawmotor->Set(-CLAWROLLERSPEED);
timesetting++;
}

// Finally, deploy the tube
else
{

// If the arm has not finished retracting, continue to deploy tube with the rollers
if (ArmRetractLimit->Get())
{
myRobot->bottomclawmotor->Set(-CLAWROLLERSPEED);
myRobot->topclawmotor->Set(CLAWROLLERSPEED);
}

// Once the arm has finished retracting, stop the rollers
else
{
myRobot->bottomclawmotor->Set(0);
myRobot->topclawmotor->Set(0);
}
}

// Retract the arm to the home limit switch
if (ArmRetractLimit->Get()) ArmExtend->Set(-1);
else
{
ArmExtend->Set(0);
BlockEncoder->Start();
BlockEncoder->Reset();
stop = 3;
}
}

else if(stop == 3 && AutonomousSwitch->Get())
{
if(ArmPivotLimitDown->Get()) ArmPivot->Set(-1);
else ArmPivot->Set(0);
myRobot->lfDrive->Set(-0.2);
myRobot->lrDrive->Set(-0.2);
myRobot->rfDrive->Set(-0.2);
myRobot->rrDrive->Set(-0.2);
myRobot->bottomclawmotor->Set(0);
myRobot->topclawmotor->Set(0);
}

else if(stop == 2 && !AutonomousSwitch->Get() && TrackSteering((int)(BRANCHANGLE * PULSES_PER_STEER), 2))
{
if(ultraDistance->GetVoltage() * 1000 < BRANCHSTOPDISTANCE) trip++;
if(trip > 320) stop = 3;
myRobot->lfDrive->Set(0.4);
myRobot->lrDrive->Set(0.4);
myRobot->rfDrive->Set(0.4);
myRobot->rrDrive->Set(0.4);
}

else if(stop == 3 && !AutonomousSwitch->Get())
{
ExtendTarget = -1;

myRobot->lfDrive->Set(0);
myRobot->lrDrive->Set(0);
myRobot->rfDrive->Set(0);
myRobot->rrDrive->Set(0);

// If the arm has not finished retracting, continue to deploy tube with the rollers
if (ArmRetractLimit->Get())
{
myRobot->bottomclawmotor->Set(-CLAWROLLERSPEED);
myRobot->topclawmotor->Set(CLAWROLLERSPEED);
}

// Once the arm has finished retracting, stop the rollers
else
{
myRobot->bottomclawmotor->Set(0);
myRobot->topclawmotor->Set(0);
}

// Retract the arm to the home limit switch
if (ArmRetractLimit->Get()) ArmExtend->Set(-1);
else
{
ArmExtend->Set(0);
BlockEncoder->Start();
BlockEncoder->Reset();
stop = 4;
}
}

if(stop == 4 && !AutonomousSwitch->Get())
{
if(ArmPivotLimitDown->Get()) ArmPivot->Set(-1);
else ArmPivot->Set(0);
if(TrackSteering(0, 0))
{
myRobot->lfDrive->Set(-0.3);
myRobot->lrDrive->Set(-0.3);
//myRobot->rfDrive->Set(0.2);
//myRobot->rrDrive->Set(0.2);
}
myRobot->bottomclawmotor->Set(0);
myRobot->topclawmotor->Set(0);
}

// Until the arm has been fully pivoted up, and we haven't finished line-tracking,
// move it up
if (stop == 0)
{
if (ArmPivotLimitUp->Get()) ArmPivot->Set(1);
else ArmPivot->Set(0);
}
} // End of loop while in autonomous mode
} // End of Autonomous

// Use the encoders, with regard to the limit switches, turn the steering to the
// goal value passed (used by autonomous function)
int TrackSteering(int Target, int side)
{
int leftSteeringEncoderValue;
int rightSteeringEncoderValue;
float speedincrement = 1/STEERING_APPROACH_DISTANCE;

// Get the current steering positions
leftSteeringEncoderValue = myRobot->leftSteeringEncoder->Get();
rightSteeringEncoderValue = myRobot->rightSteeringEncoder->Get();

if(side != 1)
{
// Turn the steering
if(Abs(leftSteeringEncoderValue - Target) < (STEERING_DEADBAND * PULSES_PER_STEER)) myRobot->leftSteer->Set(0);
else if(leftSteeringEncoderValue < Target)
{
if(!myRobot->backleftSteerLimit->Get()) myRobot->leftSteer->Set(0);
else if(leftSteeringEncoderValue + STEERING_APPROACH_DISTANCE >= Target) myRobot->leftSteer->Set(speedincrement * Abs(Target - leftSteeringEncoderValue));
else myRobot->leftSteer->Set(0.9);
}

else
{
if(!myRobot->backrightSteerLimit->Get()) myRobot->leftSteer->Set(0);
else if(leftSteeringEncoderValue - STEERING_APPROACH_DISTANCE <= Target) myRobot->leftSteer->Set(-1 * speedincrement * Abs(Target - leftSteeringEncoderValue));
else myRobot->leftSteer->Set(-0.9);
}
}

// Right steering uses inverse angle
Target *= -1;

if(side != 2)
{
// Turn the steering
if(Abs(rightSteeringEncoderValue - Target) < (0.05f * PULSES_PER_STEER)) myRobot->rightSteer->Set(0);
else if(rightSteeringEncoderValue < Target)
{
if(!myRobot->frontleftSteerLimit->Get()) myRobot->rightSteer->Set(0);
else if(rightSteeringEncoderValue + STEERING_APPROACH_DISTANCE >= Target) myRobot->rightSteer->Set(speedincrement * Abs(Target - leftSteeringEncoderValue));
else myRobot->rightSteer->Set(0.9);
}

else
{
if(!myRobot->backleftSteerLimit->Get()) myRobot->rightSteer->Set(0);
else if(rightSteeringEncoderValue - STEERING_APPROACH_DISTANCE <= Target) myRobot->rightSteer->Set(-1 * speedincrement * Abs(Target - leftSteeringEncoderValue));
else myRobot->rightSteer->Set(-0.9);
}
}

if(Abs(myRobot->rightSteer->Get()) < 0.05f && Abs(myRobot->leftSteer->Get()) < 0.05f) return 1;
else return 0;
}
}; // End of SimpleTracker class

// Return absolute value of argument (distance from zero)
float Abs(float x)
{
if (x < 0)
x *= -1;
return x;
}

// Entry point is FRC_UserProgram_StartupLibraryInit
START_ROBOT_CLASS(SimpleTracker)
;

/* To send text to driver station:
* // Send formatted text to specified line and column
DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line1, 1, "This is a test");
// Update to make text appear
DriverStationLCD::GetInstance()->UpdateLCD();
*/

You could have tried to code posting feature

Code:

code here