[PROGRAMMING] When Nintendo hates your robot, and the exponential nature of code.

[Starwhip]

Our FTC team made it to the States level. Yesterday was the competition.

Twice our autonomous code worked. Twice.

The other nine times the robot just skipped the first five commands, went forward, turned, and fell off of a foot-high ramp backwards.

And it worked 100% of the time on the practice field.

Later on they said that they had been detecting Nintendo 3DS wifi signals in the audience and asked everyone to turn them off. Which, of course, nobody did, because IT KEPT HAPPENING!

Why is the universe so cruel as to have that one guy on our frequency?

On another topic:

Our FRC team has three days before the end of the build season. Our code for the robot has ballooned into this monster:

#include "WPILib.h"
#include <thread>
/*
 These are pre-processing directives, the compiler will find
 in your code all these names, and replace them with the numbers
 listed beside them.
 */

#define frontLeftChannel  0         //the port for the front left drive motor
#define frontRightChannel 1         //the port for the front right drive motor
#define rearLeftChannel   2         //the port for the back left drive motor
#define rearRightChannel  3         //the port for the back right drive motor
#define mainLiftChannel   4         //the port for the main lift motor
#define backupLiftChannel 5

#define joystickDriveChannel   0    //what position in the controller array is the drive controller
#define joystickArmChannel     1    //what position in the controller array is the lift controller

#define joystickThreshold 0.15      //the "dead" zone of the controller
#define joystickLimit     1.0       // % of range (1.0 = 100%, .75 = 75% etc.)

#define encFLreverse  false;        //Reverse the Front Left Encoder
#define encBLreverse  false;        //Reverse the Back Left Encoder
#define encFRreverse  false;        //Reverse the Front Right Encoder
#define encBRreverse  false;        //Reverse the Back Right Encoder

#define encoderFLchannelA 0;

#define encodingType  Encoder::k4X

//idk if this even works but ima try it

#define JOYZCAPABLE false               //are you using a z-axis capable controller (in our case the Logitech Log 3D pro)

/*____________________________________HUGE PROGRAMMING NOTE!!!_____________________________________
 * TO DEPLOY TO THE roboRIO, YOU MUST RIGHT CLICK ON THE BUILD.XML FILE,
 * AND SELECT "RUN AS ANT BUILD".
 * ECLIPSE NO LONGER GIVES THE OPTION TO DEPLOY AS C++ CODE!!!
 */

class Robot: public SampleRobot {
    RobotDrive robotDrive;      // robot drive system
    Talon liftOne;                  // the lift motor controller
    Talon liftTwo;
    Joystick driveStick;        // only joystick
    Joystick armStick;          //Not anymore, you stupid comment!
    // why are you so mean to me? -driveStick comment

    Encoder encoderFL;          //Front Left Encoder
    Encoder encoderBL;          //Back Left Encoder
    Encoder encoderFR;          //Front Right Encoder
    Encoder encoderBR;          //Back Right Encoder
    Encoder encoderLift;        //Take a wild guess

    AnalogInput bottomLimitSwitch; //limit switch for the bottom bar
    AnalogInput topLimitSwitch; //limit switch for the top bar

    int encoderPorts[10] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9 };

    int thing =0;
    float driveStick_x = 0;     //the first controller x axis value
    float driveStick_y = 0;     //the first controller y axis value
    float driveStick_z = 0;     //the first controller z axis value
    float armStick_y = 0;       // the second controller y axis value
    float attackRotRate = 0.75; //This is the rotation rate when using the buttons
    float slider = 0;           // i have no clue what this is supposed to be
    float currAvgPosition;
    float startPosition;

    bool throttleEnabled = true; //True means throttle is enabled, false not...
    bool rotationLock = true; //True means that button 2 is needed for rotation.
    bool defDriveMode = true; //True defaults to analog drive

public:
    //_____________________INITIALIZATION ROUTINES__________________________
    Robot() :
        robotDrive( frontLeftChannel,
                    rearLeftChannel,
                    frontRightChannel,
                    rearRightChannel), // these must be initialized in the same order
                liftOne(mainLiftChannel),           // as they are declared above.
                liftTwo(backupLiftChannel),
                driveStick(joystickDriveChannel),
                armStick(joystickArmChannel),
                encoderFL(encoderPorts[0],encoderPorts[1], false, encodingType),
                encoderBL(encoderPorts[2], encoderPorts[3], false, encodingType),
                encoderFR(encoderPorts[4], encoderPorts[5], false, encodingType),
                encoderBR(encoderPorts[6], encoderPorts[7], false, encodingType),
                encoderLift(encoderPorts[8], encoderPorts[9], false, encodingType),
                bottomLimitSwitch(1),
                topLimitSwitch(0)

