FIRST Robotics Code 2011

#include "WPILib.h"
#include "dashboard.h"
#include "math.h"
/*change list:
 * 1. Increased acquire/spit out speed
 * 2. Release Button 4-28-11
 * 3. Improved arm extend/shift code 4-28-11
 * 4. Fixed claw in manual mode, preset now overrides manual 4-29-11
 * 5. Increased encoder distance from 210->214
 * 6. Fixed Release Button 4-29-11
 * 7. Added dual mode release button 4-29-11
 */

/*TODO list:
 * 1. Dual tube auto
 */

/********************* FOR XBOX 360 CONTROLLER****************************
BUTTON NUMBERS
ButtonA = 1
ButtonB = 2
ButtonX = 3
ButtonY = 4
LeftBumper = 5
RightBumper = 6
Select = 7
Start = 8
LeftClick = 9
RightClick = 10

THUMBSTICK AXIS'
LeftXAxis = 1 top is -1, rest is 0 , bottom is 1
LeftYAxis = 2
TriggerAxis = 3

Both triggers are in this.
Right Trigger is [-1,0]
Left Trigger is [0,1]
Return value = Right Trigger + Left Trigger

RightXAxis = 4
RightYAxis = 5
************************************************************************/

class RobotDemo : public SimpleRobot
{
    RobotDrive *myRobot; // robot drive system
    Victor *leftDrive;
    Victor *rightDrive;
    Jaguar *armJags;
    Jaguar *rightArmJag;
    Jaguar *topClawJag;
    Jaguar *bottomClawJag;
    Servo *leftDeployServo;
    Servo *rightDeployServo;

    //GetRawAxis returns from -1 to 1, -1 is top
    Joystick *xboxStickManip;
    Joystick *leftStick;
    Joystick *rightStick;

    Compressor *compressor1;

    Solenoid *shiftHigh;
    Solenoid *shiftLow;
    Solenoid *minibotDeployer;
    Solenoid *armExtend;
    Solenoid *armRetract;
    Solenoid *minibotHold;

    DigitalInput *lightLeft; //MAKE SURE THE LIGHT SENSOR RIBBON IS WIRED CORRECTLY
    DigitalInput *lightMid; //PORTS 1,2,3 MUST BE OCCUPIED
    DigitalInput *lightRight;
    DigitalInput *autoSwitch;

    DigitalOutput *redLEDs;
    DigitalOutput *blueLEDs;

    //DigitalInput *minibotDeploymentBarSwitch; // Activates when the switch is depressed, deployment bar is touching pole
    DigitalInput *highLimitSwitch;
    DigitalInput *lowLimitSwitch;

    Gyro *gyro1;

    //Ultrasonic *ultrasonic1;

    Encoder *rightEncoder;

    Timer *robotTimer;

    AnalogChannel *pot1;

    //presets for arm angle, calibrated by pot values
    #define ARM_ANGLE_MIN (1000-560)
    #define ARM_ANGLE_MAX (1000-430)

    #define ARM_ANGLE_FLOOR (1000-550)
    #define ARM_ANGLE_HOME_LOW (1000-560)
    #define ARM_ANGLE_FEED (1000-500)
    #define ARM_ANGLE_MID (1000-470)
    #define ARM_ANGLE_MID_DROP (1000-502)
    #define AUTO_RELEASE_THRESHOLD (1000-450)//the value where the auto release chooses between high release and mid release
    #define ARM_ANGLE_HIGH (1000-428)
    #define ARM_ANGLE_HIGH_DROP (1000-460)//drops the arm while spinning durning auto


