Public Pastes
SHARE
TWEET

FRC Team 6155 2016 Code

YWLAElektraBots Feb 7th, 2016 124 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "WPILib.h"
  2. #include "DriveFunc.h"
  3.  
  4. #define DEADZONE .3
  5. #define DRIVERX_0 1
  6. #define DRIVERX_1 5
  7. #define OPERATORX_0 1
  8.  
  9. #define LEFT_REVERSE
  10. //#define RIGHT_REVERSE
  11. //#define INTAKE_REVERSE
  12.  
  13. int autonomous;
  14.  
  15. class Robot: public SampleRobot
  16. {
  17.     Joystick _driver, _operator;
  18.     VictorSP left_0, left_1, right_0, right_1;
  19.     Victor intake_0;
  20.     RobotDrive Robot;
  21. #ifdef LEFT_REVERSE
  22.     int leftPolarity = -1;
  23. #else
  24.     int leftPolarity = 1;
  25. #endif
  26.  
  27. #ifdef RIGHT_REVERSE
  28.     int rightPolarity = -1;
  29. #else
  30.     int rightPolarity = 1;
  31. #endif
  32.  
  33. #ifdef INTAKE_REVERSE
  34.     int intakePolarity = -1;
  35. #else
  36.     int intakePolarity = 1;
  37. #endif
  38.  
  39. public:
  40.     Robot() :
  41.         _driver(0),
  42.         _operator(1),
  43.         left_0(0),
  44.         left_1(1),
  45.         right_0(2),
  46.         right_1(3),
  47.         intake_0(4)
  48.     {
  49.     }
  50.  
  51.     void RobotInit()
  52.     {
  53.         SmartDashboard::PutNumber("auton", 0);
  54.     }
  55.  
  56.  
  57.     void Autonomous()
  58.     {
  59.         autonomous = SmartDashboard::GetNumber("auton", 0);
  60.  
  61.         if(autonomous == 1)
  62.         {
  63.             cout << "ONE!";
  64.         Robot.SetSafetyEnabled(false);
  65.         Robot.Drive(0.5, 0);
  66.         Wait(2);
  67.         Robot.Drive(0, 0);
  68.  
  69.         }
  70.         else if(autonomous == 2)
  71.         {
  72.             cout << "TWO!";
  73.         }
  74.         else{cout << "NONE!";}
  75.     }
  76.  
  77.  
  78.     void OperatorControl()
  79.     {
  80.  
  81.         while (IsOperatorControl() && IsEnabled())
  82.         {
  83.             double driverX_0 = _driver.GetRawAxis(DRIVERX_0);
  84.             double driverX_1 = _driver.GetRawAxis(DRIVERX_1);
  85.             double operatorX_0 = _operator.GetRawAxis(OPERATORX_0);
  86.  
  87.             if(driverX_0 > DEADZONE || driverX_0 < -DEADZONE)
  88.             {
  89.                 left_0.Set(leftPolarity*LinearValueEstimator(driverX_0, DEADZONE));
  90.                 left_1.Set(leftPolarity*LinearValueEstimator(driverX_0, DEADZONE));
  91.             }
  92.             else
  93.             {
  94.                 left_0.StopMotor();
  95.                 left_1.StopMotor();
  96.             }
  97.  
  98.             if(driverX_1 > DEADZONE || driverX_1 < -DEADZONE)
  99.             {
  100.                 right_0.Set(rightPolarity*LinearValueEstimator(driverX_1, DEADZONE));
  101.                 right_1.Set(rightPolarity*LinearValueEstimator(driverX_1, DEADZONE));
  102.             }
  103.             else
  104.             {
  105.                 right_0.StopMotor();
  106.                 right_1.StopMotor();
  107.             }
  108.  
  109.             if(operatorX_0 > DEADZONE || operatorX_0 < -DEADZONE)
  110.                 intake_0.Set(intakePolarity*LinearValueEstimator(operatorX_0, DEADZONE));
  111.             else
  112.                 intake_0.StopMotor();
  113.  
  114.  
  115.  
  116.  
  117.             Wait(0.005);
  118.         }
  119.     }
  120.  
  121.  
  122.     void Test()
  123.     {
  124.     }
  125. };
  126.  
  127. START_ROBOT_CLASS(Robot)
RAW Paste Data
Top