|
1
|
|
|
|
|
2
|
|
|
package org.usfirst.frc.team3695.robot;
|
|
3
|
|
|
|
|
4
|
|
|
import edu.wpi.first.wpilibj.DriverStation;
|
|
5
|
|
|
import edu.wpi.first.wpilibj.IterativeRobot;
|
|
6
|
|
|
import edu.wpi.first.wpilibj.command.Scheduler;
|
|
7
|
|
|
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
|
|
8
|
|
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
|
9
|
|
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
10
|
|
|
import org.usfirst.frc.team3695.robot.auto.CommandGroupAuto;
|
|
11
|
|
|
import org.usfirst.frc.team3695.robot.enumeration.Bot;
|
|
12
|
|
|
import org.usfirst.frc.team3695.robot.enumeration.Drivetrain;
|
|
13
|
|
|
import org.usfirst.frc.team3695.robot.enumeration.Goal;
|
|
14
|
|
|
import org.usfirst.frc.team3695.robot.enumeration.Position;
|
|
15
|
|
|
import org.usfirst.frc.team3695.robot.subsystems.*;
|
|
16
|
|
|
import org.usfirst.frc.team3695.robot.util.Util;
|
|
17
|
|
|
|
|
18
|
|
|
// _____ _____ ____ ______ ______ _ ____ _
|
|
19
|
|
|
// |__ / / ___/ / __ \ / ____/ / ____/ ____ _ __ (_) ____ ___ __ __ _____ / __ \ _____ (_) ____ ___ ___
|
|
20
|
|
|
// /_ < / __ \ / /_/ / /___ \ / /_ / __ \ | |/_/ / / / __ `__ \ / / / / / ___/ / /_/ / / ___/ / / / __ `__ \ / _ \
|
|
21
|
|
|
// ___/ / / /_/ / \__, / ____/ / / __/ / /_/ / _> < / / / / / / / // /_/ / (__ ) / ____/ / / / / / / / / / // __/
|
|
22
|
|
|
// /____/ \____/ /____/ /_____/ /_/ \____/ /_/|_| /_/ /_/ /_/ /_/ \__,_/ /____/ /_/ /_/ /_/ /_/ /_/ /_/ \___/
|
|
23
|
|
|
|
|
24
|
|
|
/** the magic place where everything happens (where the sequence of events is controlled, top of the hierarchy) */
|
|
25
|
|
|
public class Robot extends IterativeRobot {
|
|
26
|
|
|
|
|
27
|
|
|
public static Bot bot;
|
|
|
|
|
|
|
28
|
|
|
|
|
29
|
|
|
/// choosers
|
|
30
|
|
|
SendableChooser<Bot> botChooser;
|
|
31
|
|
|
SendableChooser<Goal> goalChooser;
|
|
32
|
|
|
SendableChooser<Drivetrain> driveChooser;
|
|
33
|
|
|
SendableChooser<Position> positionChooser;
|
|
34
|
|
|
// add choosers as needed, these put drop down options in the smart dash
|
|
35
|
|
|
|
|
36
|
|
|
|
|
37
|
|
|
/// subsystems
|
|
38
|
|
|
// public static SubsystemArduino SUB_ARDUINO;
|
|
39
|
|
|
public static SubsystemClamp SUB_CLAMP;
|
|
|
|
|
|
|
40
|
|
|
public static SubsystemCompressor SUB_COMPRESSOR;
|
|
|
|
|
|
|
41
|
|
|
public static SubsystemDrive SUB_DRIVE;
|
|
|
|
|
|
|
42
|
|
|
public static SubsystemHook SUB_HOOK;
|
|
|
|
|
|
|
43
|
|
|
public static SubsystemManipulator SUB_MANIPULATOR;
|
|
|
|
|
|
|
44
|
|
|
public static SubsystemMast SUB_MAST;
|
|
|
|
|
|
|
45
|
|
|
|
|
46
|
|
|
public static OI oi;
|
|
|
|
|
|
|
47
|
|
|
public static Vision vision;
|
|
|
|
|
|
|
48
|
|
|
|
|
49
|
|
|
|
|
50
|
|
|
/// autonomous
|
|
51
|
|
|
private CommandGroupAuto auto;
|
|
52
|
|
|
|
|
53
|
|
|
/** runs when robot is turned on */
|
|
54
|
|
|
public void robotInit() {
|
|
55
|
|
|
/// instantiate bot chooser
|
|
56
|
|
|
botChooser = new SendableChooser<>();
|
|
57
|
|
|
botChooser.addDefault(Bot.TEUFELSKIND.toString(), Bot.TEUFELSKIND);
|
|
58
|
|
|
botChooser.addObject(Bot.OOF.toString(), Bot.OOF);
|
|
59
|
|
|
SmartDashboard.putData("Bot", botChooser);
|
|
60
|
|
|
|
|
61
|
|
|
if (botChooser.getSelected() != null){
|
|
62
|
|
|
bot = botChooser.getSelected();
|
|
|
|
|
|
|
63
|
|
|
} else {
|
|
64
|
|
|
bot = Bot.OOF;
|
|
|
|
|
|
|
65
|
|
|
}
|
|
66
|
|
|
|
|
67
|
|
|
DriverStation.reportWarning("ROBOT STARTED; GOOD LUCK", false);
|
|
68
|
|
|
/// instantiate subsystems
|
|
69
|
|
|
// SUB_ARDUINO = new SubsystemArduino();
|
|
70
|
|
|
SUB_MANIPULATOR = new SubsystemManipulator();
|
|
|
|
|
|
|
71
|
|
|
SUB_CLAMP = new SubsystemClamp();
|
|
|
|
|
|
|
72
|
|
|
SUB_COMPRESSOR = new SubsystemCompressor();
|
|
|
|
|
|
|
73
|
|
|
SUB_DRIVE = new SubsystemDrive();
|
|
|
|
|
|
|
74
|
|
|
|
|
75
|
|
|
SUB_DRIVE.pid.setPIDF(.5, 0, 0, 0);
|
|
|
|
|
|
|
76
|
|
|
|
|
77
|
|
|
SUB_HOOK = new SubsystemHook();
|
|
|
|
|
|
|
78
|
|
|
SUB_MAST = new SubsystemMast();
|
|
|
|
|
|
|
79
|
|
|
vision = new Vision();
|
|
|
|
|
|
|
80
|
|
|
|
|
81
|
|
|
/// instantiate operator interface
|
|
82
|
|
|
oi = new OI();
|
|
|
|
|
|
|
83
|
|
|
|
|
84
|
|
|
|
|
85
|
|
|
|
|
86
|
|
|
/// instantiate drivetrain chooser
|
|
87
|
|
|
driveChooser = new SendableChooser<>();
|
|
88
|
|
|
driveChooser.addDefault(Drivetrain.ROCKET_LEAGUE.toString(), Drivetrain.ROCKET_LEAGUE); // set default to RL drive
|
|
89
|
|
|
for(int i = 1; i < Drivetrain.values().length; i++) {
|
|
90
|
|
|
driveChooser.addObject(Drivetrain.values()[i].toString(), Drivetrain.values()[i]); } // add each drivetrain enum value to chooser
|
|
91
|
|
|
SmartDashboard.putData("Drivetrain", driveChooser); //display the chooser on the dash
|
|
92
|
|
|
|
|
93
|
|
|
/// instantiate position chooser
|
|
94
|
|
|
positionChooser = new SendableChooser<>();
|
|
95
|
|
|
positionChooser.addDefault(Position.CENTER.toString(), Position.CENTER); // set default to center
|
|
96
|
|
|
for(int i = 1; i < Position.values().length; i++) {
|
|
97
|
|
|
positionChooser.addObject(Position.values()[i].toString(), Position.values()[i]); } // add each position enum value to chooser
|
|
98
|
|
|
SmartDashboard.putData("Position", positionChooser); //display the chooser on the dash
|
|
99
|
|
|
|
|
100
|
|
|
/// instantiate goal chooser
|
|
101
|
|
|
goalChooser = new SendableChooser<>();
|
|
102
|
|
|
goalChooser.addDefault(Goal.NOTHING.toString(), Goal.NOTHING); // set default to nothing
|
|
103
|
|
|
for(int i = 1; i < Goal.values().length; i++) {
|
|
104
|
|
|
goalChooser.addObject(Goal.values()[i].toString(), Goal.values()[i]); } // add each autonomous goal to chooser
|
|
105
|
|
|
SmartDashboard.putData("Goal", goalChooser); //display the chooser on the dash
|
|
106
|
|
|
|
|
107
|
|
|
/// instantiate bot chooser
|
|
108
|
|
|
botChooser = new SendableChooser<>();
|
|
109
|
|
|
botChooser.addDefault(Bot.TEUFELSKIND.toString(), Bot.TEUFELSKIND);
|
|
110
|
|
|
botChooser.addObject(Bot.OOF.toString(), Bot.OOF);
|
|
111
|
|
|
SmartDashboard.putData("Bot", botChooser);
|
|
112
|
|
|
|
|
113
|
|
|
|
|
114
|
|
|
|
|
115
|
|
|
/// instantiate cameras
|
|
116
|
|
|
vision.startScrewCameraThread();
|
|
117
|
|
|
vision.startFrameCameraThread();
|
|
118
|
|
|
|
|
119
|
|
|
SmartDashboard.putData("Sub_Drive", SUB_DRIVE);
|
|
120
|
|
|
DriverStation.reportWarning("SUBSYSTEMS, CHOOSERS INSTANTIATED", false);
|
|
121
|
|
|
}
|
|
122
|
|
|
|
|
123
|
|
|
|
|
124
|
|
|
/** runs when robot gets disabled */
|
|
125
|
|
|
public void disabledInit() {
|
|
126
|
|
|
DriverStation.reportWarning("TELEOP IS DISABLED", false);
|
|
127
|
|
|
Scheduler.getInstance().removeAll();
|
|
128
|
|
|
}
|
|
129
|
|
|
|
|
130
|
|
|
|
|
131
|
|
|
/** runs at 50hz when bot is disabled */
|
|
132
|
|
|
public void disabledPeriodic() {
|
|
133
|
|
|
Scheduler.getInstance().run();
|
|
134
|
|
|
}
|
|
135
|
|
|
|
|
136
|
|
|
|
|
137
|
|
|
/** runs when autonomous start */
|
|
138
|
|
|
public void autonomousInit() {
|
|
139
|
|
|
DriverStation.reportWarning("AUTONOMOUS IS STARTING...", false);
|
|
140
|
|
|
if(goalChooser.getSelected() != null) {
|
|
141
|
|
|
auto = new CommandGroupAuto(positionChooser.getSelected(), goalChooser.getSelected());
|
|
142
|
|
|
auto.start();
|
|
143
|
|
|
}
|
|
144
|
|
|
}
|
|
145
|
|
|
|
|
146
|
|
|
/** runs at 50hz when in autonomous */
|
|
147
|
|
|
public void autonomousPeriodic() {
|
|
148
|
|
|
Scheduler.getInstance().run();
|
|
149
|
|
|
bot = (botChooser.getSelected() != null) ? botChooser.getSelected() : bot; // update motor inverts
|
|
|
|
|
|
|
150
|
|
|
}
|
|
151
|
|
|
|
|
152
|
|
|
|
|
153
|
|
|
/** runs when teleop starts*/
|
|
154
|
|
|
public void teleopInit() {
|
|
155
|
|
|
DriverStation.reportWarning("TELEOP IS ENABLED", false);
|
|
156
|
|
|
if (auto != null)
|
|
157
|
|
|
auto.cancel();
|
|
158
|
|
|
}
|
|
159
|
|
|
|
|
160
|
|
|
|
|
161
|
|
|
/** runs at ~50hz when in teleop mode */
|
|
162
|
|
|
public void teleopPeriodic() {
|
|
163
|
|
|
Scheduler.getInstance().run();
|
|
164
|
|
|
|
|
165
|
|
|
if (driveChooser.getSelected() != null) {
|
|
166
|
|
|
SUB_DRIVE.setDrivetrain(driveChooser.getSelected());
|
|
167
|
|
|
}
|
|
168
|
|
|
|
|
169
|
|
|
bot = (botChooser.getSelected() != null) ? botChooser.getSelected() : bot; // update motor inverts
|
|
|
|
|
|
|
170
|
|
|
}
|
|
171
|
|
|
|
|
172
|
|
|
|
|
173
|
|
|
/** runs at ~50hz when in test mode */
|
|
174
|
|
|
public void testPeriodic() {
|
|
175
|
|
|
LiveWindow.run();
|
|
176
|
|
|
}
|
|
177
|
|
|
}
|
|
178
|
|
|
|
|
179
|
|
|
|
wh1ter0se /
PowerUp-2018
Loading history...
See this CWE advisory on why this is a security issue.