    bool firstCycle;
    int previousError;
    int LEDState;

public:
    RobotDemo(void)
    {
        /* DIGITAL IO PORTS
        1   lightLeft
        2   lightMid
        3   lightRight
        4   --(ultrasonic)
        5   --(ultrasonic)
        6   redLEDs
        7   blueLEDs
        8   Encoder B Channel
        9   Encoder A Channel
        10  --(minibotDeploymentBarSwitch)
        11  highLimitSwitch
        12  compressor pressor switch
        13  autoSwitch
        14  lowLimitSwitch
        */

        leftDrive = new Victor(1);
        rightDrive = new Victor(2);
        myRobot = new RobotDrive(leftDrive, rightDrive);
        armJags = new Jaguar(3);
        topClawJag = new Jaguar(5);
        bottomClawJag = new Jaguar(6);
        leftDeployServo = new Servo(7);
        rightDeployServo = new Servo(8);
        leftDeployServo->Set(0.3); //to reset, must restart robot. Disable won't reset.
        rightDeployServo->Set(0.7);

        AxisCamera::GetInstance();

        xboxStickManip = new Joystick(1);
        leftStick = new Joystick(2);
        rightStick = new Joystick(3);

        compressor1 = new Compressor(12,1);
        compressor1->Start();

        shiftHigh = new Solenoid(1);
        shiftLow = new Solenoid(2);
        minibotDeployer = new Solenoid(3);
        armExtend = new Solenoid (4);
        armRetract = new Solenoid (5);
        minibotHold = new Solenoid (6);
        minibotHold->Set(true);
        minibotDeployer->Set(false);

        lightLeft = new DigitalInput(1);
        lightMid = new DigitalInput(2);
        lightRight = new DigitalInput(3);

        redLEDs = new DigitalOutput(6);
        blueLEDs = new DigitalOutput(7);

        //minibotDeploymentBarSwitch = new DigitalInput(10);
        highLimitSwitch = new DigitalInput(11);
        autoSwitch = new DigitalInput(13);//0 is to the side, 1 is towards robot
        lowLimitSwitch = new DigitalInput(14);

        gyro1 = new Gyro(1);

        //ultrasonic1 = new Ultrasonic(4, 5, Ultrasonic::kInches); //Digital ping out on 4, echo in on 5
        //ultrasonic1->SetAutomaticMode(true);

        rightEncoder = new Encoder(9, 8, true, Encoder::k4X);
        rightEncoder->Start();

                                                        //set how many inches traveled per tick
        rightEncoder->SetDistancePerPulse(0.0349);      //this number should be the circumference of the
                                                        //wheel divided by 360

        robotTimer = new Timer();

        SmartDashboard::init();

        pot1 = new AnalogChannel(2);

        firstCycle = true;
        previousError = 0; //do not apply d during first cycle through position arm
        LEDState = 2;

        GetWatchdog().SetExpiration(0.1);
    }

    ~RobotDemo(void)
    {
        delete myRobot;
        delete armJags;
        delete topClawJag;
        delete bottomClawJag;
        delete leftDeployServo;
        delete rightDeployServo;

        delete xboxStickManip;
        delete leftStick;
        delete rightStick;

        delete compressor1;

        delete shiftHigh;
        delete shiftLow;
        delete minibotDeployer;
        delete armExtend;
        delete armRetract;
        delete minibotHold;

        delete autoSwitch;

        delete gyro1;

        //delete ultrasonic1;

        delete rightEncoder;

        delete robotTimer;

        delete pot1;
    }

