1 package org
.usfirst
.frc
.team3501
.robot
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
.Defense
;
4 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DefenseArm
;
5 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
6 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.IntakeArm
;
7 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Scaler
;
8 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Shooter
;
10 import edu
.wpi
.first
.wpilibj
.IterativeRobot
;
11 import edu
.wpi
.first
.wpilibj
.command
.Scheduler
;
12 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SendableChooser
;
13 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SmartDashboard
;
15 public class Robot
extends IterativeRobot
{
17 public static DriveTrain driveTrain
;
18 public static Shooter shooter
;
20 public static Scaler scaler
;
22 public static IntakeArm intakeArm
;
23 public static DefenseArm defenseArm
;
25 // Sendable Choosers send a drop down menu to the Smart Dashboard.
26 SendableChooser positionChooser
;
27 SendableChooser positionOneDefense
, positionTwoDefense
, positionThreeDefense
,
28 positionFourDefense
, positionFiveDefense
;
31 public void robotInit() {
32 driveTrain
= new DriveTrain();
35 shooter
= new Shooter();
36 scaler
= new Scaler();
37 intakeArm
= new IntakeArm();
39 // initializeSendableChoosers();
40 // addPositionChooserOptions();
41 // addDefensesToAllDefenseSendableChoosers();
42 // sendSendableChoosersToSmartDashboard();
46 private void initializeSendableChoosers() {
47 positionChooser
= new SendableChooser();
48 positionOneDefense
= new SendableChooser();
49 positionTwoDefense
= new SendableChooser();
50 positionThreeDefense
= new SendableChooser();
51 positionFourDefense
= new SendableChooser();
52 positionFiveDefense
= new SendableChooser();
55 private void addPositionChooserOptions() {
56 positionChooser
.addDefault("Position 1", 1);
57 positionChooser
.addObject("Position 2", 2);
58 positionChooser
.addObject("Position 3", 3);
59 positionChooser
.addObject("Position 4", 4);
60 positionChooser
.addObject("Position 5", 5);
63 private void addDefensesToAllDefenseSendableChoosers() {
64 addDefenseOptions(positionOneDefense
);
65 addDefenseOptions(positionTwoDefense
);
66 addDefenseOptions(positionThreeDefense
);
67 addDefenseOptions(positionFourDefense
);
68 addDefenseOptions(positionFiveDefense
);
71 private void addDefenseOptions(SendableChooser chooser
) {
72 chooser
.addDefault("Portcullis", Defense
.PORTCULLIS
);
73 chooser
.addObject("Sally Port", Defense
.SALLY_PORT
);
74 chooser
.addObject("Rough Terrain", Defense
.ROUGH_TERRAIN
);
75 chooser
.addObject("Low Bar", Defense
.LOW_BAR
);
76 chooser
.addObject("Cheval De Frise", Defense
.CHEVAL_DE_FRISE
);
77 chooser
.addObject("Drawbridge", Defense
.DRAWBRIDGE
);
78 chooser
.addObject("Moat", Defense
.MOAT
);
79 chooser
.addObject("Rock Wall", Defense
.ROCK_WALL
);
82 private void sendSendableChoosersToSmartDashboard() {
83 SmartDashboard
.putData("PositionChooser", positionChooser
);
84 SmartDashboard
.putData("Position One Defense Chooser", positionOneDefense
);
85 SmartDashboard
.putData("Position Two Defense Chooser", positionTwoDefense
);
86 SmartDashboard
.putData("Position Three Defense Chooser",
87 positionThreeDefense
);
89 .putData("Position Four Defense Chooser", positionFourDefense
);
91 .putData("Position Five Defense Chooser", positionFiveDefense
);
94 .putData("Position Four Defense Chooser", positionFourDefense
);
96 .putData("Position Five Defense Chooser", positionFiveDefense
);
101 public void autonomousInit() {
102 Scheduler
.getInstance().run();
104 // // get options chosen from drop down menu
105 // Integer chosenPosition = (Integer) positionChooser.getSelected();
106 // Integer chosenDefense = 0;
108 // if (chosenPosition == 1)
109 // chosenDefense = (Integer) positionOneDefense.getSelected();
110 // else if (chosenPosition == 2)
111 // chosenDefense = (Integer) positionTwoDefense.getSelected();
112 // else if (chosenPosition == 3)
113 // chosenDefense = (Integer) positionThreeDefense.getSelected();
114 // else if (chosenPosition == 4)
115 // chosenDefense = (Integer) positionFourDefense.getSelected();
116 // else if (chosenPosition == 5)
117 // chosenDefense = (Integer) positionFiveDefense.getSelected();
119 // System.out.println("Chosen Position: " + chosenPosition);
120 // System.out.println("Chosen Defense: " + chosenDefense);
124 public void autonomousPeriodic() {
125 Scheduler
.getInstance().run();
129 public void teleopInit() {
130 Robot
.driveTrain
.setLowGear();
131 Robot
.shooter
.raiseHood();
135 public void teleopPeriodic() {
136 Scheduler
.getInstance().run();
137 if (OI
.rightJoystick
.getTrigger()) {
138 Robot
.shooter
.setSpeed(.8);
140 Robot
.shooter
.setSpeed(0);
142 if (OI
.rightJoystick
.getRawButton(2)) {
143 Robot
.shooter
.extendPunch();
145 Robot
.shooter
.retractPunch();