1 package org
.usfirst
.frc
.team3501
.robot
;
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
.Climber
;
6 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
7 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Intake
;
8 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Shooter
;
10 import edu
.wpi
.cscore
.UsbCamera
;
11 import edu
.wpi
.first
.wpilibj
.CameraServer
;
12 import edu
.wpi
.first
.wpilibj
.DriverStation
;
13 import edu
.wpi
.first
.wpilibj
.IterativeRobot
;
14 import edu
.wpi
.first
.wpilibj
.command
.Command
;
15 import edu
.wpi
.first
.wpilibj
.command
.Scheduler
;
16 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SendableChooser
;
17 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SmartDashboard
;
19 public class Robot
extends IterativeRobot
{
20 private static DriveTrain driveTrain
;
21 private static Shooter shooter
;
23 private static Intake intake
;
24 private static Climber climber
;
27 SendableChooser autonChooser
;
30 public void robotInit() {
31 driveTrain
= DriveTrain
.getDriveTrain();
33 shooter
= Shooter
.getShooter();
34 intake
= Intake
.getIntake();
35 climber
= Climber
.getClimber();
37 autonChooser
= new SendableChooser();
38 autonChooser
.addDefault("Middle Gear", new AutonMiddleGear());
39 autonChooser
.addObject("Red Boiler Gear",
40 new AutonSideGear("RED", "BOILER"));
41 autonChooser
.addObject("Red Retrieval Gear",
42 new AutonSideGear("RED", "RETRIEVAL"));
43 autonChooser
.addObject("Blue Boiler Gear",
44 new AutonSideGear("BLUE", "BOILER"));
45 autonChooser
.addObject("Blue Retrieval Gear",
46 new AutonSideGear("BLUE", "RETRIEVAL"));
47 SmartDashboard
.putData("Autonomous Chooser", autonChooser
);
49 CameraServer server
= CameraServer
.getInstance();
50 UsbCamera climberCam
= server
.startAutomaticCapture("climbercam", 0);
51 UsbCamera intakeCam
= server
.startAutomaticCapture("intakecam", 1);
54 public static DriveTrain
getDriveTrain() {
55 return DriveTrain
.getDriveTrain();
58 public static Shooter
getShooter() {
59 return Shooter
.getShooter();
62 public static OI
getOI() {
66 public static Intake
getIntake() {
67 return Intake
.getIntake();
70 public static Climber
getClimber() {
71 return Climber
.getClimber();
75 public void autonomousInit() {
76 driveTrain
.setLowGear();
78 // autonCommand = (Command) autonChooser.getSelected();
79 autonCommand
= new AutonMiddleGear();
80 Scheduler
.getInstance().add(autonCommand
);
84 public void autonomousPeriodic() {
85 Scheduler
.getInstance().run();
89 public void teleopInit() {
90 driveTrain
.setHighGear();
94 public void teleopPeriodic() {
95 Scheduler
.getInstance().run();
96 updateSmartDashboard();
99 public void updateSmartDashboard() {
100 SmartDashboard
.putNumber("left encode ",
101 driveTrain
.getLeftEncoderDistance());
102 SmartDashboard
.putNumber("right encoder",
103 driveTrain
.getRightEncoderDistance());
104 SmartDashboard
.putNumber("angle", driveTrain
.getAngle());
105 SmartDashboard
.putNumber("voltage",
106 DriverStation
.getInstance().getBatteryVoltage());
107 SmartDashboard
.putNumber("rpm", shooter
.getShooterRPM());
108 SmartDashboard
.putNumber("target shooting",
109 shooter
.getTargetShootingSpeed());