Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

nmcg

Browse Source
This commit is contained in:
Ryan Manley
2022-03-24 19:32:22 -06:00
parent c692f452cd
commit 7e45b507ed
1 changed files with 12 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+12 -14
View File
@@ -29,7 +29,6 @@ import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
@@ -110,9 +109,6 @@ public class RobotContainer {
private enum ClimberMode { MANUAL, AUTONOMOUS }; private enum ClimberMode { MANUAL, AUTONOMOUS };
private ClimberMode currentClimberMode = ClimberMode.MANUAL; private ClimberMode currentClimberMode = ClimberMode.MANUAL;
private enum DriveMode { ON, OFF };
private DriveMode currentDriveMode = DriveMode.ON;
private SendableChooser<SequentialCommandGroup> quickAutoChooser = new SendableChooser<>(); private SendableChooser<SequentialCommandGroup> quickAutoChooser = new SendableChooser<>();
/** /**
@@ -224,13 +220,13 @@ public class RobotContainer {
// Swerve Drive with Input // Swerve Drive with Input
m_robotSwerveDrive.setDefaultCommand( m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> { new RunCommand(() -> {
if (currentDriveMode.equals(DriveMode.ON)) { if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) {
m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(),
getDriverController().getLeftY(), getDriverController().getLeftY(),
getDriverController().getRightX(), getDriverController().getRightX(),
getDriverController().getRightY(), getDriverController().getRightY(),
true); } true); }
if (currentDriveMode.equals(DriveMode.OFF)) { if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) {
m_robotSwerveDrive.driveWithInput( 0, m_robotSwerveDrive.driveWithInput( 0,
0, 0,
0, 0,
@@ -309,7 +305,7 @@ public class RobotContainer {
} }
}.withName("Reload")); }.withName("Reload"));
} }
} }
/** /**
@@ -418,13 +414,15 @@ public class RobotContainer {
// Middle Switch > Climber and Shooter mode switching // Middle Switch > Climber and Shooter mode switching
new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
.whenPressed(new InstantCommand(() -> RobotContainer.currentControlMode = ControlMode.CLIMBER)) .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
.whenReleased(new InstantCommand(() -> RobotContainer.currentControlMode = ControlMode.SHOOTER)); .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER));
new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
// Right Switch > Drive On vs Off mode switching .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
.whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER));
new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
.whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF)) .whileHeld(new InstantCommand(() -> m_robotExtender.invertExtender(-1.0)))
.whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON)); .whenReleased(new InstantCommand(() -> m_robotExtender.invertExtender(1.0)));
// Left Button > Extender In // Left Button > Extender In
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
@@ -511,7 +509,7 @@ public class RobotContainer {
double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0 double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0
double offset = 10.0; // * distance (in inches) from ball that we actually want to stop double offset = 10.0; // * distance (in inches) from ball that we actually want to stop
// ! ball positions are unit tested // ! ball positions are "unit tested"
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub. Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub.
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub. Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs. Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.