{
        robotDrive.SetExpiration(0.1);
        robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, false);// do not invert the left side motors
        robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, false);
        robotDrive.SetInvertedMotor(RobotDrive::kFrontRightMotor, true);// invert the right side motors
        robotDrive.SetInvertedMotor(RobotDrive::kRearRightMotor, true);// you may need to change or remove this to match your robot
}

    float map(float x, float in_min, float in_max, float out_min, float out_max)//*Dalek Voice* EXPLAIIIIIIIIIIN
    {
        //_____________________FLOAT MAPPING__________________________
        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;//*Dalek Voice* EXPLAIIIIIIIIIIIIIIIIIIIIIIN
        //Takes in a variable, and variable range, spits out new value
    }

    void RoboInit()
    {
        float disPerTick = 0.013004; //Inches (I's small)
        //not compared to the size of an electron

        //Encoder encoderList = {encoderFL, encoderBL, encoderFR, encoderBR}; //at least we tried
    }

    void precisionDrive() {
        //_______________________________PRECISION DRIVE MODE_____________________________
        if (driveStick.GetPOV(1)) { //Forward
            driveStick_x = 0;
            driveStick_y = 1;
        } else if (driveStick.GetPOV(2)) { //Right Forward
            driveStick_x = 1;
            driveStick_y = 1;
        } else if (driveStick.GetPOV(3)) { //Right
            driveStick_x = 1;
            driveStick_y = 0;
        } else if (driveStick.GetPOV(4)) { //Right Backward
            driveStick_x = 1;
            driveStick_y = -1;
        } else if (driveStick.GetPOV(5)) { //Backward
            driveStick_x = 0;
            driveStick_y = -1;
        } else if (driveStick.GetPOV(6)) { //Left Backward
            driveStick_x = -1;
            driveStick_y = -1;
        } else if (driveStick.GetPOV(7)) { //Left
            driveStick_x = -1;
            driveStick_y = 0;
        } else if (driveStick.GetPOV(8)) { //Left Forward
            driveStick_x = -1;
            driveStick_y = 1;
        } else {
            driveStick_x = 0;
            driveStick_y = 0;
        }

        //CODE FOR PRECISION ROTATION
        // I couldn't re-do this one into a switch because of the way GetRawButton works
        if (driveStick.GetRawButton(3)) { //Left Rotation
            driveStick_z = -1;
        } else if (driveStick.GetRawButton(4)) { //Right Rotation
            driveStick_z = 1;
        } else { //No Rotation
            driveStick_z = 0;
        }
    }

    void analogDrive() {
        //_____________________________________NORMAL DRIVE MODE (ANALOG)_______________________________
        //Get Joystick Raw Values
        driveStick_x = driveStick.GetX();
        driveStick_y = driveStick.GetY();

        //Written to account for lack of Logitec 3DPro joystick (Z-axis rotation of joystick)
        //could be redone so that it takes into account any z-axis capable joystick (rename the declaration more or less)
        if (JOYZCAPABLE) {
            if (throttleEnabled) { //CUSTOM CONFIG If enabled: throttle is usable
                slider = map(driveStick.GetRawAxis(3), -1, 1, 1, 0);// how is the out min bigger than the out max?
            } else { //Locks throttle to 100%
                slider = 1;
            }

            driveStick_z = driveStick.GetZ(); //map(x, in_min, in_max, out_min, out_max)
            if (fabs(driveStick_z) < joystickThreshold
                    || (!driveStick.GetRawButton(2) && rotationLock)
                    || driveStick.GetRawButton(1)) { //why are we checking for button 1? Accidental turning?? if so then the check for 2 is redundant
                driveStick_z = 0;
            }
        }

        else { //we might want to redo all of this so that the button input is saved in a var that way we can use controllers other than ours
            if (driveStick.GetRawButton(4)) { //if button 4 is pressed (left?) turn (left?)
                driveStick_z = -attackRotRate;
            } else if (driveStick.GetRawButton(5)) { //if button 5 is pressed (right?) turn (right?)
                driveStick_z = attackRotRate;
            } else {
                driveStick_z = 0;       //you press nothing... NATHING!
            }
            slider = map(driveStick.GetRawAxis(2), -1, 1, 1, 0); //wat? i is confuzed by slider
        }

        //Threshold Value Setting
        if (fabs(driveStick_x) < joystickThreshold
                || driveStick.GetRawButton(1)) { // if the x value is inside of the "dead" zone or you are holding the trigger set the x value to 0
            driveStick_x = 0;
        }
        if (fabs(driveStick_y) < joystickThreshold
                || driveStick.GetRawButton(1)) { // if the y value is inside of the "dead" zone or you are holding the trigger se thte y value to 0
            driveStick_y = 0;
        }
    }

    void joySettings() {
        if (driveStick.GetRawButton(7)) { //Enables analog default drive
            defDriveMode = true;
        } else if (driveStick.GetRawButton(8)) { //Enables precision drive
            defDriveMode = false;
        }

        if (driveStick.GetRawButton(9)) { //Enables Rotation Lock Mode
            rotationLock = true;
        } else if (driveStick.GetRawButton(10)) { //Disables rotation lock
            rotationLock = false;
        }

        if (driveStick.GetRawButton(11)) { //Enables throttle
            throttleEnabled = true;
        } else if (driveStick.GetRawButton(12)) { //Disables Throttle (100%)
            throttleEnabled = false;
        }
    }

    void driveEncoders(float distance, float rotation)
    {

    }

    void driveEncoders(float distance) {

    }

    void logOutput() {
        //outputs to the smart dashboard so we can see real-time values.

        SmartDashboard::PutNumber("Joystick X ", double(driveStick_x));
        SmartDashboard::PutNumber("Joystick Y ", double(driveStick_y));
        SmartDashboard::PutNumber("Joystick Z ", double(driveStick_z));
        SmartDashboard::PutNumber("Throttle ", double(slider));
        //SmartDashboard::PutNumber("Top Limit ", double(topLimitSwitch.GetValue()));
        //SmartDashboard::PutNumber("Bottom Limit", double(bottomLimitSwitch->Get()));
    }

    void liftControl() {
        //__________________________LIFT_SECTION_OF_MAIN_LOOP______________________
        armStick_y = armStick.GetY();//set the arm raising thing

        if (fabs(armStick_y) < joystickThreshold){// ||
                //(armStick_y < 0 && (topLimitSwitch.GetValue()) > 2058) ||
                //(armStick_y > 0 && (bottomLimitSwitch.GetValue() > 2058))){
            // if armStick_y is in the dead zone or if you reached the top limit
            //or if you set the bottom limit set arm movement to 0
            armStick_y = 0;
        }

        liftOne.SetSpeed(armStick_y); // THEN THE CODE SAID LET THERE BE LIFT!!!! maybe..
        liftTwo.SetSpeed(armStick_y);// THEN THE CODE SAID LET THERE BE LIFT!!!! maybe.. AGAIN!
    }

    void driveModeSelect() {
        //___________________________DRIVE_SECTION_OF_MAIN_LOOP_______________________
        //if you are holding the trigger use precision drive, else use regular drive
        // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
        // This sample does not use field-oriented drive, so the gyro input is set to zero.

        if (defDriveMode) { //Analog Drive is Default
            if (!driveStick.GetRawButton(1)) { //"Button 1" is the trigger.
                analogDrive();//Button 2 is for Z-Rotation Lock... And think about it:
                // How do you hit the thumb button if you're using your thumb on the hat switch?
            } else {
                precisionDrive();
            }
        } else {
            if (!driveStick.GetRawButton(1)) {
                precisionDrive();
            } else {
                analogDrive();
            }
        }
        robotDrive.MecanumDrive_Cartesian(driveStick_x * joystickLimit * slider,
                driveStick_y * joystickLimit * slider,
                driveStick_z * joystickLimit * slider); //Actually do the moving stuff
    }

    void OperatorControl() {

        robotDrive.SetSafetyEnabled(false);     //no safety for you

        while (IsOperatorControl() && IsEnabled()) {
            joySettings();
            driveModeSelect();
            liftControl();
            logOutput();
            Wait(0.005); // wait 5ms to avoid hogging CPU cycles
        }
    }

    /*void Autonomous()
             {
             while(IsAutonomous() && IsEnabled())
             {
             if(AutonChoice == 1)
             {

             }
             }
             }*/

};

