1 package org
.usfirst
.frc
.team3501
.robot
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
.Defense
;
4 import org
.usfirst
.frc
.team3501
.robot
.commands
.auton
.ChooseStrategy
;
5 import org
.usfirst
.frc
.team3501
.robot
.commands
.driving
.SetLowGear
;
6 import org
.usfirst
.frc
.team3501
.robot
.commands
.intakearm
.Photogate
;
7 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
8 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.IntakeArm
;
9 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Shooter
;
11 import edu
.wpi
.first
.wpilibj
.IterativeRobot
;
12 import edu
.wpi
.first
.wpilibj
.command
.Scheduler
;
13 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SendableChooser
;
14 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SmartDashboard
;
16 public class Robot
extends IterativeRobot
{
18 public static DriveTrain driveTrain
;
19 public static Shooter shooter
;
20 public static IntakeArm intakeArm
;
21 public static Photogate photogate
;
22 public static CameraFeeds cameraFeeds
;
24 // Sendable Choosers send a drop down menu to the Smart Dashboard.
25 private static SendableChooser defenseChooser
;
26 private static SendableChooser frontChooser
;
27 boolean choseIntakeIsFront
;
30 public void robotInit() {
31 driveTrain
= new DriveTrain();
32 shooter
= new Shooter();
33 intakeArm
= new IntakeArm();
34 photogate
= new Photogate();
35 cameraFeeds
= new CameraFeeds();
39 defenseChooser
= new SendableChooser();
40 addDefenseOptions(defenseChooser
);
41 SmartDashboard
.putData("Defense Chooser", defenseChooser
);
43 frontChooser
= new SendableChooser();
44 addFrontChooserOptions();
45 SmartDashboard
.putData("Front Chooser", frontChooser
);
48 private void addFrontChooserOptions() {
49 frontChooser
.addDefault("Intake is Back", false);
50 frontChooser
.addObject("Intake is True", true);
53 private void addDefenseOptions(SendableChooser chooser
) {
54 chooser
.addDefault("Portcullis", Defense
.PORTCULLIS
);
55 chooser
.addObject("Sally Port", Defense
.SALLY_PORT
);
56 chooser
.addObject("Rough Terrain", Defense
.ROUGH_TERRAIN
);
57 chooser
.addObject("Low Bar", Defense
.LOW_BAR
);
58 chooser
.addObject("Chival De Frise", Defense
.CHIVAL_DE_FRISE
);
59 chooser
.addObject("Drawbridge", Defense
.DRAWBRIDGE
);
60 chooser
.addObject("Moat", Defense
.MOAT
);
61 chooser
.addObject("Rock Wall", Defense
.ROCK_WALL
);
62 chooser
.addObject("No Auton", Defense
.NONE
);
66 public void autonomousInit() {
67 Defense chosenDefense
= (Defense
) (defenseChooser
.getSelected());
68 choseIntakeIsFront
= (boolean) frontChooser
.getSelected();
70 // if the shooter is not the front (intake is front) - toggle the drivetrain
71 if (choseIntakeIsFront
)
72 driveTrain
.toggleFlipped();
74 Scheduler
.getInstance().add(new ChooseStrategy(chosenDefense
));
76 // Scheduler.getInstance().add(new TimeDrive(.6, 4));
81 public void autonomousPeriodic() {
82 Scheduler
.getInstance().run();
86 public void teleopInit() {
87 // if intake is front, switch back to shooter as front
88 // this is done on purpose!
89 if (choseIntakeIsFront
)
90 driveTrain
.toggleFlipped();
93 Scheduler
.getInstance().add(new SetLowGear());
97 public void teleopPeriodic() {
98 Scheduler
.getInstance().run();