1 package org
.usfirst
.frc
.team3501
.robot
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
.Defense
;
4 import org
.usfirst
.frc
.team3501
.robot
.commands
.shooter
.ResetCatapult
;
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
.I2C
;
12 import edu
.wpi
.first
.wpilibj
.I2C
.Port
;
13 import edu
.wpi
.first
.wpilibj
.IterativeRobot
;
14 import edu
.wpi
.first
.wpilibj
.Joystick
;
15 import edu
.wpi
.first
.wpilibj
.command
.Scheduler
;
16 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SendableChooser
;
17 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SmartDashboard
;
19 public class Robot
extends IterativeRobot
{
21 public static DriveTrain driveTrain
;
22 public static Shooter shooter
;
24 public static Scaler scaler
;
26 public static IntakeArm intakeArm
;
27 public static DefenseArm defenseArm
;
29 static I2C Wire
= new I2C(Port
.kOnboard
, 42);
30 Joystick joy
= new Joystick(1);
32 // Sendable Choosers send a drop down menu to the Smart Dashboard.
33 SendableChooser positionChooser
;
34 SendableChooser positionOneDefense
, positionTwoDefense
, positionThreeDefense
,
35 positionFourDefense
, positionFiveDefense
;
38 public void robotInit() {
39 driveTrain
= new DriveTrain();
42 shooter
= new Shooter();
43 scaler
= new Scaler();
44 intakeArm
= new IntakeArm();
46 initializeSendableChoosers();
47 addPositionChooserOptions();
48 addDefensesToAllDefenseSendableChoosers();
49 sendSendableChoosersToSmartDashboard();
53 private void initializeSendableChoosers() {
54 positionChooser
= new SendableChooser();
55 positionOneDefense
= new SendableChooser();
56 positionTwoDefense
= new SendableChooser();
57 positionThreeDefense
= new SendableChooser();
58 positionFourDefense
= new SendableChooser();
59 positionFiveDefense
= new SendableChooser();
62 private void addPositionChooserOptions() {
63 positionChooser
.addDefault("Position 1", 1);
64 positionChooser
.addObject("Position 2", 2);
65 positionChooser
.addObject("Position 3", 3);
66 positionChooser
.addObject("Position 4", 4);
67 positionChooser
.addObject("Position 5", 5);
70 private void addDefensesToAllDefenseSendableChoosers() {
71 addDefenseOptions(positionOneDefense
);
72 addDefenseOptions(positionTwoDefense
);
73 addDefenseOptions(positionThreeDefense
);
74 addDefenseOptions(positionFourDefense
);
75 addDefenseOptions(positionFiveDefense
);
78 private void addDefenseOptions(SendableChooser chooser
) {
79 chooser
.addDefault("Portcullis", Defense
.PORTCULLIS
);
80 chooser
.addObject("Sally Port", Defense
.SALLY_PORT
);
81 chooser
.addObject("Rough Terrain", Defense
.ROUGH_TERRAIN
);
82 chooser
.addObject("Low Bar", Defense
.LOW_BAR
);
83 chooser
.addObject("Cheval De Frise", Defense
.CHEVAL_DE_FRISE
);
84 chooser
.addObject("Drawbridge", Defense
.DRAWBRIDGE
);
85 chooser
.addObject("Moat", Defense
.MOAT
);
86 chooser
.addObject("Rock Wall", Defense
.ROCK_WALL
);
89 private void sendSendableChoosersToSmartDashboard() {
90 SmartDashboard
.putData("PositionChooser", positionChooser
);
91 SmartDashboard
.putData("Position One Defense Chooser", positionOneDefense
);
92 SmartDashboard
.putData("Position Two Defense Chooser", positionTwoDefense
);
93 SmartDashboard
.putData("Position Three Defense Chooser",
94 positionThreeDefense
);
95 SmartDashboard
.putData("Position Four Defense Chooser",
97 SmartDashboard
.putData("Position Five Defense Chooser",
100 SmartDashboard
.putData("Position Four Defense Chooser",
101 positionFourDefense
);
102 SmartDashboard
.putData("Position Five Defense Chooser",
103 positionFiveDefense
);
105 shooter
= new Shooter();
110 public void autonomousInit() {
111 Scheduler
.getInstance().run();
113 // get options chosen from drop down menu
114 Integer chosenPosition
= (Integer
) positionChooser
.getSelected();
115 Integer chosenDefense
= 0;
117 if (chosenPosition
== 1)
118 chosenDefense
= (Integer
) positionOneDefense
.getSelected();
119 else if (chosenPosition
== 2)
120 chosenDefense
= (Integer
) positionTwoDefense
.getSelected();
121 else if (chosenPosition
== 3)
122 chosenDefense
= (Integer
) positionThreeDefense
.getSelected();
123 else if (chosenPosition
== 4)
124 chosenDefense
= (Integer
) positionFourDefense
.getSelected();
125 else if (chosenPosition
== 5)
126 chosenDefense
= (Integer
) positionFiveDefense
.getSelected();
128 System
.out
.println("Chosen Position: " + chosenPosition
);
129 System
.out
.println("Chosen Defense: " + chosenDefense
);
133 public void autonomousPeriodic() {
134 Scheduler
.getInstance().run();
138 public void teleopInit() {
139 Scheduler
.getInstance().add(new ResetCatapult()); // Reset catapult at start
144 public void teleopPeriodic() {
145 Scheduler
.getInstance().run();
146 if (joy
.getRawButton(1)) { // Check if the button is pressed
147 String WriteString
= "RGB"; // String that is going to transfer; change to
148 // whatever the Arduino code has
149 char[] CharArray
= WriteString
.toCharArray(); // Changes data type
150 // (string) to char array
151 // Char array: array of chars
152 byte[] WriteData
= new byte[CharArray
.length
]; // Makes the char array
153 // able to transfer from
154 // roboRio to Arduino
155 for (int i
= 0; i
< CharArray
.length
; i
++) { // Repeats for each char in
157 WriteData
[i
] = (byte) CharArray
[i
]; // Sets the char to byte
159 Wire
.transaction(WriteData
, WriteData
.length
, null, 0); // Finally does