challenge-bot.com Git - 3501/2017steamworks/blob - src/org/usfirst/frc/team3501/robot/Robot.java

   1 package org.usfirst.frc.team3501.robot;
   2
   3 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
   4 import org.usfirst.frc.team3501.robot.subsystems.Intake;
   5 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
   6
   7 import edu.wpi.cscore.UsbCamera;
   8 import edu.wpi.first.wpilibj.CameraServer;
   9 import edu.wpi.first.wpilibj.DriverStation;
  10 import edu.wpi.first.wpilibj.IterativeRobot;
  11 import edu.wpi.first.wpilibj.command.Scheduler;
  12 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  13
  14 public class Robot extends IterativeRobot {
  15   private static DriveTrain driveTrain;
  16   private static Shooter shooter;
  17   private static OI oi;
  18   private static Intake intake;
  19   private static CameraServer server;
  20
  21   @Override
  22   public void robotInit() {
  23     driveTrain = DriveTrain.getDriveTrain();
  24     oi = OI.getOI();
  25     shooter = Shooter.getShooter();
  26     intake = Intake.getIntake();
  27
  28     server = CameraServer.getInstance();
  29     UsbCamera climberCam = server.startAutomaticCapture("climbercam", 0);
  30     UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 1);
  31
  32     driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
  33   }
  34
  35   public static DriveTrain getDriveTrain() {
  36
  37     return DriveTrain.getDriveTrain();
  38   }
  39
  40   public static Shooter getShooter() {
  41     return Shooter.getShooter();
  42   }
  43
  44   public static OI getOI() {
  45     return OI.getOI();
  46   }
  47
  48   public static Intake getIntake() {
  49     return Intake.getIntake();
  50   }
  51
  52   public static void swapCameraFeed() {
  53     if (Intake.getIntake().getIntakeCameraFeed() == 1) {
  54       UsbCamera climberCam = server.startAutomaticCapture("climbercam", 1);
  55       UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 0);
  56       Intake.getIntake().setIntakeCameraFeed(0);
  57     } else {
  58       UsbCamera climberCam = server.startAutomaticCapture("climbercam", 0);
  59       UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 1);
  60       Intake.getIntake().setIntakeCameraFeed(1);
  61     }
  62
  63   }
  64
  65   // If the gear values do not match in the left and right piston, then they are
  66   // both set to high gear
  67   @Override
  68   public void autonomousInit() {
  69     driveTrain.setHighGear();
  70     driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
  71   }
  72
  73   @Override
  74   public void autonomousPeriodic() {
  75     Scheduler.getInstance().run();
  76   }
  77
  78   @Override
  79   public void teleopInit() {
  80     driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
  81   }
  82
  83   @Override
  84   public void teleopPeriodic() {
  85     // driveTrain.printEncoderOutput();
  86     Scheduler.getInstance().run();
  87     updateSmartDashboard();
  88   }
  89
  90   @Override
  91   public void disabledInit() {
  92     driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_BRAKE_MODE);
  93   }
  94   //
  95   // @Override
  96   // public void disabledPeriodic() {
  97   // Scheduler.getInstance().add(new RunFlyWheel(2));
  98   // }
  99
 100   public void updateSmartDashboard() {
 101     SmartDashboard.putNumber("left encode ",
 102         driveTrain.getLeftEncoderDistance());
 103     SmartDashboard.putNumber("right encoder",
 104         driveTrain.getRightEncoderDistance());
 105     SmartDashboard.putNumber("angle", driveTrain.getAngle());
 106     SmartDashboard.putNumber("voltage",
 107         DriverStation.getInstance().getBatteryVoltage());
 108     SmartDashboard.putNumber("rpm", shooter.getShooterRPM());
 109     SmartDashboard.putNumber("target shooting",
 110         shooter.getTargetShootingSpeed());
 111   }
 112 }