    void Autonomous(void)
    {
        GetWatchdog().SetEnabled(false);

        double timeElapsed = 0.0;

        robotTimer->Reset();
        robotTimer->Start();

        rightEncoder->Reset();

        gyro1->Reset();


        ExtendArm(false);

        if (autoSwitch->Get()==1)//set to 1, gyro drive
        {
            while(IsAutonomous())
            {
                timeElapsed = robotTimer->Get();

                Shift(0);

                if(((int)(timeElapsed))%2==0)
                    SetLEDs(2);
                else
                    SetLEDs(1);

                if(timeElapsed < 8.0)
                {
                    PositionArm(ARM_ANGLE_HIGH);
                    ExtendArm(true);
                    GyroDrive(0.0, 0.5, 214);
                }
                else if (timeElapsed > 8.0 && timeElapsed <8.5)
                {
                    PositionArm(ARM_ANGLE_HIGH);
                    myRobot->Drive(0.0, 0.0);
                }

                if(timeElapsed > 8.0 && timeElapsed < 11.0)
                {
                    PositionArm(ARM_ANGLE_HIGH_DROP);
                    SpinClaw(-1, 0.7);//tilt tube forward
                }
                else if(timeElapsed > 11.0 && timeElapsed < 11.5)
                {
                    PositionArm(ARM_ANGLE_HIGH_DROP);
                    SpinClaw(0.0, 0.0);
                }

                if(timeElapsed > 9.0)
                    ExtendArm(false);

                if(timeElapsed > 11.0 && timeElapsed < 13.0)
                {
                    PositionArm(ARM_ANGLE_MID);
                    myRobot->Drive(-0.5, 0);
                }
                else if (timeElapsed > 12.0)
                {
                    PositionArm(ARM_ANGLE_MID);
                    myRobot->Drive(0.0, 0.0);
                }

            }
        }
        else
        {
            while(IsAutonomous())
            {
                timeElapsed = robotTimer->Get();

                Shift(0);

                if(((int)(timeElapsed))%2==0)
                    SetLEDs(2);
                else
                    SetLEDs(1);

                if(timeElapsed < 8.0)
                {
                    PositionArm(ARM_ANGLE_HIGH);
                    ExtendArm(true);
                    LineFollow(false, 0.5, 214);
                }
                else if (timeElapsed > 8.0 && timeElapsed <8.5)
                {
                    PositionArm(ARM_ANGLE_HIGH);
                    myRobot->Drive(0.0, 0.0);
                }

                if(timeElapsed > 8.0 && timeElapsed < 11.0)
                {
                    PositionArm(ARM_ANGLE_HIGH_DROP);
                    SpinClaw(-1, 0.7);//tilt tube forward
                }
                else if(timeElapsed > 11.0 && timeElapsed < 11.5)
                {
                    PositionArm(ARM_ANGLE_HIGH_DROP);
                    SpinClaw(0.0, 0.0);
                }

                if(timeElapsed > 9.0)
                    ExtendArm(false);

                if(timeElapsed > 11.0 && timeElapsed < 13.0)
                {
                    PositionArm(ARM_ANGLE_MID);
                    myRobot->Drive(-0.5, 0);
                }
                else if (timeElapsed > 12.0)
                {
                    PositionArm(ARM_ANGLE_MID);
                    myRobot->Drive(0.0, 0.0);
                }

            }
        }
    }
        /* Dual tube mode
         *
            while(IsAutonomous())
            {
                timeElapsed = robotTimer->Get();

                Shift(0);

                if(((int)(timeElapsed))%2==0)
                    SetLEDs(2);
                else
                    SetLEDs(1);

                if((timeElapsed > 0.0) && (timeElapsed < 2.95))
                {
                    PositionArm(ARM_ANGLE_HIGH);
                    ExtendArm(true);
                    //Shift(1);
                    GyroDrive(0.0, 1.0, 214);
                }
                else if ((timeElapsed > 2.95) && (timeElapsed < 3.0))
                {
                    rightEncoder->Reset();
                    myRobot->TankDrive(0.0, 0.0);
                }

                if((timeElapsed > 3.0) && (timeElapsed < 6.95))
                {
                    SpinClaw(-1, 1);
                    ExtendArm(false);
                    PositionArm(ARM_ANGLE_MID);
                    if(timeElapsed > 3.5)//wait 0.5 sec before backing up
                    {
                        PositionArm(ARM_ANGLE_FLOOR);
                        GyroDrive(0.0, -1.0, 237);
                    }
                }
                else if ((timeElapsed > 6.95) && (timeElapsed < 7.0))
                {
                    SpinClaw(0.0, 0.0);
                    rightEncoder->Reset();
                    myRobot->TankDrive(0.0, 0.0);
                }

                if((timeElapsed > 7.0) && (timeElapsed < 7.95))
                {
                    //Shift(0);
                    GyroTurn(-30.0, 1.0);
                }
                else if ((timeElapsed > 7.95) && (timeElapsed < 8.0))
                {
                    //Shift(1);
                    rightEncoder->Reset();
                    myRobot->TankDrive(0.0, 0.0);
                }

                if((timeElapsed > 8.0) && (timeElapsed < 9.95))
                {
                    PositionArm(ARM_ANGLE_FLOOR);
                    ExtendArm(true);
                    //Shift(1);
                    LineFollow(true, 1.0, 25);
                    SpinClaw(0.7, -0.7);
                }
                else if ((timeElapsed > 9.95) && (timeElapsed < 10.0))
                {
                    rightEncoder->Reset();
                    myRobot->TankDrive(0.0, 0.0);
                    SpinClaw(0.0, -0.0);
                }

                if((timeElapsed > 10.0) && (timeElapsed < 12.95))
                {
                    PositionArm(ARM_ANGLE_HIGH);
                    ExtendArm(true);
                    //Shift(1);
                    LineFollow(true, 1.0, 215);
                }
                else if ((timeElapsed > 12.95) && (timeElapsed < 13.0))
                {
                    rightEncoder->Reset();
                    myRobot->TankDrive(0.0, 0.0);
                }

                if((timeElapsed > 13.0) && (timeElapsed < 14.95))
                {
                    SpinClaw(-1, 1);
                    ExtendArm(false);
                    PositionArm(ARM_ANGLE_MID);
                    if(timeElapsed > 13.5)//wait 0.5 sec before backing up
                    {
                        PositionArm(ARM_ANGLE_MID);
                        GyroDrive(0.0, -1.0, 50);
                    }
                }
                else if ((timeElapsed > 14.95) && (timeElapsed < 15.0))
                {
                    SpinClaw(0.0, 0.0);
                    rightEncoder->Reset();
                    myRobot->TankDrive(0.0, 0.0);
                }
            }
                */

