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
.DriveTrain
;
6 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Intake
;
7 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Shooter
;
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
;
18 public class Robot
extends IterativeRobot
{
19 private static DriveTrain driveTrain
;
20 private static Shooter shooter
;
22 private static Intake intake
;
25 SendableChooser autonChooser
;
28 public void robotInit() {
29 driveTrain
= DriveTrain
.getDriveTrain();
31 shooter
= Shooter
.getShooter();
32 intake
= Intake
.getIntake();
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
);
46 CameraServer server
= CameraServer
.getInstance();
47 UsbCamera climberCam
= server
.startAutomaticCapture("climbercam", 0);
48 UsbCamera intakeCam
= server
.startAutomaticCapture("intakecam", 1);
50 driveTrain
.setCANTalonsBrakeMode(driveTrain
.DRIVE_COAST_MODE
);
53 public static DriveTrain
getDriveTrain() {
54 return DriveTrain
.getDriveTrain();
57 public static Shooter
getShooter() {
58 return Shooter
.getShooter();
61 public static OI
getOI() {
65 public static Intake
getIntake() {
66 return Intake
.getIntake();
70 public void autonomousInit() {
71 // driveTrain.setLowGear();
72 driveTrain
.setCANTalonsBrakeMode(driveTrain
.DRIVE_COAST_MODE
);
74 // autonCommand = (Command) autonChooser.getSelected();
75 // autonCommand = new TimeDrive(1.5, 0.6);
76 // Scheduler.getInstance().add(autonCommand);
80 public void autonomousPeriodic() {
81 Scheduler
.getInstance().run();
85 public void teleopInit() {
86 // driveTrain.setHighGear();
87 driveTrain
.setCANTalonsBrakeMode(driveTrain
.DRIVE_COAST_MODE
);
91 public void teleopPeriodic() {
92 Scheduler
.getInstance().run();
93 updateSmartDashboard();
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());