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.commandgroups.AutonMiddleGear;
   4 import org.usfirst.frc.team3501.robot.commandgroups.AutonSideGear;
   5 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
   6 import org.usfirst.frc.team3501.robot.subsystems.Intake;
   7 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
   8
   9 import edu.wpi.cscore.UsbCamera;
  10 import edu.wpi.first.wpilibj.CameraServer;
  11 import edu.wpi.first.wpilibj.DriverStation;
  12 import edu.wpi.first.wpilibj.IterativeRobot;
  13 import edu.wpi.first.wpilibj.command.Command;
  14 import edu.wpi.first.wpilibj.command.Scheduler;
  15 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
  16 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  17
  18 public class Robot extends IterativeRobot {
  19   private static DriveTrain driveTrain;
  20   private static Shooter shooter;
  21   private static OI oi;
  22   private static Intake intake;
  23
  24   Command autonCommand;
  25   SendableChooser autonChooser;
  26
  27   @Override
  28   public void robotInit() {
  29     driveTrain = DriveTrain.getDriveTrain();
  30     oi = OI.getOI();
  31     shooter = Shooter.getShooter();
  32     intake = Intake.getIntake();
  33
  34     autonChooser = new SendableChooser();
  35     autonChooser.addDefault("Middle Gear", new AutonMiddleGear());
  36     autonChooser.addObject("Red Boiler Gear",
  37         new AutonSideGear("RED", "BOILER"));
  38     autonChooser.addObject("Red Retrieval Gear",
  39         new AutonSideGear("RED", "RETRIEVAL"));
  40     autonChooser.addObject("Blue Boiler Gear",
  41         new AutonSideGear("BLUE", "BOILER"));
  42     autonChooser.addObject("Blue Retrieval Gear",
  43         new AutonSideGear("BLUE", "RETRIEVAL"));
  44     SmartDashboard.putData("Autonomous Chooser", autonChooser);
  45
  46     CameraServer server = CameraServer.getInstance();
  47     UsbCamera climberCam = server.startAutomaticCapture("climbercam", 0);
  48     UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 1);
  49
  50     driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
  51   }
  52
  53   public static DriveTrain getDriveTrain() {
  54     return DriveTrain.getDriveTrain();
  55   }
  56
  57   public static Shooter getShooter() {
  58     return Shooter.getShooter();
  59   }
  60
  61   public static OI getOI() {
  62     return OI.getOI();
  63   }
  64
  65   public static Intake getIntake() {
  66     return Intake.getIntake();
  67   }
  68
  69   @Override
  70   public void autonomousInit() {
  71     // driveTrain.setLowGear();
  72     driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
  73
  74     // autonCommand = (Command) autonChooser.getSelected();
  75     // autonCommand = new TimeDrive(1.5, 0.6);
  76     // Scheduler.getInstance().add(autonCommand);
  77   }
  78
  79   @Override
  80   public void autonomousPeriodic() {
  81     Scheduler.getInstance().run();
  82   }
  83
  84   @Override
  85   public void teleopInit() {
  86     // driveTrain.setHighGear();
  87     driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
  88   }
  89
  90   @Override
  91   public void teleopPeriodic() {
  92     Scheduler.getInstance().run();
  93     updateSmartDashboard();
  94   }
  95
  96   public void updateSmartDashboard() {
  97     SmartDashboard.putNumber("left encode ",
  98         driveTrain.getLeftEncoderDistance());
  99     SmartDashboard.putNumber("right encoder",
 100         driveTrain.getRightEncoderDistance());
 101     SmartDashboard.putNumber("angle", driveTrain.getAngle());
 102     SmartDashboard.putNumber("voltage",
 103         DriverStation.getInstance().getBatteryVoltage());
 104     SmartDashboard.putNumber("rpm", shooter.getShooterRPM());
 105     SmartDashboard.putNumber("target shooting",
 106         shooter.getTargetShootingSpeed());
 107   }
 108 }