API tools faq
paste
Advertisement
Guest User

Beta Code 2/23/18

a guest
Feb 23rd, 2018
86
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 17.12 KB | None | 0 0
raw download clone embed print report
  1. package org.usfirst.frc.team5510.robot;
  2.  
  3. import edu.wpi.first.wpilibj.*;
  4. import edu.wpi.first.wpilibj.drive.DifferentialDrive;
  5. //import edu.wpi.first.wpilibj.drive.DifferentialDrive;
  6. //import edu.wpi.first.wpilibj.IterativeRobot;
  7. //import edu.wpi.first.wpilibj.Joystick;
  8. //import edu.wpi.first.wpilibj.RobotDrive;
  9. //import edu.wpi.first.wpilibj.drive.DifferentialDrive;
  10. //import edu.wpi.first.wpilibj.Spark;
  11. //import edu.wpi.first.wpilibj.Talon;
  12. //import edu.wpi.first.wpilibj.Timer;
  13. //import edu.wpi.first.wpilibj.Timer.StaticInterface;
  14. //import edu.wpi.first.wpilibj.Victor;
  15. import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
  16. import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  17.  
  18.  
  19. public class Robot extends IterativeRobot {
  20.     private SendableChooser<String> m_chooser = new SendableChooser<>();
  21.  
  22.     final String leftStartAlpha = "Left Start Alpha";
  23.     final String midStartAlpha = "Mid Start Alpha";
  24.     final String rightStartAlpha = "Right Start Alpha";
  25.     final String leftStartBeta = "Left Start Beta";
  26.     final String midStartBeta = "Mid Start Beta";
  27.     final String rightStartBeta = "Right Start Beta";
  28.     final String shtf = "Sh*t Hits The Fan";
  29.     String autoSelected;
  30.     SendableChooser <String> chooser = new SendableChooser<>();
  31.    
  32.     Victor leftFront;
  33.     Victor leftBack;
  34.     Victor rightFront;
  35.     Victor rightBack;
  36.    
  37.     Victor cubeLeft;
  38.     Victor cubeRight;
  39.    
  40.     Victor climber;
  41.     Victor elevator;
  42.  
  43.     boolean onDrive;
  44.     boolean driveArcade;
  45.     boolean driveGTA;
  46.     boolean driveReverse;
  47.    
  48.     boolean speedNos;
  49.     boolean speedCreep;
  50.     boolean speedNormal;
  51.    
  52.     Joystick xboxController; //mechanism
  53.     Joystick logitechController; //drive
  54.    
  55.     DigitalInput climberLimitSwitch;
  56.    
  57.     double autoTimerStart;
  58.    
  59.     DifferentialDrive tomato;
  60.    
  61.     @Override
  62.     public void robotInit() {
  63.         //m_chooser.addDefault("Default Auto", kDefaultAuto);
  64.         //m_chooser.addObject("My Auto", kCustomAuto);
  65.         SmartDashboard.putData("Auto choices", m_chooser);
  66.        
  67.          chooser.addObject("Left Start Alpha", leftStartAlpha);
  68.             chooser.addObject("Mid Start Alpha", midStartAlpha);
  69.             chooser.addObject("Right Start Alpha" , rightStartAlpha);
  70.             chooser.addObject("Left Start Beta", leftStartBeta);
  71.             chooser.addObject("Mid Start Beta", midStartBeta);
  72.             chooser.addObject("Right Start Beta", rightStartBeta);
  73.             chooser.addObject("Sh*t Hits The Fan", shtf);
  74.            
  75.             SmartDashboard.putData("Auto modes", chooser);
  76.            
  77.             logitechController = new Joystick(0);
  78.             xboxController = new Joystick(1);
  79.            
  80.             onDrive = false;
  81.             driveArcade = false;
  82.             driveGTA = false;
  83.             driveReverse=false;
  84.             speedCreep = false;
  85.             speedNos = false;
  86.             speedNormal = true;
  87.            
  88.             rightFront = new Victor (1); //port 1
  89.             rightBack = new Victor (2); //port 2
  90.             leftFront = new Victor (3); //port 3
  91.             leftBack = new Victor (4); //port 4
  92.             cubeLeft = new Victor (8); //ASSUMING THE TWO MOTORS ARE IN PORTS 6 AND 7
  93.             cubeRight = new Victor (7);
  94.             climber = new Victor (5);
  95.             elevator = new Victor (6);
  96.            
  97.             climberLimitSwitch = new DigitalInput(1);
  98.            
  99.             tomato = new DifferentialDrive(new SpeedControllerGroup(leftFront, leftBack),
  100.                        new SpeedControllerGroup(rightFront, rightBack));
  101.            
  102.      
  103.     }
  104.    
  105.    
  106.     @Override
  107.     public void autonomousInit() {
  108.         //m_autoSelected = m_chooser.getSelected();
  109.         //System.out.println("Auto selected: " + m_autoSelected);
  110.     }
  111.    
  112.  
  113.    
  114.     @Override
  115.     public void autonomousPeriodic() {
  116.  
  117.          double currTimer = Timer.getFPGATimestamp();
  118.          double timeElapsed = currTimer-autoTimerStart;
  119.        
  120.         String gameData;
  121.         gameData = DriverStation.getInstance().getGameSpecificMessage();
  122.                 if(gameData.length() > 0)
  123.                 {
  124.           if(gameData.charAt(0) == 'L')
  125.           {
  126.             //Put left auto code here
  127.               switch (autoSelected) {    
  128.                
  129.                 case leftStartAlpha:
  130.                     // Put Mid Start code here
  131.                     if (timeElapsed <2){
  132.                         tomato.arcadeDrive(-0.6, 0.0);
  133.                     }
  134.                    
  135.                     else if (timeElapsed <5){
  136.                         tomato.arcadeDrive(0.6, 0.0);
  137.                     }
  138.                    
  139.                     else if (timeElapsed < 6) {
  140.                         tomato.arcadeDrive(0.2, 0.0);
  141.                     }
  142.                     else if (timeElapsed>=15){
  143.                         tomato.arcadeDrive(0.0, 0.0);          
  144.                         tomato.setSafetyEnabled(true);
  145.                     }
  146.                     break;
  147.                    
  148.                 case midStartAlpha:
  149.                     //Put Left Start Code here
  150.                    
  151.                     if (timeElapsed <2) {
  152.                         tomato.arcadeDrive(-0.6, 0.5);
  153.                     }
  154.                    
  155.                     else if (timeElapsed <5){
  156.                         tomato.arcadeDrive(0.0, 0.0);
  157.                         tomato.setSafetyEnabled(true);
  158.                     }
  159.                     break;
  160.                    
  161.                 case rightStartAlpha:
  162.                     //Put Right Start Code here
  163.                     if (timeElapsed<2){
  164.                         tomato.arcadeDrive(-0.5, 0.5);
  165.                     }
  166.                     else if (timeElapsed<5){
  167.                         tomato.arcadeDrive(0.5, 0.5);
  168.                     }
  169.                     else if (timeElapsed<6){
  170.                         tomato.arcadeDrive(0.0, 0.0);
  171.                         tomato.setSafetyEnabled(true);
  172.                     }
  173.                     break;
  174.                
  175.                 case leftStartBeta:
  176.                     if (timeElapsed<2){
  177.                         tomato.arcadeDrive(-0.5, 0.5);
  178.                     }
  179.                     else if (timeElapsed<5){
  180.                         tomato.arcadeDrive(0.5, 0.5);
  181.                     }
  182.                     else if (timeElapsed<6){
  183.                         tomato.arcadeDrive(0.0, 0.0);
  184.                         tomato.setSafetyEnabled(true);
  185.                     }
  186.                     break;
  187.                    
  188.                 case midStartBeta:
  189.                     if (timeElapsed<2){
  190.                         tomato.arcadeDrive(-0.5, 0.5);
  191.                     }
  192.                     else if (timeElapsed<5){
  193.                         tomato.arcadeDrive(0.5, 0.5);
  194.                     }
  195.                     else if (timeElapsed<6){
  196.                         tomato.arcadeDrive(0.0, 0.0);
  197.                         tomato.setSafetyEnabled(true);
  198.                     }
  199.                     break;
  200.                    
  201.                 case rightStartBeta:
  202.                     if (timeElapsed<2){
  203.                         tomato.arcadeDrive(-0.5, 0.5);
  204.                     }
  205.                     else if (timeElapsed<5){
  206.                         tomato.arcadeDrive(0.5, 0.5);
  207.                     }
  208.                     else if (timeElapsed<6){
  209.                         tomato.arcadeDrive(0.0, 0.0);
  210.                         tomato.setSafetyEnabled(true);
  211.                     }
  212.                     break;
  213.                    
  214.                 case shtf:
  215.                     if (timeElapsed<2){
  216.                         tomato.arcadeDrive(-0.2, 0);
  217.                     }
  218.                     else if (timeElapsed<3){
  219.                         tomato.arcadeDrive(0.0, 0.0);
  220.                     }
  221.               }
  222.              
  223.           } else {
  224.             //Put right auto code here
  225.               switch (autoSelected) {    
  226.                
  227.                 case leftStartAlpha:
  228.                     // Put Mid Start code here
  229.                     if (timeElapsed <2){
  230.                         tomato.arcadeDrive(-0.6, 0.0);
  231.                     }
  232.                    
  233.                     else if (timeElapsed <5){
  234.                         tomato.arcadeDrive(0.6, 0.0);
  235.                     }
  236.                    
  237.                     else if (timeElapsed < 6) {
  238.                         tomato.arcadeDrive(0.2, 0.0);
  239.                     }
  240.                     else if (timeElapsed>=15){
  241.                         tomato.arcadeDrive(0.0, 0.0);          
  242.                         tomato.setSafetyEnabled(true);
  243.                     }
  244.                     break;
  245.                    
  246.                 case midStartAlpha:
  247.                     //Put Left Start Code here
  248.                    
  249.                     if (timeElapsed <2) {
  250.                         tomato.arcadeDrive(-0.6, 0.5);
  251.                     }
  252.                    
  253.                     else if (timeElapsed <5){
  254.                         tomato.arcadeDrive(0.0, 0.0);
  255.                         tomato.setSafetyEnabled(true);
  256.                     }
  257.                     break;
  258.                    
  259.                 case rightStartAlpha:
  260.                     //Put Right Start Code here
  261.                     if (timeElapsed<2){
  262.                         tomato.arcadeDrive(-0.5, 0.5);
  263.                     }
  264.                     else if (timeElapsed<5){
  265.                         tomato.arcadeDrive(0.5, 0.5);
  266.                     }
  267.                     else if (timeElapsed<6){
  268.                         tomato.arcadeDrive(0.0, 0.0);
  269.                         tomato.setSafetyEnabled(true);
  270.                     }
  271.                     break;
  272.                
  273.                 case leftStartBeta:
  274.                     if (timeElapsed<2){
  275.                         tomato.arcadeDrive(-0.5, 0.5);
  276.                     }
  277.                     else if (timeElapsed<5){
  278.                         tomato.arcadeDrive(0.5, 0.5);
  279.                     }
  280.                     else if (timeElapsed<6){
  281.                         tomato.arcadeDrive(0.0, 0.0);
  282.                         tomato.setSafetyEnabled(true);
  283.                     }
  284.                     break;
  285.                    
  286.                 case midStartBeta:
  287.                     if (timeElapsed<2){
  288.                         tomato.arcadeDrive(-0.5, 0.5);
  289.                     }
  290.                     else if (timeElapsed<5){
  291.                         tomato.arcadeDrive(0.5, 0.5);
  292.                     }
  293.                     else if (timeElapsed<6){
  294.                         tomato.arcadeDrive(0.0, 0.0);
  295.                         tomato.setSafetyEnabled(true);
  296.                     }
  297.                     break;
  298.                    
  299.                 case rightStartBeta:
  300.                     if (timeElapsed<2){
  301.                         tomato.arcadeDrive(-0.5, 0.5);
  302.                     }
  303.                     else if (timeElapsed<5){
  304.                         tomato.arcadeDrive(0.5, 0.5);
  305.                     }
  306.                     else if (timeElapsed<6){
  307.                         tomato.arcadeDrive(0.0, 0.0);
  308.                         tomato.setSafetyEnabled(true);
  309.                     }
  310.                     break;
  311.                    
  312.                 case shtf:
  313.                     if (timeElapsed<2){
  314.                         tomato.arcadeDrive(-0.2, 0);
  315.                     }
  316.                     else if (timeElapsed<3){
  317.                         tomato.arcadeDrive(0.0, 0.0);
  318.                     }
  319.               }
  320.           }
  321.        
  322.              
  323.              
  324.              
  325.              
  326.           } else {
  327.             //Put right auto code here
  328.               switch (autoSelected) {    
  329.                
  330.                 case leftStartAlpha:
  331.                     // Put Mid Start code here
  332.                     if (timeElapsed <2){
  333.                         tomato.arcadeDrive(-0.6, 0.0);
  334.                     }
  335.                    
  336.                     else if (timeElapsed <5){
  337.                         tomato.arcadeDrive(0.6, 0.0);
  338.                     }
  339.                    
  340.                     else if (timeElapsed < 6) {
  341.                         tomato.arcadeDrive(0.2, 0.0);
  342.                     }
  343.                     else if (timeElapsed>=15){
  344.                         tomato.arcadeDrive(0.0, 0.0);          
  345.                         tomato.setSafetyEnabled(true);
  346.                     }
  347.                     break;
  348.                    
  349.                 case midStartAlpha:
  350.                     //Put Left Start Code here
  351.                    
  352.                     if (timeElapsed <2) {
  353.                         tomato.arcadeDrive(-0.6, 0.5);
  354.                     }
  355.                    
  356.                     else if (timeElapsed <5){
  357.                         tomato.arcadeDrive(0.0, 0.0);
  358.                         tomato.setSafetyEnabled(true);
  359.                     }
  360.                     break;
  361.                    
  362.                 case rightStartAlpha:
  363.                     //Put Right Start Code here
  364.                     if (timeElapsed<2){
  365.                         tomato.arcadeDrive(-0.5, 0.5);
  366.                     }
  367.                     else if (timeElapsed<5){
  368.                         tomato.arcadeDrive(0.5, 0.5);
  369.                     }
  370.                     else if (timeElapsed<6){
  371.                         tomato.arcadeDrive(0.0, 0.0);
  372.                         tomato.setSafetyEnabled(true);
  373.                     }
  374.                     break;
  375.                
  376.                 case leftStartBeta:
  377.                     if (timeElapsed<2){
  378.                         tomato.arcadeDrive(-0.5, 0.5);
  379.                     }
  380.                     else if (timeElapsed<5){
  381.                         tomato.arcadeDrive(0.5, 0.5);
  382.                     }
  383.                     else if (timeElapsed<6){
  384.                         tomato.arcadeDrive(0.0, 0.0);
  385.                         tomato.setSafetyEnabled(true);
  386.                     }
  387.                     break;
  388.                    
  389.                 case midStartBeta:
  390.                     if (timeElapsed<2){
  391.                         tomato.arcadeDrive(-0.5, 0.5);
  392.                     }
  393.                     else if (timeElapsed<5){
  394.                         tomato.arcadeDrive(0.5, 0.5);
  395.                     }
  396.                     else if (timeElapsed<6){
  397.                         tomato.arcadeDrive(0.0, 0.0);
  398.                         tomato.setSafetyEnabled(true);
  399.                     }
  400.                     break;
  401.                    
  402.                 case rightStartBeta:
  403.                     if (timeElapsed<2){
  404.                         tomato.arcadeDrive(-0.5, 0.5);
  405.                     }
  406.                     else if (timeElapsed<5){
  407.                         tomato.arcadeDrive(0.5, 0.5);
  408.                     }
  409.                     else if (timeElapsed<6){
  410.                         tomato.arcadeDrive(0.0, 0.0);
  411.                         tomato.setSafetyEnabled(true);
  412.                     }
  413.                     break;
  414.                    
  415.                 case shtf:
  416.                     if (timeElapsed<2){
  417.                         tomato.arcadeDrive(-0.2, 0);
  418.                     }
  419.                     else if (timeElapsed<3){
  420.                         tomato.arcadeDrive(0.0, 0.0);
  421.                     }
  422.               }
  423.              
  424.              
  425.              
  426.              
  427.              
  428.           }
  429.            
  430.    
  431.     }
  432.                
  433.        
  434.    
  435.  
  436.  
  437.     @Override
  438.     public void teleopPeriodic() {
  439.        
  440.         //Drivetrain
  441.        
  442.         //double driveSensitivity;
  443.         //driveSensitivity = Math.abs(logitechController.getRawAxis(3)); //Positive is 0 and Negative is 1
  444.        
  445.         boolean driveReverse;
  446.         driveReverse = false;
  447.        
  448.         boolean deployToggle1;
  449.         deployToggle1 = false;
  450.        
  451.            
  452.         tomato.arcadeDrive(-logitechController.getRawAxis(1) * 0.92, logitechController.getRawAxis(0) * 0.92);
  453.        
  454.             if (logitechController.getRawButton(2) && driveReverse == false)
  455.             {
  456.                 //drive normal
  457.                 tomato.arcadeDrive(-logitechController.getRawAxis(1) * 0.92, logitechController.getRawAxis(0) * 0.92);
  458.                 deployToggle1 = true;
  459.             }
  460.             else if (!logitechController.getRawButton(2) && driveReverse == false && deployToggle1 == true)
  461.             {
  462.                 driveReverse = true;
  463.             } else if (logitechController.getRawButton(2) && driveReverse == true)
  464.             {
  465.                 //drive in reverse
  466.                 tomato.arcadeDrive(logitechController.getRawAxis(7) * 0.92, -logitechController.getRawAxis(0) * 0.92);
  467.                 deployToggle1 = false;
  468.             }
  469.             else if (!logitechController.getRawButton(2) && driveArcade == true && deployToggle1 == false)
  470.             {
  471.                 driveReverse = false;
  472.             }
  473.            
  474.          //Mechanisms
  475.          
  476.          elevator.set((xboxController.getRawAxis(1) != 0 ? 0.8 : xboxController.getRawAxis(1) != 0 ? -0.5 : xboxController.getRawAxis(1) != 0 ? -0.5: xboxController.getRawAxis(1) != 0 ? 0.8 : 0));
  477.        
  478.          //elevator.set((xboxController.getRawButton(5) ? 0.8 : xboxController.getRawButton(5) ? -0.5 : xboxController.getRawButton(6) ? -0.5: xboxController.getRawButton(6) ? 0.8 : 0));
  479.            
  480.          cubeLeft.set((xboxController.getRawButton(5) ? -0.7 : xboxController.getRawButton(6) ? 0.7 : xboxController.getRawButton(5) ? 0.7 : xboxController.getRawButton(5) ? 0.7 : 0));
  481.          cubeRight.set((xboxController.getRawButton(5) ? 0.7 : xboxController.getRawButton(6) ? -0.7 : xboxController.getRawButton(6) ? -0.7 : xboxController.getRawButton(6) ? -0.7 : 0));
  482.          
  483.          //Limit Switch
  484.          
  485.         if (climberLimitSwitch.get() == true) {
  486.             climber.set((xboxController.getRawButton(2) ? 0.5 : xboxController.getRawButton(2) ? -0.9 : xboxController.getRawButton(4) ? -0.9 : xboxController.getRawButton(4) ? 0.5 : 0));
  487.         } else {
  488.               climber.set(0.95);
  489.             }
  490.        
  491.     /*
  492.      *
  493.      * CODE FOR USING ONLY ONE CONTROLLER
  494.         elevator.set((logitechController.getRawButton(7) ? 0.5 : logitechController.getRawButton(7) ? -0.5 : logitechController.getRawButton(8) ? -0.5: logitechController.getRawButton(8) ? 0.5 : 0));
  495.        
  496.          cubeLeft.set((logitechController.getRawButton(1) ? -0.7 : logitechController.getRawButton(2) ? 0.7 : logitechController.getRawButton(1) ? 0.7 : logitechController.getRawButton(1) ? 0.7 : 0));
  497.          cubeRight.set((logitechController.getRawButton(1) ? 0.7 : logitechController.getRawButton(2) ? -0.7 : logitechController.getRawButton(2) ? -0.7 : logitechController.getRawButton(2) ? -0.7 : 0));
  498.          
  499.         if (climbLimitSwitch.get() == true) {
  500.             climb.set((logitechController.getRawButton(3) ? 0.9 : logitechController.getRawButton(3) ? -0.9 : logitechController.getRawButton(4) ? -0.9 : logitechController.getRawButton(4) ? 0.9 : 0));
  501.         } else {
  502.               climb.set(-0.95);
  503.             }
  504.     */
  505.         }
  506.    
  507.  
  508.    
  509.    
  510.        
  511.    
  512.     @Override
  513.     public void testPeriodic() {
  514.     }
  515. }
  516.  
  517. /*XBOXCONTROLLER MAPPING:
  518.  * a = button1
  519.  * b = button2
  520.  * x = button3
  521.  * y = button4
  522.  * LB = Button5
  523.  * RB = Button6
  524.  * leftJoystickUp&Down = Axis1
  525.  * rightJoystickUp&Down = Axis5
  526.  * leftJoystickLeft&Right = Axis 0
  527.  * rightJoystickLeft&Right = Axis 4
  528.  * LT = Axis2
  529.  * RT = Axis3
  530.  * back = Button7
  531.  * start Button8
  532.  * leftJoystickPushDown = Button 9
  533.  * rightJoystickPushDown = Button 10
  534.  */
  535.  
  536.  
  537.  
  538. /* Questions for Davinci
  539.  * How to control elevator and climb with axis, not buttons
  540.  */
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!