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
.DriveDistance
;
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
.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
;
21 public static Scaler scaler
;
23 public static IntakeArm intakeArm
;
24 public static DefenseArm defenseArm
;
26 // Sendable Choosers send a drop down menu to the Smart Dashboard.
27 SendableChooser positionChooser
;
28 SendableChooser positionOneDefense
, positionTwoDefense
, positionThreeDefense
,
29 positionFourDefense
, positionFiveDefense
;
32 public void robotInit() {
33 driveTrain
= new DriveTrain();
36 // shooter = new Shooter();
37 // scaler = new Scaler();
38 // intakeArm = new IntakeArm();
40 // initializeSendableChoosers();
41 // addPositionChooserOptions();
42 // addDefensesToAllDefenseSendableChoosers();
43 // sendSendableChoosersToSmartDashboard();
47 private void initializeSendableChoosers() {
48 positionChooser
= new SendableChooser();
49 positionOneDefense
= new SendableChooser();
50 positionTwoDefense
= new SendableChooser();
51 positionThreeDefense
= new SendableChooser();
52 positionFourDefense
= new SendableChooser();
53 positionFiveDefense
= new SendableChooser();
56 private void addPositionChooserOptions() {
57 positionChooser
.addDefault("Position 1", 1);
58 positionChooser
.addObject("Position 2", 2);
59 positionChooser
.addObject("Position 3", 3);
60 positionChooser
.addObject("Position 4", 4);
61 positionChooser
.addObject("Position 5", 5);
64 private void addDefensesToAllDefenseSendableChoosers() {
65 addDefenseOptions(positionOneDefense
);
66 addDefenseOptions(positionTwoDefense
);
67 addDefenseOptions(positionThreeDefense
);
68 addDefenseOptions(positionFourDefense
);
69 addDefenseOptions(positionFiveDefense
);
72 private void addDefenseOptions(SendableChooser chooser
) {
73 chooser
.addDefault("Portcullis", Defense
.PORTCULLIS
);
74 chooser
.addObject("Sally Port", Defense
.SALLY_PORT
);
75 chooser
.addObject("Rough Terrain", Defense
.ROUGH_TERRAIN
);
76 chooser
.addObject("Low Bar", Defense
.LOW_BAR
);
77 chooser
.addObject("Cheval De Frise", Defense
.CHEVAL_DE_FRISE
);
78 chooser
.addObject("Drawbridge", Defense
.DRAWBRIDGE
);
79 chooser
.addObject("Moat", Defense
.MOAT
);
80 chooser
.addObject("Rock Wall", Defense
.ROCK_WALL
);
83 private void sendSendableChoosersToSmartDashboard() {
84 SmartDashboard
.putData("PositionChooser", positionChooser
);
85 SmartDashboard
.putData("Position One Defense Chooser", positionOneDefense
);
86 SmartDashboard
.putData("Position Two Defense Chooser", positionTwoDefense
);
87 SmartDashboard
.putData("Position Three Defense Chooser",
88 positionThreeDefense
);
89 SmartDashboard
.putData("Position Four Defense Chooser",
91 SmartDashboard
.putData("Position Five Defense Chooser",
94 SmartDashboard
.putData("Position Four Defense Chooser",
96 SmartDashboard
.putData("Position Five Defense Chooser",
99 shooter
= new Shooter();
104 public void autonomousInit() {
105 Scheduler
.getInstance().run();
107 // // get options chosen from drop down menu
108 // Integer chosenPosition = (Integer) positionChooser.getSelected();
109 // Integer chosenDefense = 0;
111 // if (chosenPosition == 1)
112 // chosenDefense = (Integer) positionOneDefense.getSelected();
113 // else if (chosenPosition == 2)
114 // chosenDefense = (Integer) positionTwoDefense.getSelected();
115 // else if (chosenPosition == 3)
116 // chosenDefense = (Integer) positionThreeDefense.getSelected();
117 // else if (chosenPosition == 4)
118 // chosenDefense = (Integer) positionFourDefense.getSelected();
119 // else if (chosenPosition == 5)
120 // chosenDefense = (Integer) positionFiveDefense.getSelected();
122 // System.out.println("Chosen Position: " + chosenPosition);
123 // System.out.println("Chosen Defense: " + chosenDefense);
127 public void autonomousPeriodic() {
128 Scheduler
.getInstance().run();
132 public void teleopInit() {
133 Scheduler
.getInstance().add(new DriveDistance(24, 5));
134 // Scheduler.getInstance().add(new DriveForTime(2, 0.3));
135 // Scheduler.getInstance().add(new TurnForAngle(90, 5));
136 // Scheduler.getInstance().add(
137 // new TurnForTime(3, Constants.Direction.RIGHT, 0.3));
138 // Scheduler.getInstance().add(
139 // new TurnForTime(3, Constants.Direction.LEFT, 0.3));
140 // Scheduler.getInstance().add(new JoystickDrive());
141 // Scheduler.getInstance().add(new Turn180());
145 public void teleopPeriodic() {
146 Scheduler
.getInstance().run();