START_ROBOT_CLASS(Robot); //Initializes the class so the robot actually does stuff

Say hi to Mecanum drive!

I have not a single clue as to how it expanded so fast. It was only a hundred lines a few days ago!

[Newt]

Our FTC competition is in less than a week, so we are trying to finish up our autonomous and general final tweaks today. So far we are able to get preloaded balls into a goal quite reliably and I am glad (but still nervous). At our last competition, there was a robot that was placed on the ramp backwards, and in its autonomous drove itself over the edge, crashing and damaging itself, while also removing itself from that game (the team fixed it up and was able to advance anyway).

Sorry about the nintendo and other errors.

I like some of the comments in your code 'EXPLAIIIIIN'. Hopefully all will go well for your FRC, and without annoying nintendos:).

[Starwhip]

Our autonomous program was really nice. We drove down the ramp, put a ball in the 60 centimeter goal, rotated and put the goal in the parking zone. It worked great.

As for the comments, that's what you get when you've got three people working on the same code. Hoping it goes well in FRC too.

[Newt]

Our autonomous can drive down, load the 30cm goal, put it in the home base, and then knock out the stick (provided no one gets in our way). Incredibly, we have also had the robot hit the wall on accident, turn too much, and drive the loaded goal up the ramp by itself (which is neat, but not so great...).