    void GyroDrive(float desiredDriveAngle, float speed, int desiredDistance)//todo:fix for teleop
    {
        double encoderInchesTraveled = fabs(rightEncoder->GetDistance());//absolute value distance
        float myAngle = gyro1->GetAngle();
        //normalizes angle from gyro to [-180,180) with zero as straight ahead
        while(myAngle >= 180)
        {
            GetWatchdog().Feed();
            myAngle = myAngle - 360;
        }
        while(myAngle < -180)
        {
            GetWatchdog().Feed();
            myAngle = myAngle + 360;
        }

        float my_speed = 0.0;
        float turn = 0.0;

        if(speed > 0)
            turn = -((myAngle + desiredDriveAngle) / 30.0); //proportionally adjust turn. As the robot gets more off 0, the greater the turn will be
        else
            turn = (myAngle + desiredDriveAngle) / 30.0; //proportionally adjust turn. As the robot gets more off 0, the greater the turn will be

        if (encoderInchesTraveled < desiredDistance)
            my_speed = speed;
        else
            my_speed = 0.0;

        myRobot->Drive(my_speed, turn);
    }

//probably should use a postive speed here
    void LineFollow(bool lookForLine, float speed, int desiredDistance)
    {
        //flips 0 to 1, so 0 means the sensor does not detect a line
        int lightLeftValue = (int)(!((bool)lightLeft->Get()));
        int lightMidValue = (int)(!((bool)lightMid->Get()));
        int lightRightValue = (int)(!((bool)lightRight->Get()));

        //double ultrasonicInchesFromWall = ultrasonic1->GetRangeInches();
        double encoderInchesTraveled = rightEncoder->GetDistance();

        float my_speed = 0.0;
        float turn = 0.0;

        if (encoderInchesTraveled<desiredDistance)
            my_speed = speed;
        else
            my_speed = 0.0;

        if((lightLeftValue == 1) && (lightMidValue == 1) && (lightRightValue == 1)) // sensors detect everything, maybe at end?
        {
            my_speed = (0.0);
            turn = (0.0);
        }
        else if((lightLeftValue == 0) && (lightMidValue == 0) && (lightRightValue == 0)) // sensors detect nothing, maybe passed end?
        {
            if(!lookForLine)//if not looking for line, set speed to 0 if sensors detect nothing
            {
                my_speed = (0.0);
                turn = (0.0);
            }
        }
        else if ((lightLeftValue == 0) && (lightMidValue == 1) && (lightRightValue == 0)) // ideal condition
            turn = 0.0;
        else if ((lightLeftValue == 1) && (lightMidValue == 0) && (lightRightValue == 0))// far left, so turn right
            turn = -0.8;
        else if ((lightLeftValue == 0) && (lightMidValue == 0) && (lightRightValue == 1))// far right, so turn left
            turn = 0.8;
        else if ((lightLeftValue == 1) && (lightMidValue == 1) && (lightRightValue == 0))// near left, so turn right
            turn = -0.8;
        else if ((lightLeftValue == 0) && (lightMidValue == 1) && (lightRightValue == 1))// near right, so turn left
            turn = 0.8;
        else if((lightLeftValue == 1) && (lightMidValue == 0) && (lightRightValue == 1)) // should happen at branch, but unreliable
            turn = -0.8; // in a perfect world, turns right at branch
        else turn = 0.0; //should never reach this


        myRobot->Drive(my_speed, turn);


    }

