competition fixes
[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / Robot.java
1 package org.usfirst.frc.team3501.robot;
2
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;
9
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;
18
19 public class Robot extends IterativeRobot {
20 private static DriveTrain driveTrain;
21 private static Shooter shooter;
22 private static OI oi;
23 private static Intake intake;
24 private static Climber climber;
25
26 Command autonCommand;
27 SendableChooser autonChooser;
28
29 @Override
30 public void robotInit() {
31 driveTrain = DriveTrain.getDriveTrain();
32 oi = OI.getOI();
33 shooter = Shooter.getShooter();
34 intake = Intake.getIntake();
35 climber = Climber.getClimber();
36
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);
48
49 CameraServer server = CameraServer.getInstance();
50 UsbCamera climberCam = server.startAutomaticCapture("climbercam", 0);
51 UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 1);
52 }
53
54 public static DriveTrain getDriveTrain() {
55 return DriveTrain.getDriveTrain();
56 }
57
58 public static Shooter getShooter() {
59 return Shooter.getShooter();
60 }
61
62 public static OI getOI() {
63 return OI.getOI();
64 }
65
66 public static Intake getIntake() {
67 return Intake.getIntake();
68 }
69
70 public static Climber getClimber() {
71 return Climber.getClimber();
72 }
73
74 @Override
75 public void autonomousInit() {
76 driveTrain.setLowGear();
77
78 // autonCommand = (Command) autonChooser.getSelected();
79 autonCommand = new AutonMiddleGear();
80 Scheduler.getInstance().add(autonCommand);
81 }
82
83 @Override
84 public void autonomousPeriodic() {
85 Scheduler.getInstance().run();
86 }
87
88 @Override
89 public void teleopInit() {
90 driveTrain.setHighGear();
91 }
92
93 @Override
94 public void teleopPeriodic() {
95 Scheduler.getInstance().run();
96 updateSmartDashboard();
97 }
98
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());
110 }
111 }