diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6cf0f1f..4ec8acb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -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) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4d2dd0c..72317c3 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -15,6 +15,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj.Servo; import frc4388.robot.Constants.ArmConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; @@ -170,7 +171,9 @@ public class RobotMap { } // claw stuff (WHAT IS A SOAP ENGINEER) - PWM leftClaw = new PWM(0); - PWM rightClaw = new PWM(1); + // PWM leftClaw = new PWM(0); + // PWM rightClaw = new PWM(1); + Servo leftClaw = new Servo(0); + Servo rightClaw = new Servo(1); CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless); } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java index 212d979..5fe2113 100644 --- a/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java +++ b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java @@ -26,11 +26,14 @@ public class AprilRotAlign extends PelvicInflammatoryDisease { @Override public double getError() { + System.out.println("Pipeline: " + lime.getCamera().getPipelineIndex()); double err = 0.0; try { err = lime.getAprilSkew(); - } catch (NullPointerException ex) {} + } catch (NullPointerException ex) { + System.out.println("CANT SEE APRIL POINT"); + } return err; } diff --git a/src/main/java/frc4388/robot/commands/TimedCommand.java b/src/main/java/frc4388/robot/commands/TimedCommand.java new file mode 100644 index 0000000..770ba6a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/TimedCommand.java @@ -0,0 +1,61 @@ +// 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 frc4388.robot.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Subsystem; +import frc4388.utility.RobotTime; + +public class TimedCommand extends CommandBase { + + Supplier cs; + double duration; + + long startTime; + long deltaTime; + + /** Creates a new TimedCommand. Duration is in seconds. */ + public TimedCommand(Supplier cs, double duration) { + this.cs = cs; + this.duration = duration; + + + Object[] reqs_obj = cs.get().getRequirements().toArray(); + Subsystem[] reqs = new Subsystem[reqs_obj.length]; + for (int i = 0; i < reqs.length; i++) + reqs[i] = (Subsystem) reqs_obj[i]; + + addRequirements(reqs); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + cs.get().initialize(); + + startTime = System.currentTimeMillis(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + cs.get().execute(); + + deltaTime = System.currentTimeMillis() - startTime; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (deltaTime / 1000.0) >= duration; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index adbc558..f48c4df 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -6,41 +6,62 @@ import java.util.TimerTask; import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Claw extends SubsystemBase { - private final PWM m_leftMotor; - private final PWM m_rightMotor; + // private final PWM m_leftMotor; + // private final PWM m_rightMotor; + private final Servo m_leftMotor; + private final Servo m_rightMotor; private final CANSparkMax m_spinnyspin; private boolean m_open = false; // Opens claw - public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) { + // public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) { + // m_leftMotor = leftMotor; + // m_rightMotor = rightMotor; + // m_spinnyspin = spinnyspin; + + // setClaw(false); + // } + public Claw(Servo leftMotor, Servo rightMotor, CANSparkMax spinnyspin) { m_leftMotor = leftMotor; m_rightMotor = rightMotor; m_spinnyspin = spinnyspin; - setClaw(true); + setClaw(false); } public void setClaw(boolean open) { // Open claw m_open = open; - m_leftMotor.setRaw(m_open ? 1000 : 2000); - m_rightMotor.setRaw(m_open ? 2000 : 1000); + // m_leftMotor.setRaw(m_open ? 1000 : 2000); + // m_rightMotor.setRaw(m_open ? 2000 : 1000); - if (!m_open) { - m_spinnyspin.set(-0.2); - - new Timer().schedule(new TimerTask() { - @Override - public void run() { - nospinnyspin(); - } - }, 750); - } + // m_leftMotor.setBounds(4000, 20000, 2000, 0, 0); + + m_leftMotor.setAngle(m_open ? 0 : 180); + m_rightMotor.setAngle(m_open ? 180 : 0); + + // m_leftMotor.setRaw(m_open ? 0 : 4000); + // m_rightMotor.setRaw(m_open ? 4000 : 0); + + // m_leftMotor.setRaw(m_open ? 1500 : 2000); + // m_rightMotor.setRaw(m_open ? 1500 : 1000); + + if (m_open) + m_spinnyspin.set(0.2); + else + m_spinnyspin.set(-0.2); + new Timer().schedule(new TimerTask() { + @Override + public void run() { + nospinnyspin(); + } + }, 750); } public void toggle() { diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index dff16ce..d9b5bec 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -28,6 +28,10 @@ public class Limelight extends SubsystemBase { cam.setDriverMode(true); } + public PhotonCamera getCamera() { + return cam; + } + public void setLEDs(boolean on) { lightOn = on; cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); @@ -44,12 +48,14 @@ public class Limelight extends SubsystemBase { public void setToLimePipeline() { cam.setPipelineIndex(1); - setLEDs(true); + // setLEDs(true); + cam.setLED(VisionLEDMode.kOn); } public void setToAprilPipeline() { cam.setPipelineIndex(0); - setLEDs(false); + // setLEDs(false); + cam.setLED(VisionLEDMode.kOff); } public PhotonTrackedTarget getAprilPoint() { @@ -63,11 +69,17 @@ public class Limelight extends SubsystemBase { } private List getAprilCorners() { - if (!cam.isConnected()) return null; + if (!cam.isConnected()) { + System.out.println("Camera is not connected"); + return null; + } PhotonPipelineResult result = cam.getLatestResult(); - if (!result.hasTargets()) return null; + if (!result.hasTargets()) { + System.out.println("Camera does not have targets"); + return null; + } return result.getBestTarget().getDetectedCorners(); } @@ -76,7 +88,10 @@ public class Limelight extends SubsystemBase { List corners = getAprilCorners(); ArrayList bottomSide = getAprilBottomSide(corners); - if (bottomSide == null) return 0; + if (bottomSide == null) { + // System.out.println("CANT SEE APRIL TAG"); + return 0; + } TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);