daily pastebin goal
60%
SHARE
TWEET

Untitled

a guest Aug 17th, 2018 62 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*----------------------------------------------------------------------------*/
  2. /* Copyright (c) FIRST 2008. All Rights Reserved.                             */
  3. /* Open Source Software - may be modified and shared by FRC teams. The code   */
  4. /* must be accompanied by the FIRST BSD license file in the root directory of */
  5. /* the project.                                                               */
  6. /*----------------------------------------------------------------------------*/
  7.  
  8. package com.Tyr;
  9. import edu.wpi.first.wpilibj.*;
  10. import edu.wpi.first.wpilibj.camera.AxisCamera;
  11.  
  12. public class Robot1777 extends SimpleRobot {
  13.  
  14.     private RobotDrive drive14;     // Controls Wheel Motors 1 and 4.
  15.     private RobotDrive drive32;     // Controls Wheel Motors 2 and 3.
  16.     private Joystick joystick1;     // All-in-one Joystick.
  17.     private Joystick joystick2;     // Joystick used for the Robot Functions.
  18.     private Relay kicker;           // Kicker.
  19.     private Relay recock;           // Recock.
  20.     private AxisCamera cam;         // Camera.
  21.     private Watchdog ralph;         // Watchdog.
  22.     private Compressor comp;            // Compressor.
  23.     private RobotDrive magnet;      // Ball Magnet.
  24.     private DriverStationLCD uM;        // User Messages (Display Messages on Driver Station.
  25.  
  26.     // UM1 = Mode           // UM2 = Drive          // UM3 = Kicker
  27.     // UM4 = None           // UM5 = Compressor     // UM6 = System Alive?
  28.  
  29.     /*
  30.      *  Joystick1 --->  Button 1 = NONE;
  31.      *              Button 2 = Magnet Drive;    <<--------|
  32.      *              Button 3 = NONE;                |
  33.      *              Button 4 = NONE;                |
  34.      *              Button 5 = Used with Magnet -------|
  35.      *              Button 6 = Gyroscope;
  36.      *              Button 7 = Camera;
  37.      *              Button 8 = System Safety;
  38.      */
  39.  
  40.     public Robot1777() {
  41.  
  42.         drive14 = new RobotDrive(1,4);
  43.         drive32 = new RobotDrive(3,2);
  44.         magnet = new RobotDrive(5,8);
  45.         joystick1 = new Joystick(1);
  46.         joystick2 = new Joystick(2);
  47.         comp = new Compressor(1,1);
  48.         kicker = new Relay(2, Relay.Direction.kBoth);
  49.         recock = new Relay(3, Relay.Direction.kBoth);
  50.         uM = DriverStationLCD.getInstance();
  51.         ralph = Watchdog.getInstance();
  52.         cam = AxisCamera.getInstance();
  53.         cam.writeBrightness(50);
  54.         cam.writeResolution(AxisCamera.ResolutionT.k320x240);
  55.         cam.writeWhiteBalance(AxisCamera.WhiteBalanceT.automatic);
  56.         cam.writeExposureControl(AxisCamera.ExposureT.automatic);
  57.         cam.writeExposurePriority(AxisCamera.ExposurePriorityT.frameRate);
  58.     }
  59.  
  60.     /////////////////////////////////////////////////////////////////
  61.     // Main Robot (BRAIN) CODE
  62.     /////////////////////////////////////////////////////////////////
  63.     public void robotMain() {
  64.  
  65.         System.out.print("\n\n*** Code Deployed, Robot Ready! ***\n\n");
  66.         writeUM(1, "Robot Ready!");
  67.         boolean enabledOnce = false;
  68.         ralph.setExpiration(0.2);
  69.  
  70.         while(isEnabled() || isDisabled()) {
  71.             ralph.feed();
  72.             if(isAutonomous() && isEnabled()) {
  73.                 enabledOnce = true;
  74.                 autonomous();
  75.             }
  76.             else if(isOperatorControl() && isEnabled()) {
  77.                 enabledOnce = true;
  78.                 operatorControl();
  79.             }
  80.             else if(isDisabled() && enabledOnce) {
  81.                 disabled();
  82.             }
  83.         }
  84.     }
  85.  
  86.  
  87.     /////////////////////////////////////////////////////////////////
  88.     // Autonomous CODE
  89.     /////////////////////////////////////////////////////////////////
  90.     public void autonomous() {
  91.  
  92.         System.out.println("Entered Autonomous");
  93.         writeUM(1, "Autonomous Mode");
  94.         writeUM(3, "Kicker Ready");
  95.         magnet.tankDrive(1.0, 0.0);
  96.         ralph.setEnabled(true);
  97.         comp.start();
  98.         int reps = 0;
  99.         int kloops = 0;
  100.         int dloops = 0;
  101.         double p = 0.0;
  102.         boolean tru = true;
  103.         boolean dkick = false;
  104.         boolean kick1 = false;
  105.         boolean kick2 = true;   // Recocks at the begining
  106.         boolean kick3 = false;
  107.         boolean kickerReady = false;
  108.  
  109.         while(isAutonomous() && isEnabled()) {
  110.  
  111.             ralph.feed();
  112.  
  113.             /////////////////////////////////////////////
  114.             // Compressor Code
  115.             /////////////////////////////////////////////
  116.             if(!comp.getPressureSwitchValue()) {
  117.                 writeUM(5, "Compressor Enabled");
  118.                 comp.setRelayValue(Relay.Value.kForward);
  119.             }
  120.             else if(comp.getPressureSwitchValue()) {
  121.                 writeUM(5, "Compressor Disabled");
  122.                 comp.setRelayValue(Relay.Value.kOff);
  123.             }
  124.  
  125.             /////////////////////////////////////////////
  126.             // Kicker Code
  127.             /////////////////////////////////////////////
  128.             if(kick1 && kloops == 100) {
  129.                 kicker.set(Relay.Value.kOff);
  130.                 kick1 = false;
  131.                 kick2 = true;
  132.                 kloops = 0;
  133.             }
  134.             else if(kick2 && kloops == 100) {
  135.                 writeUM(3, "Reloading Kicker");
  136.                 recock.set(Relay.Value.kForward);
  137.                 kick2 = false;
  138.                 kick3 = true;
  139.                 kloops = 0;
  140.             }
  141.             else if(kick3 && kloops == 100) {
  142.                 writeUM(3, "Kicker Ready");
  143.                 recock.set(Relay.Value.kOff);
  144.                 kick3 = false;
  145.                 kickerReady = true;
  146.                 kloops = 0;
  147.             }
  148.             if(kickerReady && dkick) {
  149.  
  150.                 writeUM(3, "Kick!");
  151.                 kicker.set(Relay.Value.kForward);
  152.                 kickerReady = false;
  153.                 kick1 = true;
  154.                 kloops = 0;
  155.             }
  156.  
  157.             /////////////////////////////////////////////
  158.             // Drive and loops Code
  159.             /////////////////////////////////////////////
  160.             if(dloops == 200) {
  161.                 writeUM(2, "Moving Forward");
  162.                 drive14.tankDrive(0.7, 0.7);
  163.                 drive32.tankDrive(0.7, 0.7);
  164.             }
  165.             else if(dloops == 400) {
  166.                 writeUM(2, "");
  167.                 drive14.tankDrive(0.0, 0.0);
  168.                 drive32.tankDrive(0.0, 0.0);
  169.             }
  170.             else if(dloops == 500) { dkick = true; }
  171.             else if(dloops == 700) { dkick = false; }
  172.             else if(dloops == 800) {    dloops = 0; reps++; }
  173.             if(reps == 1) { dloops = 2000; }
  174.  
  175.             kloops++;
  176.             dloops++;
  177.             Timer.delay(0.005);
  178.         }
  179.     }
  180.  
  181.  
  182.     /////////////////////////////////////////////////////////////////
  183.     // Operator Control CODE
  184.     /////////////////////////////////////////////////////////////////
  185.     public void operatorControl() {
  186.  
  187.         System.out.println("Entered Operator Control");
  188.         writeUM(1, "Tele-OP Mode");
  189.         writeUM(3, "Kicker Ready");
  190.         ralph.setEnabled(true);
  191.         magnet.drive(0.0, 0.0);
  192.         comp.start();
  193.         int kloops = 0;
  194.         double p = 0.0;
  195.         boolean tru = true;
  196.         boolean kickerReady = false;
  197.         boolean kick1 = false;
  198.         boolean kick2 = true;
  199.         boolean kick3 = false;
  200.         boolean mag = false;
  201.         boolean firstTime = true;
  202.  
  203.         while(isOperatorControl() && isEnabled()) {
  204.  
  205.             ralph.feed();
  206.  
  207.             /////////////////////////////////////////////
  208.             // Camera CODE
  209.             /////////////////////////////////////////////
  210.             if(joystick1.getRawButton(7) == true || firstTime) {
  211.                 System.out.println("Button - 7");
  212.                 cam.writeWhiteBalance(AxisCamera.WhiteBalanceT.automatic);
  213.                 cam.writeExposureControl(AxisCamera.ExposureT.automatic);
  214.                 cam.writeExposurePriority(AxisCamera.ExposurePriorityT.frameRate);
  215.             }
  216.  
  217.             /////////////////////////////////////////////
  218.             // Watchdog and System (SAFETY) CODE
  219.             /////////////////////////////////////////////
  220. /*          if(ds.getBatteryVoltage() <= 9.0)
  221.                 writeUM(6, "Battery LOW!");
  222.  
  223.             if(!ralph.isSystemActive())
  224.                 ralph.kill();
  225.  
  226.             if(!ralph.isAlive()) {
  227.                 ralph.feed();
  228.                 ralph.setEnabled(true);
  229.                 ralph.feed();
  230.             }
  231.  
  232.             if(joystick1.getRawButton(8) == true || joystick4.getRawButton(8) == true) {
  233.                 System.out.println("Button - 8");
  234.                 while(tru)
  235.                     ralph.feed();
  236.                 if(ralph.isAlive() && ralph.isSystemActive()) {
  237.                     writeUM(6, "Ralph is Alive");
  238.                     Timer.delay(1);
  239.                     writeUM(6, "");
  240.                 }
  241.                 else if(!ralph.isSystemActive()) {
  242.                     writeUM(6, "System is not Active!");
  243.                     Timer.delay(1);
  244.                     writeUM(6, "");
  245.                 }
  246.                 else if(!ralph.isAlive()) {
  247.                     writeUM(6, "Ralph Died! REBOOT cRIO!");
  248.                     Timer.delay(1);
  249.                     writeUM(6, "");
  250.                 }
  251.                 else {
  252.                     writeUM(6, "Something is Wrong!");
  253.                     Timer.delay(1);
  254.                     writeUM(6, "");
  255.                 }
  256.             }
  257.             else {
  258.                 while(tru)
  259.                     ralph.feed();
  260.             }
  261.  
  262.  */
  263.  
  264.             /////////////////////////////////////////////
  265.             // Compressor Code
  266.             /////////////////////////////////////////////
  267.             if(!comp.getPressureSwitchValue()) {
  268.                 writeUM(5, "Compressor Enabled");
  269.                 comp.setRelayValue(Relay.Value.kForward);
  270.             }
  271.             else if(comp.getPressureSwitchValue()) {
  272.                 writeUM(5, "Compressor Disabled");
  273.                 comp.setRelayValue(Relay.Value.kOff);
  274.             }
  275.  
  276.             /////////////////////////////////////////////
  277.             // Kicker Code
  278.             /////////////////////////////////////////////
  279.             if(kick1 && kloops == 100) {
  280.                 kicker.set(Relay.Value.kOff);
  281.                 kick1 = false;
  282.                 kick2 = true;
  283.                 kloops = 0;
  284.             }
  285.             else if(kick2 && kloops == 100) {
  286.                 writeUM(3, "Reloading Kicker");
  287.                 recock.set(Relay.Value.kForward);
  288.                 kick2 = false;
  289.                 kick3 = true;
  290.                 kloops = 0;
  291.             }
  292.             else if(kick3 && kloops == 100) {
  293.                 writeUM(3, "Kicker Ready");
  294.                 recock.set(Relay.Value.kOff);
  295.                 kick3 = false;
  296.                 kickerReady = true;
  297.                 kloops = 0;
  298.             }
  299.  
  300.             /////////////////////////////////////////////
  301.             // Trigger (Kick) CODE
  302.             /////////////////////////////////////////////
  303.             if((joystick1.getTrigger() == true || joystick2.getTrigger() == true) && kickerReady) {
  304.  
  305.                 writeUM(3, "Kick!");
  306.                 System.out.println("Trigger");
  307.                 kicker.set(Relay.Value.kForward);
  308.                 kickerReady = false;
  309.                 kick1 = true;
  310.                 kloops = 0;
  311.             }
  312.  
  313.             /////////////////////////////////////////////
  314.             // Magnet Drive CODE
  315.             /////////////////////////////////////////////
  316.             if(joystick1.getRawButton(2) || joystick2.getRawButton(2)) {                // Magnet Motor ON or OFF
  317.  
  318.                 System.out.println("Button - 2");
  319.                 if(mag) {
  320.                     magnet.tankDrive(1.0, 0.0);
  321.                     mag = false;
  322.                 }
  323.                 else {
  324.                     magnet.tankDrive(0.0, 0.0);
  325.                     mag = true;
  326.                 }
  327.             }
  328.             if(joystick1.getRawButton(5))
  329.                 magnet.tankDrive(1.0, 0.0);
  330.             if(joystick1.getRawButton(3))
  331.                 magnet.tankDrive(0.0, 0.0);
  332.  
  333.             ////////////////////////////////////
  334.             // Drive Code
  335.             ////////////////////////////////////
  336.             if(joystick1.getY() <= -0.2 || joystick1.getY() >= 0.2) {               // Robot Moving Forwards and Backwards
  337.  
  338.                 p = joystick1.getY();
  339.                 if(p > 0)
  340.                     writeUM(2, "Moving Backwards");
  341.                 else
  342.                     writeUM(2, "Moving Forwards");
  343.                 drive14.tankDrive(-p, -p);
  344.                 drive32.tankDrive(-p, -p);
  345.             }
  346.  
  347.             else if(joystick1.getX() <= -0.2 || joystick1.getX() >= 0.2) {              // Robot moving Left and right
  348.  
  349.                 p = joystick1.getX();
  350.                 if(p > 0)
  351.                     writeUM(2, "Moving Right");
  352.                 else
  353.                     writeUM(2, "Moving Left");
  354.                 drive14.tankDrive(p, p);
  355.                 drive32.tankDrive(-p, -p);
  356.             }
  357.  
  358.             else if(joystick1.getTwist() >= 0.3 || joystick1.getTwist() <= -0.3) {      // Robot moving CW and CCW
  359.  
  360.                 p = joystick1.getTwist();
  361.                 if(p > 0)
  362.                     writeUM(2, "Rotating Right CW");
  363.                 else
  364.                     writeUM(2, "Rotating Left CCW");
  365.                 drive14.tankDrive(p, -p);
  366.                 drive32.tankDrive(p, -p);
  367.             }
  368.  
  369.             ////////////////////////////////////
  370.             // Tank Drive Code
  371.             ////////////////////////////////////
  372. /*          else if(joystick3.getY() <= -0.2 || joystick3.getY() >= 0.2) {              // Left Motors Forwards and Backwards
  373.  
  374.                 p = joystick3.getY();
  375.                 if(p > 0) {
  376.                     uM.println(DriverStationLCD.Line.kUser2, 23, "L");
  377.                     uM.updateLCD();
  378.                 }
  379.                 else {
  380.                     uM.println(DriverStationLCD.Line.kUser2, 23, "L");
  381.                     uM.updateLCD();
  382.                 }
  383.                 drive14.tankDrive(-p, 0.0);
  384.                 drive32.tankDrive(-p, 0.0);
  385.             }
  386.  
  387.             else if(joystick4.getY() <= -0.2 || joystick4.getX() >= 0.2) {              // Right Motors Forward and Backwards
  388.  
  389.                 p = joystick4.getY();
  390.                 if(p > 0)
  391.                     writeUM(2, "TankDrive Backward - R");
  392.                 else
  393.                     writeUM(2, "TankDrive Forward - R");
  394.                 drive14.tankDrive(0.0, -p);
  395.                 drive32.tankDrive(0.0, -p);
  396.             }
  397.             else if(joystick4.getX() <= -0.2 || joystick4.getX() >= 0.2) {              // Robot Moving Left and Right
  398.  
  399.                 p = joystick4.getX();
  400.                 if(p > 0)
  401.                     writeUM(2, "Moving Right");
  402.                 else
  403.                     writeUM(2, "Moving Left");
  404.                 drive14.tankDrive(p, p);
  405.                 drive32.tankDrive(-p, -p);
  406.             }
  407.  
  408.             else if(joystick4.getTwist() >= 0.3 || joystick4.getTwist() <= -0.3) {      // Robot Moving CW and CCW
  409.  
  410.                 p = joystick4.getTwist();
  411.                 if(p > 0)
  412.                     writeUM(2, "Rotating Right CW");
  413.                 else
  414.                     writeUM(2, "Rotating Left CCW");
  415.                 drive14.tankDrive(p, -p);
  416.                 drive32.tankDrive(p, -p);
  417.             }
  418. */
  419.  
  420.             else {                                                          // Robot Not Moving
  421.                 writeUM(2, "");
  422.                 drive14.tankDrive(0.0, 0.0);
  423.                 drive32.tankDrive(0.0, 0.0);
  424.             }
  425.  
  426.             kloops++;
  427.             firstTime = false;
  428.             Timer.delay(0.005);
  429.         }
  430.     }
  431.  
  432. /*  public void drive1432(int d1, int d4, int d3, int d2) {
  433.  
  434.         drive14.tankDrive(d1, d4);
  435.         drive32.tankDrive(d3, d2);
  436.  
  437.         if(d1 == 0.0 && d4 == 0.0 && d3 == 0.0 && d2 == 0.0)
  438.             writeUM(2, "");
  439.  
  440.         if(d1 < 0 && d4 < 0 && d3 < 0 && d2 < 0)
  441.             writeUM(2, "Moving Forwards");
  442.  
  443.         if(d1 > 0 && d4 > 0 && d3 > 0 && d2 > 0)
  444.             writeUM(2, "Moving Backwards");
  445.  
  446.         if(d1 > 0 && d4 > 0 && d3 < 0 && d2 < 0)
  447.             writeUM(2, "Moving Right");
  448.  
  449.         if(d1 < 0 && d4 < 0 && d3 > 0 && d2 > 0)
  450.             writeUM(2, "Moving Left");
  451.  
  452.         if(d1 < 0 && d4 > 0 && d3 < 0 && d2 > 0)
  453.             writeUM(2, "Rotating Left CCW");
  454.  
  455.         if(d1 > 0 && d4 < 0 && d3 > 0 && d2 < 0)
  456.             writeUM(2, "Rotating Right CW");
  457.  
  458.     }
  459. */
  460.     /////////////////////////////////////////////////////////////////
  461.     // DISABLED MODE
  462.     /////////////////////////////////////////////////////////////////
  463.     public void disabled() {
  464.  
  465.         while(isDisabled())
  466.             ralph.feed();
  467.  
  468.         comp.stop();
  469.         magnet.tankDrive(0.0, 0.0);
  470.         System.out.println("Disabled Mode");
  471.         writeUM(1, "Disabled Mode");
  472.         writeUM(3, "Kicker Disabled");
  473.     }
  474.  
  475.     /////////////////////////////////////////////////////////////////
  476.     // User Messages
  477.     /////////////////////////////////////////////////////////////////
  478.     public void writeUM(int line, String Message) {
  479.  
  480.         if(line == 1) {
  481.             uM.println(DriverStationLCD.Line.kMain6, 1, "                         ");
  482.             uM.println(DriverStationLCD.Line.kMain6, 1, Message);
  483.             uM.updateLCD();
  484.         }
  485.         else if(line == 2) {
  486.             uM.println(DriverStationLCD.Line.kUser2, 1, "                         ");
  487.             uM.println(DriverStationLCD.Line.kUser2, 1, Message);
  488.             uM.updateLCD();
  489.         }
  490.         else if(line == 3) {
  491.             uM.println(DriverStationLCD.Line.kUser3, 1, "                         ");
  492.             uM.println(DriverStationLCD.Line.kUser3, 1, Message);
  493.             uM.updateLCD();
  494.         }
  495.         else if(line == 4) {
  496.             uM.println(DriverStationLCD.Line.kUser4, 1, "                         ");
  497.             uM.println(DriverStationLCD.Line.kUser4, 1, Message);
  498.             uM.updateLCD();
  499.         }
  500.         else if(line == 5) {
  501.             uM.println(DriverStationLCD.Line.kUser5, 1, "                         ");
  502.             uM.println(DriverStationLCD.Line.kUser5, 1, Message);
  503.             uM.updateLCD();
  504.         }
  505.         else if(line == 6) {
  506.             uM.println(DriverStationLCD.Line.kUser6, 1, "                         ");
  507.             uM.println(DriverStationLCD.Line.kUser6, 1, Message);
  508.             uM.updateLCD();
  509.         }
  510.     }
  511.  
  512. }
RAW Paste Data
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
 
Top