ElektraBots 2016 Code

#include "WPILib.h"
#include "DriveFunc.h"

#define DEADZONE .3
#define DRIVERX_0 1
#define DRIVERX_1 5
#define OPERATORX_0 1

#define LEFT_REVERSE
//#define RIGHT_REVERSE
//#define INTAKE_REVERSE

int autonomous;

class Robot: public SampleRobot
{
    Joystick _driver, _operator;
    VictorSP left_0, left_1, right_0, right_1;
    Victor intake_0;

#ifdef LEFT_REVERSE
    int leftPolarity = -1;
#else
    int leftPolarity = 1;
#endif

#ifdef RIGHT_REVERSE
    int rightPolarity = -1;
#else
    int rightPolarity = 1;
#endif

#ifdef INTAKE_REVERSE
    int intakePolarity = -1;
#else
    int intakePolarity = 1;
#endif

public:
    Robot() :
        _driver(0),
        _operator(1),
        left_0(0),
        left_1(1),
        right_0(2),
        right_1(3),
        intake_0(4)

    {
    }

    void RobotInit()
    {
        SmartDashboard::PutNumber("auton", 0);
    }


    void Autonomous()
    {
        autonomous = SmartDashboard::GetNumber("auton", 0);

        if(autonomous == 1)
        {
            cout << "ONE!";
            left_0.Set(leftPolarity*LinearValueEstimator(-0.5, 0.0));
            right_0.Set(leftPolarity*LinearValueEstimator(-0.5, 0.0));
            left_1.Set(leftPolarity*LinearValueEstimator(-0.5, 0.0));
            right_1.Set(leftPolarity*LinearValueEstimator(-0.5, 0.0));
            Wait(0.005);
        }
        else if(autonomous == 2)
        {
            cout << "TWO!";
        }
        else{cout << "NONE!";}
    }


    void OperatorControl()
    {

        while (IsOperatorControl() && IsEnabled())
        {
            double driverX_0 = _driver.GetRawAxis(DRIVERX_0);
            double driverX_1 = _driver.GetRawAxis(DRIVERX_1);
            double operatorX_0 = _operator.GetRawAxis(OPERATORX_0);

            if(driverX_0 > DEADZONE || driverX_0 < -DEADZONE)
            {
                left_0.Set(leftPolarity*LinearValueEstimator(driverX_0, DEADZONE));
                left_1.Set(leftPolarity*LinearValueEstimator(driverX_0, DEADZONE));
            }
            else
            {
                left_0.StopMotor();
                left_1.StopMotor();
            }

            if(driverX_1 > DEADZONE || driverX_1 < -DEADZONE)
            {
                right_0.Set(rightPolarity*LinearValueEstimator(driverX_1, DEADZONE));
                right_1.Set(rightPolarity*LinearValueEstimator(driverX_1, DEADZONE));
            }
            else
            {
                right_0.StopMotor();
                right_1.StopMotor();
            }

            if(operatorX_0 > DEADZONE || operatorX_0 < -DEADZONE)
                intake_0.Set(intakePolarity*LinearValueEstimator(operatorX_0, DEADZONE));
            else
                intake_0.StopMotor();




            Wait(0.005);
        }
    }


    void Test()
    {
    }
};

START_ROBOT_CLASS(Robot)