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
.commands
.driving
.TimeDrive
;
6 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Climber
;
7 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
8 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Intake
;
9 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Shooter
;
11 import edu
.wpi
.cscore
.UsbCamera
;
12 import edu
.wpi
.first
.wpilibj
.CameraServer
;
13 import edu
.wpi
.first
.wpilibj
.DriverStation
;
14 import edu
.wpi
.first
.wpilibj
.IterativeRobot
;
15 import edu
.wpi
.first
.wpilibj
.command
.Command
;
16 import edu
.wpi
.first
.wpilibj
.command
.Scheduler
;
17 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SendableChooser
;
18 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SmartDashboard
;
20 public class Robot
extends IterativeRobot
{
21 private static DriveTrain driveTrain
;
22 private static Shooter shooter
;
24 private static Intake intake
;
25 private static Climber climber
;
28 SendableChooser autonChooser
;
31 public void robotInit() {
32 driveTrain
= DriveTrain
.getDriveTrain();
34 shooter
= Shooter
.getShooter();
35 intake
= Intake
.getIntake();
36 climber
= Climber
.getClimber();
38 autonChooser
= new SendableChooser();
39 autonChooser
.addDefault("Middle Gear", new AutonMiddleGear());
40 autonChooser
.addObject("Red Boiler Gear",
41 new AutonSideGear("RED", "BOILER"));
42 autonChooser
.addObject("Red Retrieval Gear",
43 new AutonSideGear("RED", "RETRIEVAL"));
44 autonChooser
.addObject("Blue Boiler Gear",
45 new AutonSideGear("BLUE", "BOILER"));
46 autonChooser
.addObject("Blue Retrieval Gear",
47 new AutonSideGear("BLUE", "RETRIEVAL"));
48 SmartDashboard
.putData("Autonomous Chooser", autonChooser
);
50 CameraServer server
= CameraServer
.getInstance();
51 UsbCamera climberCam
= server
.startAutomaticCapture("climbercam", 0);
52 UsbCamera intakeCam
= server
.startAutomaticCapture("intakecam", 1);
55 public static DriveTrain
getDriveTrain() {
56 return DriveTrain
.getDriveTrain();
59 public static Shooter
getShooter() {
60 return Shooter
.getShooter();
63 public static OI
getOI() {
67 public static Intake
getIntake() {
68 return Intake
.getIntake();
71 public static Climber
getClimber() {
72 return Climber
.getClimber();
76 public void autonomousInit() {
77 driveTrain
.setLowGear();
79 // autonCommand = (Command) autonChooser.getSelected();
80 autonCommand
= new TimeDrive(2, 0.6);
81 Scheduler
.getInstance().add(autonCommand
);
85 public void autonomousPeriodic() {
86 Scheduler
.getInstance().run();
90 public void teleopInit() {
91 driveTrain
.setHighGear();
95 public void teleopPeriodic() {
96 Scheduler
.getInstance().run();
97 updateSmartDashboard();
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());