-
Notifications
You must be signed in to change notification settings - Fork 1
/
RobotContainer.java
203 lines (174 loc) · 7.41 KB
/
RobotContainer.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
//import java.util.List;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.Joystick;
//import edu.wpi.first.math.controller.PIDController;
//import edu.wpi.first.math.controller.ProfiledPIDController;
//import edu.wpi.first.math.geometry.Pose2d;
//import edu.wpi.first.math.geometry.Rotation2d;
//import edu.wpi.first.math.geometry.Translation2d;
//import edu.wpi.first.math.trajectory.Trajectory;
//import edu.wpi.first.math.trajectory.TrajectoryConfig;
//import edu.wpi.first.math.trajectory.TrajectoryGenerator;
/*import edu.wpi.first.math.controller.PIDController;
* import edu.wpi.first.math.controller.ProfiledPIDController;
* import edu.wpi.first.math.geometry.Pose2d;
* import edu.wpi.first.math.geometry.Rotation2d;
* import edu.wpi.first.math.geometry.Translation2d;
* import edu.wpi.first.math.trajectory.Trajectory;
* import edu.wpi.first.math.trajectory.TrajectoryConfig;
* import edu.wpi.first.math.trajectory.TrajectoryGenerator;
*/
import edu.wpi.first.wpilibj.XboxController;
//import frc.robot.Constants.AutoConstants;
//import frc.robot.Constants.DriveConstants;
import frc.robot.Constants.OIConstants;
import frc.robot.commands.ArmAdjust;
import frc.robot.commands.ArmAdjustDown;
import frc.robot.commands.ArmDown;
import frc.robot.commands.ArmMidRow;
//import frc.robot.commands.ArmPick;
import frc.robot.commands.ArmTopRow;
//import frc.robot.commands.ArmTravel;
import frc.robot.commands.AutonomousC;
import frc.robot.commands.DropObject;
import frc.robot.commands.IntakeCone;
import frc.robot.commands.IntakeCube;
import frc.robot.commands.IntakeIdle;
import frc.robot.commands.IntakeStop;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.Intake;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.RunCommand;
//import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
/*
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot
* (including subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems
public final static DriveSubsystem m_robotDrive = new DriveSubsystem();
private final Intake intake;
private final Arm arm;
private final IntakeIdle intakeIdle;
private final IntakeCube intakeCube;
private final IntakeCone intakeCone;
private final IntakeStop intakeStop;
private final DropObject dropObject;
private final ArmDown armDown;
private final ArmMidRow armMidRow;
private final ArmTopRow armTopRow;
private final AutonomousC autonomousC;
// private final ArmTravel armTravel;
// private final ArmPick armPick;
private final ArmAdjust armAdjust;
private final ArmAdjustDown armAdjustDown;
// The driver's controller
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
XboxController m_operatorController = new XboxController(OIConstants.kOperatorControllerPort);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
CameraServer.startAutomaticCapture(0);
intake = new Intake();
arm = new Arm();
// armTravel = new ArmTravel(arm);
// armTravel.addRequirements(arm);
intakeIdle = new IntakeIdle(intake);
intakeIdle.addRequirements(intake);
intakeCube = new IntakeCube(intake);
intakeCube.addRequirements(intake);
intakeCone = new IntakeCone(intake);
intakeCone.addRequirements(intake);
intakeStop = new IntakeStop(intake);
intakeStop.addRequirements(intake);
dropObject = new DropObject(intake);
dropObject.addRequirements(intake);
armDown = new ArmDown(arm);
armDown.addRequirements(arm);
armMidRow = new ArmMidRow(arm);
armMidRow.addRequirements(arm);
armTopRow = new ArmTopRow(arm);
armTopRow.addRequirements(arm);
autonomousC = new AutonomousC(intake, m_robotDrive);
autonomousC.addRequirements(intake);
/* armPick = new ArmPick(arm);
armPick.addRequirements(arm);*/
armAdjust = new ArmAdjust(arm);
armAdjust.addRequirements(arm);
armAdjustDown = new ArmAdjustDown(arm);
armAdjustDown.addRequirements(arm);
// Configure the button bindings
configureButtonBindings();
// Configure default commands
m_robotDrive.setDefaultCommand(
// The left stick controls translation of the robot.
// Turning is controlled by the X axis of the right stick.
new RunCommand(
() -> m_robotDrive.drive(
-MathUtil.applyDeadband(m_driverController.getLeftY(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(m_driverController.getLeftX(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband),
true, true),
m_robotDrive));
}
/**
* Use this method to define your button->command mappings. Buttons can be
* created by
* instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its
* subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling
* passing it to a
* {@link JoystickButton}.
*/
private void configureButtonBindings() {
new JoystickButton(m_driverController, XboxController.Button.kX.value)
.whileTrue(new RunCommand(
() -> m_robotDrive.setX(),
m_robotDrive));
new JoystickButton(m_operatorController, XboxController.Button.kStart.value)
.whileTrue(new ArmAdjust(arm));
new JoystickButton(m_operatorController, XboxController.Button.kBack.value)
.whileTrue(new ArmAdjustDown(arm));
new JoystickButton(m_operatorController, 5)
.whileTrue(new IntakeCube(intake))
.onFalse(new IntakeIdle(intake));
new JoystickButton(m_operatorController, 6)
.whileTrue(new IntakeCone(intake))
.onFalse(new IntakeIdle(intake));
new JoystickButton(m_operatorController, 3)
.whileTrue(new DropObject(intake))
.onFalse(new IntakeStop(intake));
new JoystickButton(m_operatorController, 1)
.onTrue(new ArmDown(arm));
new JoystickButton(m_operatorController, 2)
.onTrue(new ArmMidRow(arm));
new JoystickButton(m_operatorController, 4)
.onTrue(new ArmTopRow(arm));
/* new JoystickButton(m_operatorController, XboxController.Button.kStart.value)
.onTrue(new ArmTravel(arm));
new JoystickButton(m_operatorController, XboxController.Button.kBack.value)
.onTrue(new ArmPick(arm)); */
}
public void setArmDown() {
arm.armDown();
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// Run path following command, then stop at the end.
return autonomousC;
}
}