The code has some silly comments, but mostly it has been made by us all working at the same time, and thus there have been less occasions where questions were needed. The greater silliness comes from long-winded names, which sometimes seem to try to explain in one sentence (without spaces) exactly what the program should do and when to use it .

[Starwhip]

Holy cow:

#include "WPILib.h"
#include <thread>
#include <tuple>
#include <cmath>
/*
 These are pre-processing directives, the compiler will find
 in your code all these names, and replace them with the numbers
 listed beside them.
 */

#define frontLeftChannel  0         //the port for the front left drive motor
#define frontRightChannel 1         //the port for the front right drive motor
#define rearLeftChannel   2         //the port for the back left drive motor
#define rearRightChannel  3         //the port for the back right drive motor
#define liftChannel       4         //the port for the main lift motor
#define rollerOneChannel  6
#define rollerTwoChannel  7

#define rightRollerSolenoidChannel   0
#define leftRollerSolenoidChannel    1
#define rightForkSolenoidChannel     2
#define leftForkSolenoidChannel      3

#define joystickDriveChannel   0    //what position in the controller array is the drive controller
#define joystickArmChannel     1    //what position in the controller array is the lift controller

#define joystickThreshold 0.15      //the "dead" zone of the controller
#define joystickLimit     1.0       // % of range (1.0 = 100%, .75 = 75% etc.)

#define encFLreverse  false;        //Reverse the Front Left Encoder
#define encBLreverse  false;        //Reverse the Back Left Encoder
#define encFRreverse  false;        //Reverse the Front Right Encoder
#define encBRreverse  false;        //Reverse the Back Right Encoder

#define encoderFLchannelA 0;

#define encodingType  Encoder::k4X

//idk if this even works but ima try it

#define JOYZCAPABLE false               //are you using a z-axis capable controller (in our case the Logitech Log 3D pro)

/*____________________________________HUGE PROGRAMMING NOTE!!!_____________________________________
 * TO DEPLOY TO THE roboRIO, YOU MUST RIGHT CLICK ON THE BUILD.XML FILE,
 * AND SELECT "RUN AS ANT BUILD".
 * ECLIPSE NO LONGER GIVES THE OPTION TO DEPLOY AS C++ CODE!!!
 */

//Autonomous turning does not work as of now.

class Robot: public SampleRobot {
    RobotDrive robotDrive;      // robot drive system
    Talon liftMotor;                  // the lift motor controller
    //Talon liftTwo;
    Talon rollerOne;
    Talon rollerTwo;

    Joystick driveStick;        // only joystick
    Joystick armStick;          //Not anymore, you stupid comment!
    // why are you so mean to me? -driveStick comment