    void Shift(bool gear)
    {
        if (gear)
        {
            shiftHigh->Set(true);
            shiftLow->Set(false);
        }
        else
        {
            shiftHigh->Set(false);
            shiftLow->Set(true);
        }
    }

    void PositionArm(INT16 desiredArmAngle)
    {

        double proportionalControl = 0.0;
        double derivativeControl = 0.0;
        double combinedControl = 0.0;

        int currentAngle = 1000-(pot1->GetValue());
        int currentError = desiredArmAngle - currentAngle;
        if(firstCycle)
        {
            previousError = currentError;
            firstCycle = false;
        }
        int deltaError = currentError - previousError;
        previousError = currentError;

        proportionalControl = currentError * 0.02;
        derivativeControl = deltaError * 0.01; //todo: fix this.
        combinedControl = proportionalControl + derivativeControl;

        MoveArm(combinedControl);

    }
    void MoveArm(float speed) //negative speed is lift up arm
    {
        /*
        int currentAngle = 1000-(pot1->GetValue());
        if(currentAngle > ARM_ANGLE_MAX) //todo:make sure this works, not in the opposite way
            armJags->Set(-0.1);
        if(currentAngle < ARM_ANGLE_MIN)
            armJags->Set(0.1);
        */
        float my_speed = speed;

        if(speed > 0.7)
            my_speed = 0.7;
        if(speed < -0.7)
            my_speed = -0.7;

        //Prevent the arm from overshooting, mainly for manual arm control, which does not slow down on its own
        //if((1000-(pot1->GetValue()) > ARM_ANGLE_MID+20) && (speed > 0.3))
            //my_speed = 0.3;

        if(highLimitSwitch->Get() && (speed > 0))
            my_speed = 0;
        if(lowLimitSwitch->Get() && (speed < 0))
            my_speed = 0;

        armJags->Set(my_speed);

    }
    void ExtendArm(bool extend)
    {
        if (extend)
        {
            armExtend->Set(true);
            armRetract->Set(false);
        }
        else
        {
            armExtend->Set(false);
            armRetract->Set(true);
        }
    }
    void SpinClaw(float topSpeed, float bottomSpeed)//squared for more sensitive low end
    {
        if (topSpeed > 0)
            topClawJag->Set(topSpeed*topSpeed);
        else
            topClawJag->Set(-topSpeed*topSpeed);
        if (bottomSpeed > 0)
            bottomClawJag->Set(bottomSpeed*bottomSpeed);
        else
            bottomClawJag->Set(-bottomSpeed*bottomSpeed);
    }
    //robot turns to desired position with a deadband of 2 degrees in each direction
    void GyroTurn (float desiredTurnAngle, float turnSpeed)
    {
        float myAngle = gyro1->GetAngle();
        //normalizes angle from gyro to [-180,180) with zero as straight ahead
        while(myAngle >= 180)
        {
            GetWatchdog().Feed();
            myAngle = myAngle - 360;
        }
        while(myAngle < -180)
        {
            GetWatchdog().Feed();
            myAngle = myAngle + 360;
        }
        if(myAngle < (desiredTurnAngle - 2))// if robot is too far left, turn right a bit
        {
            myRobot->TankDrive(turnSpeed, -turnSpeed); //(right,left)
        }
        if(myAngle > (desiredTurnAngle + 2))// if robot is too far right, turn left a bit
        {
            myRobot->TankDrive(-turnSpeed, turnSpeed); //(right,left)
        }
    }

    void SetLEDs(int state)//0 is off, 1 is red, 2 is blue, 3 is both on.
    {//hmm red and blue may be reversed
        if(state==0)
        {
            redLEDs->Set(0);
            blueLEDs->Set(0);
        }
        else if(state==1)
        {
            redLEDs->Set(1);
            blueLEDs->Set(0);
        }
        else if(state==2)
        {
            redLEDs->Set(0);
            blueLEDs->Set(1);
        }
        else
        {
            redLEDs->Set(1);
            blueLEDs->Set(1);
        }
    }


