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
;
21 public void robotInit() {
22 driveTrain
= DriveTrain
.getDriveTrain();
24 shooter
= Shooter
.getShooter();
25 intake
= Intake
.getIntake();
27 CameraServer server
= CameraServer
.getInstance();
28 UsbCamera climberCam
= server
.startAutomaticCapture("climbercam", 0);
29 UsbCamera intakeCam
= server
.startAutomaticCapture("intakecam", 1);
31 driveTrain
.setCANTalonsBrakeMode(driveTrain
.DRIVE_COAST_MODE
);
34 public static DriveTrain
getDriveTrain() {
35 return DriveTrain
.getDriveTrain();
38 public static Shooter
getShooter() {
39 return Shooter
.getShooter();
42 public static OI
getOI() {
46 public static Intake
getIntake() {
47 return Intake
.getIntake();
50 // If the gear values do not match in the left and right piston, then they are
51 // both set to high gear
53 public void autonomousInit() {
54 driveTrain
.setHighGear();
55 driveTrain
.setCANTalonsBrakeMode(driveTrain
.DRIVE_COAST_MODE
);
59 public void autonomousPeriodic() {
60 Scheduler
.getInstance().run();
64 public void teleopInit() {
65 driveTrain
.setCANTalonsBrakeMode(driveTrain
.DRIVE_COAST_MODE
);
69 public void teleopPeriodic() {
70 driveTrain
.printEncoderOutput();
71 Scheduler
.getInstance().run();
72 updateSmartDashboard();
76 public void disabledInit() {
77 driveTrain
.setCANTalonsBrakeMode(driveTrain
.DRIVE_BRAKE_MODE
);
81 // public void disabledPeriodic() {
82 // Scheduler.getInstance().add(new RunFlyWheel(2));
85 public void updateSmartDashboard() {
86 SmartDashboard
.putNumber("left encode ",
87 driveTrain
.getLeftEncoderDistance());
88 SmartDashboard
.putNumber("right encoder",
89 driveTrain
.getRightEncoderDistance());
90 SmartDashboard
.putNumber("angle", driveTrain
.getAngle());
91 SmartDashboard
.putNumber("voltage",
92 DriverStation
.getInstance().getBatteryVoltage());
93 SmartDashboard
.putNumber("rpm", shooter
.getShooterRPM());
94 SmartDashboard
.putNumber("target shooting",
95 shooter
.getTargetShootingSpeed());