    Encoder encoderFL;          //Front Left Encoder
    Encoder encoderBL;          //Back Left Encoder
    Encoder encoderFR;          //Front Right Encoder
    Encoder encoderBR;          //Back Right Encoder
    Encoder encoderLift;        //Take a wild guess

    AnalogInput bottomLimitSwitch; //limit switch for the bottom bar
    AnalogInput topLimitSwitch; //limit switch for the top bar
    AnalogInput toteLimitSwitch;

    Solenoid rightArm;
    Solenoid leftArm;
    Solenoid rightFork;
    Solenoid leftFork;

    int encoderPorts[10] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9 };

    int FLdis;
    int FRdis;
    int BLdis;
    int BRdis;


    int Auton = 0;
    int AutonChoice = 1;
    int rollerOneMult = 1;
    int rollerTwoMult = -1;

    float driveStick_x = 0;     //the first controller x axis value
    float driveStick_y = 0;     //the first controller y axis value
    float driveStick_z = 0;     //the first controller z axis value
    float armStick_y = 0;       // the second controller y axis value
    float attackRotRate = 0.75; //This is the rotation rate when using the buttons
    float slider = 0;           // i have no clue what this is supposed to be
    float currAvgPosition;
    float startPosition;
    float rollerSpeed = 1;

    bool throttleEnabled = true; //True means throttle is enabled, false not...
    bool rotationLock = true; //True means that button 2 is needed for rotation.
    bool defDriveMode = true; //True defaults to analog drive
    bool topSwitchPressed = false;
    bool bottomSwitchPressed = false;

    bool rollerArmsOut = false;

    double wheelDiam = 5.98; //Inches

public:
    //_____________________INITIALIZATION ROUTINES__________________________
    Robot() :
        robotDrive( frontLeftChannel,
                rearLeftChannel,
                frontRightChannel,
                rearRightChannel), // these must be initialized in the same order
                liftMotor(liftChannel),           // as they are declared above.
                //liftTwo(backupLiftChannel),
                rollerOne(rollerOneChannel),
                rollerTwo(rollerTwoChannel),
                driveStick(joystickDriveChannel),
                armStick(joystickArmChannel),
                encoderFL(encoderPorts[0],encoderPorts[1], false, encodingType),
                encoderBL(encoderPorts[2], encoderPorts[3], false, encodingType),
                encoderFR(encoderPorts[4], encoderPorts[5], true, encodingType),
                encoderBR(encoderPorts[6], encoderPorts[7], true, encodingType),
                encoderLift(encoderPorts[8], encoderPorts[9], false, encodingType),
                bottomLimitSwitch(0),
                topLimitSwitch(1),
                toteLimitSwitch(2),
                rightArm(rightRollerSolenoidChannel),
                leftArm(leftRollerSolenoidChannel),
                rightFork(rightForkSolenoidChannel),
                leftFork(leftForkSolenoidChannel)