    void OperatorControl(void)
    {

        robotTimer->Stop();
        rightEncoder->Reset();
        myRobot->TankDrive(0.0, 0.0);

        bool minibotStage1Complete = false;// stage 1 (drop) has to be done before stage 2 (push)

        Shift(0);
        ExtendArm(false);
        MoveArm(0.0);

        bool previousLEDToggle = false;
        bool currentLEDToggle = false;

        bool previousShiftToggle = false;
        bool currentShiftToggle = false;

        bool previousArmExtendToggle = false;
        bool currentArmExtendToggle = false;


        LEDState = 1;
        bool ShiftState = false;
        bool ArmExtendState = false;

        GetWatchdog().SetEnabled(true);

        while (IsOperatorControl())
        {
            GetWatchdog().Feed();

            float leftStickYAxis = leftStick->GetY();                           //Tank Drive
            float rightStickYAxis = rightStick->GetY();                         //Tank Drive
            bool rightStickButton1 = rightStick->GetRawButton(1);               //Shifts Robot
            bool rightStickButton10 = rightStick->GetRawButton(10);             //Button 10+11 = Activate deployment servos
            bool rightStickButton11 = rightStick->GetRawButton(11);
            bool leftStickButton9 = leftStick->GetRawButton(9);                 //Backup Deploy Minibot
            bool rightStickButton8 = rightStick->GetRawButton(8);

            float manipRightYAxis = xboxStickManip->GetRawAxis(5);              //Manual Arm Up/Down
            float manipTriggerAxis = xboxStickManip->GetRawAxis(3);             //Rotate Tube Up/Down
            float manipLeftYAxis = xboxStickManip->GetRawAxis(2);               //Cycle LEDs
            bool manipLeftBumper = xboxStickManip->GetRawButton(5);             //Acquire Tube
            bool manipRightBumper = xboxStickManip->GetRawButton(6);            //Spit Out Tube
            bool manipYButton = xboxStickManip->GetRawButton(4);                //High Preset
            bool manipBButton = xboxStickManip->GetRawButton(2);                //Mid Preset
            bool manipAButton = xboxStickManip->GetRawButton(1);                //Floor Preset
            bool manipXButton = xboxStickManip->GetRawButton(3);                //Home/Low Preset
            bool manipLeftClick = xboxStickManip->GetRawButton(9);              //Feed preset = L click
            bool manipStartButton = xboxStickManip->GetRawButton(8);            //Start + Select deploys minibot
            bool manipSelectButton = xboxStickManip->GetRawButton(7);
            bool manipRightClick = xboxStickManip->GetRawButton(10);            //Manual Arm Extend


            //SmartDashboard::Log(ultrasonic1->GetRangeInches(), "Ultrasonic1 inches");

            //SmartDashboard::Log(rightEncoder->GetDistance(), "Encoder Inches Traveled");

            //SmartDashboard::Log(pot1->GetValue(), "pot1 Value");//must not be neg, read true value to calibrate

            //SmartDashboard::Log(gyro1->GetAngle(), "gyro1 Angle");

            //int lightLeftValue = (int)(!((bool)lightLeft->Get()));
            //int lightMidValue = (int)(!((bool)lightMid->Get()));
            //int lightRightValue = (int)(!((bool)lightRight->Get()));
            //SmartDashboard::Log(lightLeftValue, "Left Light");
            //SmartDashboard::Log(lightMidValue, "Middle Light");
            //SmartDashboard::Log(lightRightValue, "Right Light");

            //SmartDashboard::Log(leftDeployServo->Get(), "Left Servo Position");
            //SmartDashboard::Log(rightDeployServo->Get(), "Right Servo Position");

            //int autoVal = autoSwitch->Get();
            //SmartDashboard::Log(autoVal, "Autoswitch");

            /****************DRIVE CODE STARTS HERE***************/
            myRobot->TankDrive(-leftStickYAxis, -rightStickYAxis);


            //MINIBOT DEPLOY STAGE 1
            if (rightStickButton10 && rightStickButton11)
            {

                leftDeployServo->Set(0.7); //to reset, must restart robot. Disable won't reset.
                rightDeployServo->Set(0.3);
                minibotStage1Complete = true;
            }

            //MINIBOT DEPLOY STAGE 2 BACKUP
            if(leftStickButton9 && rightStickButton8 && minibotStage1Complete)
            {
                minibotHold->Set(false);
                minibotDeployer->Set(true);
            }

            Shift(ShiftState); //shifts back to previous gear when line follow/gyro routine ends

            //Shift code, keeps track of shift button state and actual gear state
            if(rightStickButton1)
                currentShiftToggle = true;
            else
                currentShiftToggle = false;

            if(currentShiftToggle && (previousShiftToggle != currentShiftToggle))
                ShiftState=!ShiftState;

            previousShiftToggle = currentShiftToggle;
            Shift(ShiftState);

            /****************MANIPULATOR CODE STARTS HERE***************/
            //CYCLE LEDS
            if(manipLeftYAxis < -0.6 || manipLeftYAxis > 0.6)
                currentLEDToggle = true;
            else
                currentLEDToggle = false;
            //LEDState will only advance if thumbstick moves from 0 to an extreme, not from an extreme to 0,
            //nor when the thumbstick is held. This prevents rapid LED cycling.
            if(currentLEDToggle && (previousLEDToggle != currentLEDToggle))
            {
                if (LEDState > 3)
                    LEDState = 0;
                else
                    LEDState++;
            }
            previousLEDToggle = currentLEDToggle;

            //if(minibotDeploymentBarSwitch->Get()==1)
                //SetLEDs(2);
            //else
                SetLEDs(LEDState);


            // ARM ANGLE PRESETS
            if(manipXButton)
            {
                if(1000-(pot1->GetValue()) > AUTO_RELEASE_THRESHOLD) //high release
                {
                    ArmExtendState = false;
                    PositionArm(ARM_ANGLE_HIGH_DROP);
                    SpinClaw(-1.0, 0.7);
                }
                else //mid release
                {
                    ArmExtendState = false;
                    PositionArm(ARM_ANGLE_MID_DROP);
                    SpinClaw(-1.0, 0.7);
                }
            }
            else if(manipAButton)
            {
                ArmExtendState = true;
                PositionArm(ARM_ANGLE_FLOOR);
            }
            else if(manipYButton)
            {
                ArmExtendState = true;
                PositionArm(ARM_ANGLE_HIGH);
            }
            else if(manipBButton)
            {
                ArmExtendState = false;
                PositionArm(ARM_ANGLE_MID);
            }
            else if(manipLeftClick)
            {
                ArmExtendState = false;
                PositionArm(ARM_ANGLE_FEED);
            }
            else if(manipRightYAxis>=0.1 || manipRightYAxis<=-0.1)
                MoveArm(-manipRightYAxis); //manual arm control
            else
                MoveArm(0.0);

            ExtendArm(ArmExtendState);

            //Arm extend code, keeps track of extend button state and actual extend state
            if(manipRightClick)
                currentArmExtendToggle = true;
            else
                currentArmExtendToggle = false;

            if(currentArmExtendToggle && (previousArmExtendToggle != currentArmExtendToggle))
                ArmExtendState=!ArmExtendState;

            previousArmExtendToggle = currentArmExtendToggle;
            ExtendArm(ArmExtendState);

            //MINIBOT DEPLOY STAGE 2
            if(manipStartButton && manipSelectButton && minibotStage1Complete)
            {
                minibotHold->Set(false);
                minibotDeployer->Set(true);
            }

            // MANUAL CLAW BEGINS HERE
            if (manipXButton)//if the release preset is activated, skip the manual claw section
                ;//do nothing
            else if(manipLeftBumper) //Left Bumper aquires tube
                SpinClaw(0.85, -0.85);
            else if(manipRightBumper)
                SpinClaw(-0.85, 0.85);
            else SpinClaw(-manipTriggerAxis, -manipTriggerAxis); // Left Trigger rotates tube up, Right rotates down


        }

    }

};

START_ROBOT_CLASS(RobotDemo);