1 package org
.usfirst
.frc
.team3501
.robot
;
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
;
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
;
14 public class Robot
extends IterativeRobot
{
15 private static DriveTrain driveTrain
;
16 private static Shooter shooter
;
18 private static Intake intake
;
19 private static CameraServer server
;
22 public void robotInit() {
23 driveTrain
= DriveTrain
.getDriveTrain();
25 shooter
= Shooter
.getShooter();
26 intake
= Intake
.getIntake();
28 server
= CameraServer
.getInstance();
29 UsbCamera climberCam
= server
.startAutomaticCapture("climbercam", 0);
30 UsbCamera intakeCam
= server
.startAutomaticCapture("intakecam", 1);
32 driveTrain
.setCANTalonsBrakeMode(driveTrain
.DRIVE_COAST_MODE
);
35 public static DriveTrain
getDriveTrain() {
37 return DriveTrain
.getDriveTrain();
40 public static Shooter
getShooter() {
41 return Shooter
.getShooter();
44 public static OI
getOI() {
48 public static Intake
getIntake() {
49 return Intake
.getIntake();
52 public static void swapCameraFeed() {
53 if (Intake
.getIntake().getIntakeCameraFeed() == 1) {
54 UsbCamera climberCam
= server
.startAutomaticCapture("climbercam", 1);
55 UsbCamera intakeCam
= server
.startAutomaticCapture("intakecam", 0);
56 Intake
.getIntake().setIntakeCameraFeed(0);
58 UsbCamera climberCam
= server
.startAutomaticCapture("climbercam", 0);
59 UsbCamera intakeCam
= server
.startAutomaticCapture("intakecam", 1);
60 Intake
.getIntake().setIntakeCameraFeed(1);
65 // If the gear values do not match in the left and right piston, then they are
66 // both set to high gear
68 public void autonomousInit() {
69 driveTrain
.setHighGear();
70 driveTrain
.setCANTalonsBrakeMode(driveTrain
.DRIVE_COAST_MODE
);
74 public void autonomousPeriodic() {
75 Scheduler
.getInstance().run();
79 public void teleopInit() {
80 driveTrain
.setCANTalonsBrakeMode(driveTrain
.DRIVE_COAST_MODE
);
84 public void teleopPeriodic() {
85 // driveTrain.printEncoderOutput();
86 Scheduler
.getInstance().run();
87 updateSmartDashboard();
91 public void disabledInit() {
92 driveTrain
.setCANTalonsBrakeMode(driveTrain
.DRIVE_BRAKE_MODE
);
96 // public void disabledPeriodic() {
97 // Scheduler.getInstance().add(new RunFlyWheel(2));
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());