diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java index 912c3f43..23c845bd 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java @@ -1,5 +1,5 @@ /************************ PROJECT JIM *************************/ -/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/ /* This work is licensed under the terms of the MIT license. */ /**************************************************************/ diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveTranslateToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveTranslateToNote.java new file mode 100644 index 00000000..14a05ee3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveTranslateToNote.java @@ -0,0 +1,78 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.swerve; + +import static com.stuypulse.robot.constants.Settings.NoteDetection.*; + +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.vision.NoteVision; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +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.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveTranslateToNote extends Command { + + private final SwerveDrive swerve; + private final Odometry odometry; + private final NoteVision vision; + + private final HolonomicController controller; + private final BStream aligned; + + public SwerveDriveTranslateToNote(){ + this.swerve = SwerveDrive.getInstance(); + this.odometry = Odometry.getInstance(); + this.vision = NoteVision.getInstance(); + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D) + ); + + SmartDashboard.putData("Note Detection/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get()); + } + + @Override + public void execute() { + Translation2d targetTranslation = odometry.getTranslation().plus( + new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation())); + + Rotation2d targetRotation = vision.getEstimatedNotePose().minus(targetTranslation).getAngle(); + + Pose2d targetPose = new Pose2d(targetTranslation, targetRotation); + + // translate to note only if note in view + if (vision.hasNoteData()) { + swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(odometry.getTranslation(), targetRotation))); + } + + SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get()); + } + + @Override + public boolean isFinished() { + return aligned.get(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/robotrelative/SwerveDriveTranslateToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/robotrelative/SwerveDriveTranslateToNote.java new file mode 100644 index 00000000..4f45b458 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/robotrelative/SwerveDriveTranslateToNote.java @@ -0,0 +1,77 @@ +/************************ PROJECT IZZI *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.swerve.robotrelative; + +import static com.stuypulse.robot.constants.Settings.NoteDetection.*; + +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.subsystems.vision.NoteVision; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +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.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveTranslateToNote extends Command { + + private final SwerveDrive swerve; + private final NoteVision vision; + private final Odometry odometry; + + private final HolonomicController controller; + private final BStream aligned; + + public SwerveDriveTranslateToNote(){ + this.swerve = SwerveDrive.getInstance(); + this.vision = NoteVision.getInstance(); + odometry = Odometry.getInstance(); + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D) + ); + + SmartDashboard.putData("Note Detection/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get()); + } + + @Override + public void execute() { + Translation2d robotToNote = vision.getRobotRelativeNotePose(); + Rotation2d kZero = new Rotation2d(); + Pose2d targetPose = new Pose2d( + new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation()), + kZero); + + // translate to note only if note in view + if(vision.hasNoteData()) { + swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(robotToNote, kZero))); + } + + SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get()); + } + + @Override + public boolean isFinished() { + return aligned.get(); + } +} \ No newline at end of file