diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 71b30e9..dfffdf4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -135,34 +135,34 @@ public class RobotContainer { private SequentialCommandGroup placeCubeHigh = new SequentialCommandGroup( new PivotCommand(m_robotArm, 60 + 135), - new InstantCommand(() -> m_robotArm.setRotVel(0)), - new TeleCommand(m_robotArm, 90000), - toggleClaw.asProxy(), - armToHome.asProxy() + new WaitCommand(0.3), + new TeleCommand(m_robotArm, 90000) + // toggleClaw.asProxy(), + // armToHome.asProxy() ); private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup( new PivotCommand(m_robotArm, 55 + 135), - new InstantCommand(() -> m_robotArm.setRotVel(0)), - new TeleCommand(m_robotArm, 28000), - toggleClaw.asProxy(), - armToHome.asProxy() + new WaitCommand(0.3), + new TeleCommand(m_robotArm, 28000) + // toggleClaw.asProxy(), + // 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() + new WaitCommand(0.3), + new TeleCommand(m_robotArm, 28000) + // toggleClaw.asProxy(), + // armToHome.asProxy() ); private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup( new PivotCommand(m_robotArm, 178), new WaitCommand(0.3), - new TeleCommand(m_robotArm, 56000), - new ParallelRaceGroup(new InstantCommand(() -> m_robotClaw.toggle()), new RunCommand(() -> m_robotClaw.outtake())) + new TeleCommand(m_robotArm, 56000) + // new ParallelRaceGroup(new InstantCommand(() -> m_robotClaw.toggle()), new RunCommand(() -> m_robotClaw.outtake())) // toggleClaw.asProxy(), // armToHome.asProxy() ); @@ -304,10 +304,10 @@ public class RobotContainer { .onFalse(interruptCommand.asProxy()); // toggle claw - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final - .onTrue(toggleClaw.asProxy()); // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotClaw.setAngle(45), m_robotClaw)); + // .onTrue(toggleClaw.asProxy()); + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotClaw.setAngle(45), m_robotClaw)); // kill soft limits new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final diff --git a/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java index 5fe2113..208859e 100644 --- a/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java +++ b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java @@ -26,13 +26,13 @@ public class AprilRotAlign extends PelvicInflammatoryDisease { @Override public double getError() { - System.out.println("Pipeline: " + lime.getCamera().getPipelineIndex()); + // System.out.println("Pipeline: " + lime.getCamera().getPipelineIndex()); double err = 0.0; try { err = lime.getAprilSkew(); } catch (NullPointerException ex) { - System.out.println("CANT SEE APRIL POINT"); + // System.out.println("CANT SEE APRIL POINT"); } return err; diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index b3e9d51..6e4c052 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -147,8 +147,8 @@ public class Arm extends SubsystemBase { tele_reset = true; } - SmartDashboard.putNumber("Pivot CANCoder", getArmRotation()); - SmartDashboard.putNumber("Tele Encoder", getArmLength()); + // SmartDashboard.putNumber("Pivot CANCoder", getArmRotation()); + // SmartDashboard.putNumber("Tele Encoder", getArmLength()); // double x = Math.cos(Math.toRadians(degrees)); }