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
  20   @Override
  21   public void robotInit() {
  22     driveTrain = DriveTrain.getDriveTrain();
  23     oi = OI.getOI();
  24     shooter = Shooter.getShooter();
  25     intake = Intake.getIntake();
  26
  27     CameraServer server = CameraServer.getInstance();
  28     UsbCamera climberCam = server.startAutomaticCapture("climbercam", 0);
  29     UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 1);
  30
  31     driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
  32   }
  33
  34   public static DriveTrain getDriveTrain() {
  35     return DriveTrain.getDriveTrain();
  36   }
  37
  38   public static Shooter getShooter() {
  39     return Shooter.getShooter();
  40   }
  41
  42   public static OI getOI() {
  43     return OI.getOI();
  44   }
  45
  46   public static Intake getIntake() {
  47     return Intake.getIntake();
  48   }
  49
  50   // If the gear values do not match in the left and right piston, then they are
  51   // both set to high gear
  52   @Override
  53   public void autonomousInit() {
  54     driveTrain.setHighGear();
  55     driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
  56   }
  57
  58   @Override
  59   public void autonomousPeriodic() {
  60     Scheduler.getInstance().run();
  61   }
  62
  63   @Override
  64   public void teleopInit() {
  65     driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
  66   }
  67
  68   @Override
  69   public void teleopPeriodic() {
  70     // driveTrain.printEncoderOutput();
  71     Scheduler.getInstance().run();
  72     updateSmartDashboard();
  73   }
  74
  75   @Override
  76   public void disabledInit() {
  77     driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_BRAKE_MODE);
  78   }
  79   //
  80   // @Override
  81   // public void disabledPeriodic() {
  82   // Scheduler.getInstance().add(new RunFlyWheel(2));
  83   // }
  84
  85   public void updateSmartDashboard() {
  86     SmartDashboard.putNumber("left encode ",
  87         driveTrain.getLeftEncoderDistance());
  88     SmartDashboard.putNumber("right encoder",
  89         driveTrain.getRightEncoderDistance());
  90     SmartDashboard.putNumber("angle", driveTrain.getAngle());
  91     SmartDashboard.putNumber("voltage",
  92         DriverStation.getInstance().getBatteryVoltage());
  93     SmartDashboard.putNumber("rpm", shooter.getShooterRPM());
  94     SmartDashboard.putNumber("target shooting",
  95         shooter.getTargetShootingSpeed());
  96   }
  97 }