Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

new claw servo working

Browse Source
This commit is contained in:
Aarav
2023-03-23 15:45:08 -06:00
parent 731f14ecb8
commit 947b65481a
6 changed files with 165 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+38 -14
View File
@@ -27,12 +27,14 @@ import frc4388.robot.subsystems.Claw;
import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.commands.BooleanCommand;
import frc4388.robot.commands.TimedCommand;
import frc4388.robot.commands.Arm.PivotCommand;
import frc4388.robot.commands.Arm.TeleCommand;
import frc4388.robot.commands.Autos.AutoBalance;
import frc4388.robot.commands.Autos.PlaybackChooser;
import frc4388.robot.commands.Placement.AprilRotAlign;
import frc4388.robot.commands.Placement.LimeAlign;
import frc4388.robot.commands.Swerve.JoystickRecorder;
import frc4388.robot.commands.Swerve.RotateToAngle;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.XboxController;
@@ -93,9 +95,9 @@ public class RobotContainer {
private SequentialCommandGroup alignToPole =
new SequentialCommandGroup(
new RotateToAngle(m_robotSwerveDrive, 0.0),
new InstantCommand(() -> m_robotLimeLight.setDriverMode(false), m_robotLimeLight),
new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight),
new RotateToAngle(m_robotSwerveDrive, 0.0),
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04),
new RotateToAngle(m_robotSwerveDrive, 0.0),
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
@@ -113,9 +115,9 @@ public class RobotContainer {
private SequentialCommandGroup alignToShelf =
new SequentialCommandGroup(
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
new InstantCommand(() -> m_robotLimeLight.setDriverMode(false), m_robotLimeLight),
new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
@@ -144,6 +146,15 @@ public class RobotContainer {
armToHome.asProxy()
);
private SequentialCommandGroup placeCubeLow = new SequentialCommandGroup(
new TimedCommand(() -> new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.2), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive), 0.7),
new PivotCommand(m_robotArm, 70 + 135),
new InstantCommand(() -> m_robotArm.setRotVel(0)),
new TeleCommand(m_robotArm, 28000),
toggleClaw.asProxy(),
armToHome.asProxy()
);
private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(
new PivotCommand(m_robotArm, 0),
new TeleCommand(m_robotArm, 0),
@@ -221,6 +232,12 @@ public class RobotContainer {
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// * TEST BUTTONS
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
// * Driver Buttons
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
@@ -255,14 +272,15 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(interruptCommand.asProxy()); // final
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
// () -> getDeadbandedDriverController().getLeftX(),
// () -> getDeadbandedDriverController().getLeftY(),
// () -> getDeadbandedDriverController().getRightX(),
// () -> getDeadbandedDriverController().getRightY(),
// "Blue1Path.txt"))
// .onFalse(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_TRIGGER_AXIS)
.whileTrue(new JoystickRecorder(m_robotSwerveDrive,
() -> getDeadbandedDriverController().getLeftX(),
() -> getDeadbandedDriverController().getLeftY(),
() -> getDeadbandedDriverController().getRightX(),
() -> getDeadbandedDriverController().getRightY(),
"Blue1Path.txt"))
.onFalse(new InstantCommand());
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
@@ -288,19 +306,21 @@ public class RobotContainer {
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
// outtake
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_TRIGGER_AXIS) // final
.onTrue (new InstantCommand(() -> m_robotClaw.reversespinnyspin()))
.onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_TRIGGER_AXIS) // final
// intake
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_TRIGGER_AXIS) // final
.onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin()))
.onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
//Arm to Home
// arm to Home
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final
.onTrue(armToHome.asProxy());
//Interupt Button
// interupt
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(interruptCommand.asProxy());
@@ -337,9 +357,13 @@ public class RobotContainer {
// () -> readyForPlacement == true)
// );
// // place low
// new POVButton(getDeadbandedOperatorController(), 180)
// .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
// place low
new POVButton(getDeadbandedOperatorController(), 180)
.onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
.onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeCubeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
// confirm
new POVButton(getDeadbandedOperatorController(), 90)