1 package org
.usfirst
.frc
.team3501
.robot
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
.Defense
;
4 import org
.usfirst
.frc
.team3501
.robot
.commands
.driving
.JoystickDrive
;
5 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DefenseArm
;
6 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
7 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.IntakeArm
;
8 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Scaler
;
9 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Shooter
;
11 import edu
.wpi
.first
.wpilibj
.Compressor
;
12 import edu
.wpi
.first
.wpilibj
.IterativeRobot
;
13 import edu
.wpi
.first
.wpilibj
.command
.Scheduler
;
14 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SendableChooser
;
15 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SmartDashboard
;
17 public class Robot
extends IterativeRobot
{
19 public static DriveTrain driveTrain
;
20 public static Shooter shooter
;
22 public static Scaler scaler
;
24 public static IntakeArm intakeArm
;
25 public static DefenseArm defenseArm
;
27 public static Compressor compressor
;
29 // Sendable Choosers send a drop down menu to the Smart Dashboard.
30 SendableChooser positionChooser
;
31 SendableChooser positionOneDefense
, positionTwoDefense
, positionThreeDefense
,
32 positionFourDefense
, positionFiveDefense
;
35 public void robotInit() {
36 driveTrain
= new DriveTrain();
39 // shooter = new Shooter();
40 // scaler = new Scaler();
41 // intakeArm = new IntakeArm();
43 // initializeSendableChoosers();
44 // addPositionChooserOptions();
45 // addDefensesToAllDefenseSendableChoosers();
46 // sendSendableChoosersToSmartDashboard();
47 compressor
= new Compressor();
51 private void initializeSendableChoosers() {
52 positionChooser
= new SendableChooser();
53 positionOneDefense
= new SendableChooser();
54 positionTwoDefense
= new SendableChooser();
55 positionThreeDefense
= new SendableChooser();
56 positionFourDefense
= new SendableChooser();
57 positionFiveDefense
= new SendableChooser();
60 private void addPositionChooserOptions() {
61 positionChooser
.addDefault("Position 1", 1);
62 positionChooser
.addObject("Position 2", 2);
63 positionChooser
.addObject("Position 3", 3);
64 positionChooser
.addObject("Position 4", 4);
65 positionChooser
.addObject("Position 5", 5);
68 private void addDefensesToAllDefenseSendableChoosers() {
69 addDefenseOptions(positionOneDefense
);
70 addDefenseOptions(positionTwoDefense
);
71 addDefenseOptions(positionThreeDefense
);
72 addDefenseOptions(positionFourDefense
);
73 addDefenseOptions(positionFiveDefense
);
76 private void addDefenseOptions(SendableChooser chooser
) {
77 chooser
.addDefault("Portcullis", Defense
.PORTCULLIS
);
78 chooser
.addObject("Sally Port", Defense
.SALLY_PORT
);
79 chooser
.addObject("Rough Terrain", Defense
.ROUGH_TERRAIN
);
80 chooser
.addObject("Low Bar", Defense
.LOW_BAR
);
81 chooser
.addObject("Cheval De Frise", Defense
.CHEVAL_DE_FRISE
);
82 chooser
.addObject("Drawbridge", Defense
.DRAWBRIDGE
);
83 chooser
.addObject("Moat", Defense
.MOAT
);
84 chooser
.addObject("Rock Wall", Defense
.ROCK_WALL
);
87 private void sendSendableChoosersToSmartDashboard() {
88 SmartDashboard
.putData("PositionChooser", positionChooser
);
89 SmartDashboard
.putData("Position One Defense Chooser", positionOneDefense
);
90 SmartDashboard
.putData("Position Two Defense Chooser", positionTwoDefense
);
91 SmartDashboard
.putData("Position Three Defense Chooser",
92 positionThreeDefense
);
93 SmartDashboard
.putData("Position Four Defense Chooser",
95 SmartDashboard
.putData("Position Five Defense Chooser",
98 SmartDashboard
.putData("Position Four Defense Chooser",
100 SmartDashboard
.putData("Position Five Defense Chooser",
101 positionFiveDefense
);
103 shooter
= new Shooter();
108 public void autonomousInit() {
109 Scheduler
.getInstance().run();
111 // Scheduler.getInstance().add(new DriveDistance(24, 5));
112 // Scheduler.getInstance().add(new DriveForTime(2, 0.3));
113 // Scheduler.getInstance().add(new TurnForAngle(90, 5));
114 // Scheduler.getInstance().add(
115 // new TurnForTime(3, Constants.Direction.RIGHT, 0.3));
116 // Scheduler.getInstance().add(
117 // new TurnForTime(3, Constants.Direction.LEFT, 0.3));
118 // Scheduler.getInstance().add(new Turn180());
122 public void autonomousPeriodic() {
123 Scheduler
.getInstance().run();
127 public void teleopInit() {
129 Scheduler
.getInstance().add(new JoystickDrive());
131 driveTrain
.leftGearPiston
.set(Constants
.DriveTrain
.HIGH_GEAR
);
132 driveTrain
.rightGearPiston
.set(Constants
.DriveTrain
.HIGH_GEAR
);
136 public void teleopPeriodic() {
137 Scheduler
.getInstance().run();