{
        robotDrive.SetExpiration(0.1);
        robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, false);// do not invert the left side motors
        robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, false);
        robotDrive.SetInvertedMotor(RobotDrive::kFrontRightMotor, true);// invert the right side motors
        robotDrive.SetInvertedMotor(RobotDrive::kRearRightMotor, true);// you may need to change or remove this to match your robot
}

    float map(float x, float in_min, float in_max, float out_min, float out_max)//*Dalek Voice* EXPLAIIIIIIIIIIN
    {
        //_____________________FLOAT MAPPING__________________________
        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;//*Dalek Voice* EXPLAIIIIIIIIIIIIIIIIIIIIIIN
        //Takes in a variable, and variable range, spits out new value
    }

    float degToRad(float deg){
        return (deg*M_PI) / 180;
    }

    float radToDeg(float rad){
        return (rad*180)/M_PI;
    }

    void rollerSpin(bool in)
    {
        if(in){
            rollerOne.SetSpeed(rollerSpeed); //Rollers IN
            rollerTwo.SetSpeed(-rollerSpeed);
        }
        else{
            rollerOne.SetSpeed(-rollerSpeed); //Rollers OUT
            rollerTwo.SetSpeed(rollerSpeed);
        }
    }

    void rollerArms(bool extend){
        if(extend){
            leftArm.Set(true);
            Wait(.05);
            rightArm.Set(true);
            rollerArmsOut = true;
        }
        else{
            rightArm.Set(false);
            Wait(.05);
            leftArm.Set(false);
            rollerArmsOut = false;
        }
    }

    void joySettings() {
        if (driveStick.GetRawButton(7)) { //Enables analog default drive
            defDriveMode = true;
        } else if (driveStick.GetRawButton(8)) { //Enables precision drive
            defDriveMode = false;
        }

        if (driveStick.GetRawButton(9)) { //Enables Rotation Lock Mode
            rotationLock = true;
        } else if (driveStick.GetRawButton(10)) { //Disables rotation lock
            rotationLock = false;
        }

        if (driveStick.GetRawButton(11)) { //Enables throttle
            throttleEnabled = true;
        } else if (driveStick.GetRawButton(12)) { //Disables Throttle (100%)
            throttleEnabled = false;
        }
    }

    void virtualDriveStick(float l_angle,float l_rot,float l_mag,float l_throttle){
        //Angle 0 is rightmost, PI is rightmost
        slider = l_throttle; //Bad naming convention is bad... Sets throttle
        driveStick_x = l_mag * cos(l_angle); //X axis of "joystick"
        driveStick_y = l_mag * sin(l_angle); //Y axis of "joystick"
        driveStick_z = l_rot;                //Rotation Zed Axis
    }

    void killMotors(){
        virtualDriveStick(0,0,0,0); //No drive or rotation of "joystick"
        drive();
        liftControl(0);
    }

    float getAvgEncoder(Encoder enc1, Encoder enc2, Encoder enc3, Encoder enc4){
        //Average encoder value. So far unused.
        //and that makes me sad.
        return (enc1.GetDistance()+enc2.GetDistance()+enc3.GetDistance()+enc4.GetDistance())/4;
    }

    std::tuple<double,double> getRobotDis(){
        //Gets your x and y components of translation
        double enc1Dis = encoderFL.GetDistance(); //FL
        double enc2Dis = encoderBL.GetDistance(); //BL
        double enc3Dis = encoderFR.GetDistance(); //FR
        double enc4Dis = encoderBR.GetDistance(); //BR

        if(!encoderFL.GetDirection()){ //Lots of complicated stuff to get direction of encoders.
            enc1Dis *= -1;             //Encoder.GetDirection() returns true or false, false is default and "backwards".
        }
        if(!encoderBL.GetDirection()){
            enc2Dis *= -1;
        }
        if(!encoderFR.GetDirection()){
            enc3Dis *= -1;
        }
        if(!encoderBR.GetDirection()){
            enc4Dis *= -1;
        }

        double robotVector_x = (-(enc1Dis+enc2Dis)+enc3Dis+enc4Dis)*cos(M_PI/4); //YAY, summations
        double robotVector_y = (-(enc2Dis+enc4Dis)+enc3Dis+enc1Dis)*sin(M_PI/4);

        return std::make_tuple(robotVector_x,robotVector_y); //WPI lib says hi. C++ doesn't support tuples by default
    }

    void RoboInit()
    {
        double wheelCirc = wheelDiam * M_PI;
        double disPerTick = wheelCirc / 1440; //Inches (I's small)
        //not compared to the size of an electron
        //Encoder encoderList = {encoderFL, encoderBL, encoderFR, encoderBR}; //at least we tried

        encoderFL.SetDistancePerPulse(disPerTick); //So that encoders return inches
        encoderBL.SetDistancePerPulse(disPerTick);
        encoderFR.SetDistancePerPulse(disPerTick);
        encoderBR.SetDistancePerPulse(disPerTick);
        //close solenoids
    }

    void precisionControl() {
        //_______________________________PRECISION DRIVE MODE_____________________________
        int povAngle = driveStick.GetPOV(0);
        if(povAngle == -1){
            killMotors();
        }
        if (driveStick.GetRawButton(3)) { //Left Rotation
            driveStick_z = -1;
        } else if (driveStick.GetRawButton(4)) { //Right Rotation
            driveStick_z = 1;
        } else { //No Rotation
            driveStick_z = 0;
        }
        virtualDriveStick(degToRad(povAngle),driveStick_z,1,1);
    }

    void analogControl() {
        //_____________________________________NORMAL DRIVE MODE (ANALOG)_______________________________
        //Get Joystick Raw Values
        driveStick_x = driveStick.GetX();
        driveStick_y = driveStick.GetY();

        //Written to account for lack of Logitec 3DPro joystick (Z-axis rotation of joystick)
        //could be redone so that it takes into account any z-axis capable joystick (rename the declaration more or less)
        if (JOYZCAPABLE) {
            if (throttleEnabled) { //CUSTOM CONFIG If enabled: throttle is usable
                slider = map(driveStick.GetRawAxis(3), -1, 1, 1, 0);// how is the out min bigger than the out max?
            } else { //Locks throttle to 100%
                slider = 1;
            }

            driveStick_z = driveStick.GetZ(); //map(x, in_min, in_max, out_min, out_max)
            if (fabs(driveStick_z) < joystickThreshold
                    || (!driveStick.GetRawButton(2) && rotationLock)
                    || driveStick.GetRawButton(1)) { //why are we checking for button 1? Accidental turning?? if so then the check for 2 is redundant
                driveStick_z = 0;
            }
        }

        else { //we might want to redo all of this so that the button input is saved in a var that way we can use controllers other than ours
            if (driveStick.GetRawButton(4)) { //if button 4 is pressed (left?) turn (left?)
                driveStick_z = -attackRotRate;
            } else if (driveStick.GetRawButton(5)) { //if button 5 is pressed (right?) turn (right?)
                driveStick_z = attackRotRate;
            } else {
                driveStick_z = 0;       //you press nothing... NATHING!
            }
            slider = map(driveStick.GetRawAxis(2), -1, 1, 1, 0); //wat? i is confuzed by slider
        }

        //Threshold Value Setting
        if (fabs(driveStick_x) < joystickThreshold
                || driveStick.GetRawButton(1)) { // if the x value is inside of the "dead" zone or you are holding the trigger set the x value to 0
            driveStick_x = 0;
        }
        if (fabs(driveStick_y) < joystickThreshold
                || driveStick.GetRawButton(1)) { // if the y value is inside of the "dead" zone or you are holding the trigger set the y value to 0
            driveStick_y = 0;
        }
    }

    void liftControl(float yAxis) {
        //__________________________LIFT_SECTION_OF_MAIN_LOOP______________________

        topSwitchPressed = (topLimitSwitch.GetValue() < 2048) ? false : true;
        bottomSwitchPressed = (bottomLimitSwitch.GetValue() < 2048) ? false : true;

        if((topSwitchPressed && yAxis >= 0) ||
                (fabs(yAxis) <= joystickThreshold) ||
                (bottomSwitchPressed && yAxis <= 0)){

            liftMotor.SetSpeed(0);
        }
        else{
            liftMotor.SetSpeed(yAxis);
        }
    }

    void liftArm(float speed, float time){
        liftControl(speed);
        Wait(time);
        killMotors();
    }

    void rollerArmSolenoidControl(){
        if (armStick.GetRawButton(7) && toteLimitSwitch.GetValue() < 2048){ //Retract Roller Arms!
            rollerArms(false);
        }
        else if (armStick.GetRawButton(6)){ //Extend Roller Arms!
            rollerArms(true);
        }
        if (armStick.GetRawButton(1) && rollerArmsOut){ //Trigger: Used to suck in totes
            rollerSpin(true);
        }
        else if (armStick.GetRawButton(3) && rollerArmsOut){ //Middlemost button of joystick. Spits out totes
            rollerSpin(false);
        }

    }

    void forkArms(bool extend){
        if(extend){
            rightFork.Set(true);
            leftFork.Set(true);
        }
        else{
            rightFork.Set(false);
            leftFork.Set(false);
        }
    }

    void forkArmSolenoidControl(){
        if(armStick.GetRawButton(11)){
            forkArms(true);
        }
        else if(armStick.GetRawButton(10)){
            forkArms(false);
        }
    }

    void logOutput() {
        //outputs to the smart dashboard so we can see real-time values.

        SmartDashboard::PutNumber("Joystick X ", double(driveStick_x));
        SmartDashboard::PutNumber("Joystick Y ", double(driveStick_y));
        SmartDashboard::PutNumber("Joystick Z ", double(driveStick_z));
        SmartDashboard::PutNumber("Throttle ", double(slider));
        //SmartDashboard::PutNumber("Top Limit ", double(topLimitSwitch.GetValue()));
        //SmartDashboard::PutNumber("Bottom Limit", double(bottomLimitSwitch->Get()));
    }

    void drive(){
        //Instead of copy-pasting below
        robotDrive.MecanumDrive_Cartesian(driveStick_x * joystickLimit * slider,
                driveStick_y * joystickLimit * slider,
                driveStick_z * joystickLimit * slider); //Actually do the moving stuff
    }

    void driveAuto(float dis,float angle,float speed,float rot){
        //Autopilot drive system. Takes a distance at angle from front of robot, a speed, and a rotation value
        virtualDriveStick(angle,rot,speed,1); //Throttle is 1 by convention. Speed controls speed.

        float dis_x = dis*cos(degToRad(angle)); //Distance to move (X-axis)
        float dis_y = dis*sin(degToRad(angle)); //Other distance (Y-axis)

        double vec_x; //Encoder value X
        double vec_y; //Encoder value Y

        drive(); //ENGAGE MOTORS!

        while(vec_x >= dis_x || vec_y >= dis_y){ //If it's gone too far...
            std::tie(vec_x,vec_y) = getRobotDis(); //Update distance
        }

        killMotors(); //NO JOYSTICK FOR JU
    }

    void driveModeSelect() {
        //___________________________DRIVE_SECTION_OF_MAIN_LOOP_______________________
        //if you are holding the trigger use precision drive, else use regular drive
        // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
        // This sample does not use field-oriented drive, so the gyro input is set to zero.

        if (defDriveMode) { //Analog Drive is Default
            if (!driveStick.GetRawButton(1)) { //"Button 1" is the trigger.
                analogControl();//Button 2 is for Z-Rotation Lock... And think about it:
                // How do you hit the thumb button if you're using your thumb on the hat switch?
            } else {
                precisionControl();
            }
        } else {
            if (!driveStick.GetRawButton(1)) {
                precisionControl();
            } else {
                analogControl();
            }
        }
        drive();
    }

    void OperatorControl() {

        robotDrive.SetSafetyEnabled(false);     //no safety for you

        while (IsOperatorControl() && IsEnabled()) {
            joySettings();
            driveModeSelect();
            liftControl(armStick.GetY());
            rollerArmSolenoidControl();
            forkArmSolenoidControl();

            logOutput();
            Wait(0.005); // wait 5ms to avoid hogging CPU cycles
        }
    }

    void Autonomous()
    {
        while(IsAutonomous() && IsEnabled())
        {
            if(Auton != 1 && Auton != 2 && Auton != 3 && Auton != 4){
                Auton = 1;
            }

            switch (Auton){
            case 1:
                driveAuto(140,90,0.75,0); //Go to auto zone
                break;
            case 2:
                liftArm(0.75,0.5); //Lift up tote
                driveAuto(0,0,0.75,90); //Rotate 90 degrees
                Wait(0.1);
                driveAuto(140,90,0.75,0); //Go to auto zone
                break;
            case 3:
                forkArms(true); //Extend fork
                Wait(0.5);
                liftArm(0.75,0.5); //Lift tote
                driveAuto(0,0,0.75,-90); //Rotate left 90
                Wait(0.1);
                driveAuto(140,90,0.75,0); //Go to auto zone
                break;

            case 4:
                forkArms(true); //Extend fork
                Wait(0.5);
                liftArm(0.75,1.5); //Raise arm and pick up trash can
                driveAuto(24,90,0.75,0); //Drive forward 2 feet to tote
                Wait(0.1);
                liftArm(-0.75,1.0); //Lower arm, drop trash can on tote
                forkArms(false); //Retract fork
                Wait(0.5);
                driveAuto(8,90,0.75,0); //Drive forward 8 inches
                Wait(0.1);
                liftArm(-0.75,1.5); //Lower arm under tote
                liftArm(0.75,1.5); //Raise arm, pick up both
                driveAuto(0,0,0.75,-90); //Rotate 90 left
                Wait(0.1);
                driveAuto(140,90,0.75,0); //Go to parking zone
                break;
            }
        }
    }

};

START_ROBOT_CLASS(Robot); //Initializes the class so the robot actually does stuff

Five Hundred and Twelve lines of code.

And all of it yet untested!

GO BUILD TEAM!

Seriously, the programming team never gets a chance to test anything... competition should be interesting.

It should now be able to to whatever the heck we want it to: just a couple lines and you've got another autonomous program!