From 0fe1a0ce647e1f8a0527cc301be8194ad641992c Mon Sep 17 00:00:00 2001 From: iamawesomecat Date: Sat, 9 Mar 2024 12:57:58 -0800 Subject: [PATCH 01/11] PIDFF for pivot & manual control rework Reworked manual control and added a feedforward system to the pivot --- .../java/frc/team2412/robot/Controls.java | 6 +-- .../diagnostic/LauncherDiagnosticCommand.java | 10 ++--- ...leCommand.java => ManualAngleCommand.java} | 10 ++--- .../launcher/SetAngleAmpLaunchCommand.java | 1 + .../launcher/SetAngleLaunchCommand.java | 1 + .../robot/subsystems/LauncherSubsystem.java | 44 +++++++++++++++---- 6 files changed, 50 insertions(+), 22 deletions(-) rename src/main/java/frc/team2412/robot/commands/launcher/{SetAngleCommand.java => ManualAngleCommand.java} (63%) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index e5b44ea7..017a9484 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -20,8 +20,8 @@ import frc.team2412.robot.commands.intake.FeederInCommand; import frc.team2412.robot.commands.intake.IntakeRejectCommand; import frc.team2412.robot.commands.launcher.FullTargetCommand; +import frc.team2412.robot.commands.launcher.ManualAngleCommand; import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand; -import frc.team2412.robot.commands.launcher.SetAngleCommand; import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; @@ -109,7 +109,7 @@ private void bindDrivebaseControls() { () -> Rotation2d.fromRotations(driveController.getRightX()))); driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro)); driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels)); - // driveController + // driveController x // .back() // .onTrue( // new InstantCommand( @@ -140,7 +140,7 @@ private void bindLauncherControls() { CommandScheduler.getInstance() .setDefaultCommand( s.launcherSubsystem, - new SetAngleCommand( + new ManualAngleCommand( s.launcherSubsystem, () -> MathUtil.applyDeadband(codriveController.getLeftY(), 0.1) diff --git a/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java b/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java index eab92cfb..fe254e5f 100644 --- a/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java +++ b/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java @@ -2,8 +2,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.team2412.robot.commands.launcher.SetAngleCommand; -import frc.team2412.robot.commands.launcher.SetLaunchSpeedCommand; +import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; import frc.team2412.robot.commands.launcher.StopLauncherCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; @@ -15,12 +14,11 @@ public LauncherDiagnosticCommand(LauncherSubsystem launcherSubsystem) { this.launcherSubsystem = launcherSubsystem; this.Angle = launcherSubsystem.getAngle(); addCommands( - new SetAngleCommand(launcherSubsystem, () -> 45), + new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 10), new WaitCommand(2), - new SetAngleCommand(launcherSubsystem, () -> 90), + new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 20), new WaitCommand(1), - new SetAngleCommand(launcherSubsystem, () -> Angle), - new SetLaunchSpeedCommand(launcherSubsystem, 100), + new SetAngleLaunchCommand(launcherSubsystem, 500, Angle), new WaitCommand(5), new StopLauncherCommand(launcherSubsystem)); } diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java similarity index 63% rename from src/main/java/frc/team2412/robot/commands/launcher/SetAngleCommand.java rename to src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java index 7aaa630c..8c5d293a 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java @@ -5,20 +5,20 @@ import java.util.function.DoubleSupplier; // this command adjusts the angle using a speed that is input by the user -public class SetAngleCommand extends Command { +public class ManualAngleCommand extends Command { private final LauncherSubsystem launcherSubsystem; - private final DoubleSupplier launcherAngleSpeed; + private final DoubleSupplier launcherAngle; - public SetAngleCommand(LauncherSubsystem launcherSubsystem, DoubleSupplier angleSpeed) { + public ManualAngleCommand(LauncherSubsystem launcherSubsystem, DoubleSupplier angleSpeed) { this.launcherSubsystem = launcherSubsystem; - this.launcherAngleSpeed = angleSpeed; + this.launcherAngle = angleSpeed; addRequirements(launcherSubsystem); } @Override public void execute() { - launcherSubsystem.setAngleSpeed(launcherAngleSpeed.getAsDouble()); + launcherSubsystem.setAngleManual(launcherAngle.getAsDouble()); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleAmpLaunchCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SetAngleAmpLaunchCommand.java index e6e740ca..b081498b 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleAmpLaunchCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SetAngleAmpLaunchCommand.java @@ -22,6 +22,7 @@ public SetAngleAmpLaunchCommand(LauncherSubsystem launcherSubsystem, double spee public void initialize() { launcherSubsystem.setAngle(launcherAngle); launcherSubsystem.ampLaunch(launcherSpeed); + launcherSubsystem.manualSetpoint(launcherAngle); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java index a791b948..f69ffc2d 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java @@ -22,6 +22,7 @@ public SetAngleLaunchCommand(LauncherSubsystem launcherSubsystem, double speed, public void initialize() { launcherSubsystem.setAngle(launcherAngle); launcherSubsystem.launch(launcherSpeed); + launcherSubsystem.manualSetpoint(launcherAngle); } @Override diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index e2e53976..cdcf2073 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -10,6 +10,7 @@ import com.revrobotics.SparkAbsoluteEncoder.Type; import com.revrobotics.SparkPIDController; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; @@ -29,6 +30,11 @@ public class LauncherSubsystem extends SubsystemBase { public static final int SUBWOOFER_AIM_ANGLE = 298; public static final int PODIUM_AIM_ANGLE = 39; public static final int TRAP_AIM_ANGLE = 290; + public static final double MANUAL_MODIFIER = 0.02; + public static final double RETRACTED_ANGLE = 255; + // offset for FF so parallel to floor is 0 + public static final double FF_PIVOT_OFFSET = 250; + // MOTOR VALUES // max Free Speed: 6784 RPM private static final int MAX_FREE_SPEED_RPM = 6784; @@ -52,12 +58,13 @@ public class LauncherSubsystem extends SubsystemBase { private final SparkAbsoluteEncoder launcherAngleEncoder; private final SparkPIDController launcherAngleOnePIDController; // private final SparkPIDController launcherAngleTwoPIDController; - + private final ArmFeedforward launcherAngleFF = new ArmFeedforward(0.1, 0.1, 0); private final SparkPIDController launcherTopPIDController; private final SparkPIDController launcherBottomPIDController; private double rpmSetpoint; private double angleSetpoint; + private double manualAngleSetpoint; private GenericEntry setLauncherSpeedEntry; @@ -73,6 +80,8 @@ public class LauncherSubsystem extends SubsystemBase { private GenericEntry launcherIsAtSpeed; + private GenericEntry launcherAngleManual; + // Constructors public LauncherSubsystem() { @@ -88,6 +97,7 @@ public LauncherSubsystem() { launcherTopEncoder = launcherTopMotor.getEncoder(); launcherBottomEncoder = launcherBottomMotor.getEncoder(); launcherAngleEncoder = launcherAngleOneMotor.getAbsoluteEncoder(Type.kDutyCycle); + manualAngleSetpoint = launcherAngleEncoder.getPosition(); // PID controllers // Create launcherTopPIDController and launcherTopMotor] @@ -134,8 +144,11 @@ public void configMotors() { // PID launcherAngleOnePIDController.setP(5.0); launcherAngleOnePIDController.setI(0); - launcherAngleOnePIDController.setD(0); - launcherAngleOnePIDController.setFF(0); + launcherAngleOnePIDController.setD(3); + launcherAngleOnePIDController.setFF( + launcherAngleFF.calculate( + Units.rotationsToRadians(launcherAngleEncoder.getPosition() - FF_PIVOT_OFFSET), + launcherAngleEncoder.getVelocity())); launcherAngleOnePIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED); // launcherAngleTwoPIDController.setP(0.1); @@ -183,7 +196,7 @@ public void launch(double speed) { } public void ampLaunch(double speed) { - launcherTopPIDController.setReference(-speed, ControlType.kVelocity); + launcherTopPIDController.setReference(-speed, ControlType.kVelocity); launcherBottomMotor.disable(); } @@ -224,10 +237,18 @@ public double getAngleSpeed() { return launcherAngleEncoder.getVelocity(); } - public void setAngleSpeed(double Speed) { - // launcherAngleOnePIDController.setReference(Speed, ControlType.kVelocity); - // launcherAngleTwoPIDController.setReference(Speed, ControlType.kVelocity); - launcherAngleOneMotor.set(Speed); + public void setAngleManual(double joystickInput) { + manualAngleSetpoint = + MathUtil.clamp(manualAngleSetpoint + joystickInput * MANUAL_MODIFIER, 0.71f, 0.95f); + + if (Units.degreesToRotations(getAngle()) > 0.71 + && Units.degreesToRotations(getAngle()) < 0.95) { + launcherAngleOnePIDController.setReference(manualAngleSetpoint, ControlType.kPosition); + } + } + + public void manualSetpoint(double setpoint) { + manualAngleSetpoint = setpoint; } private void initShuffleboard() { @@ -282,6 +303,12 @@ private void initShuffleboard() { .withProperties(Map.of("Min", -MAX_FREE_SPEED_RPM, "Max", MAX_FREE_SPEED_RPM)) .withPosition(5, 0) .getEntry(); + launcherAngleManual = + Shuffleboard.getTab("Launcher") + .add("Launcher manual increase", 0) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); Shuffleboard.getTab("Launcher") .add(new SparkPIDWidget(launcherAngleOnePIDController, "launcherAnglePID")) .withPosition(2, 0); @@ -303,5 +330,6 @@ public void periodic() { launcherTopFlywheelTemp.setDouble(launcherTopMotor.getMotorTemperature()); launcherBottomFlyWheelTemp.setDouble(launcherTopMotor.getMotorTemperature()); launcherIsAtSpeed.setBoolean(isAtSpeed()); + launcherAngleManual.setDouble(manualAngleSetpoint); } } From 0dc7ab8fff8c64dbd1647547c2b6a4182260caf3 Mon Sep 17 00:00:00 2001 From: iamawesomecat Date: Tue, 12 Mar 2024 18:05:08 -0700 Subject: [PATCH 02/11] SetPivotCommand + lower pivot preset (also changed some poses for the red alliance trap) --- .../java/frc/team2412/robot/Controls.java | 6 ++++ .../commands/launcher/SetPivotCommand.java | 29 +++++++++++++++++++ .../frc/team2412/robot/util/TrapAlign.java | 10 +++++-- 3 files changed, 42 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 017a9484..f79742b4 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -23,6 +23,7 @@ import frc.team2412.robot.commands.launcher.ManualAngleCommand; import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand; import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; +import frc.team2412.robot.commands.launcher.SetPivotCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; public class Controls { @@ -46,6 +47,7 @@ public static class ControlConstants { // Launcher private final Trigger launcherAmpPresetButton; private final Trigger launcherSubwooferPresetButton; + private final Trigger launcherLowerPresetButton; // private final Trigger launcherPodiumPresetButton; // private final Trigger launcherTrapPresetButton; private final Trigger launcherLaunchButton; @@ -59,6 +61,7 @@ public Controls(Subsystems s) { launcherAmpPresetButton = codriveController.x(); launcherSubwooferPresetButton = codriveController.a(); + launcherLowerPresetButton = codriveController.y(); // launcherPodiumPresetButton = codriveController.povLeft(); // launcherTrapPresetButton = codriveController.y(); launcherLaunchButton = codriveController.leftBumper(); @@ -146,6 +149,9 @@ private void bindLauncherControls() { MathUtil.applyDeadband(codriveController.getLeftY(), 0.1) * LauncherSubsystem.ANGLE_MAX_SPEED)); + launcherLowerPresetButton.onTrue( + new SetPivotCommand(s.launcherSubsystem, LauncherSubsystem.RETRACTED_ANGLE) + ); launcherSubwooferPresetButton.onTrue( new SetAngleLaunchCommand( s.launcherSubsystem, diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java new file mode 100644 index 00000000..4cecc0cc --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java @@ -0,0 +1,29 @@ +package frc.team2412.robot.commands.launcher; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.Angle; +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.subsystems.LauncherSubsystem; + +public class SetPivotCommand extends Command{ + private final LauncherSubsystem launcherSubsystem; + private final double angle; + public SetPivotCommand(LauncherSubsystem launcherSubsystem, double angle){ + this.launcherSubsystem = launcherSubsystem; + this.angle = angle; + addRequirements(launcherSubsystem); + } + + @Override + public void initialize(){ + launcherSubsystem.setAngle(angle); + } + + @Override + public boolean isFinished() { + return MathUtil.isNear( + angle, launcherSubsystem.getAngle(), LauncherSubsystem.ANGLE_TOLERANCE); + } + + +} diff --git a/src/main/java/frc/team2412/robot/util/TrapAlign.java b/src/main/java/frc/team2412/robot/util/TrapAlign.java index 167fa8d9..0abdedc6 100644 --- a/src/main/java/frc/team2412/robot/util/TrapAlign.java +++ b/src/main/java/frc/team2412/robot/util/TrapAlign.java @@ -18,10 +18,12 @@ public class TrapAlign { private static final Pose2d[] BLUE_TRAP_POSES = { // trap that faces amp new Pose2d(new Translation2d(4.3, 5.14), Rotation2d.fromDegrees(-60)), - // trap that faces source + // trap that faces source new Pose2d(new Translation2d(4.3, 3.09), Rotation2d.fromDegrees(-300)), // trap that faces mid - new Pose2d(new Translation2d(6, 4), Rotation2d.fromDegrees(180)) + //DO THIS ONE FIRST + //brute force the X lol + new Pose2d(new Translation2d(6, 4.10), Rotation2d.fromDegrees(180)) }; private static final Pose2d[] RED_TRAP_POSES = { @@ -30,7 +32,9 @@ public class TrapAlign { // trap that faces source new Pose2d(new Translation2d(12.3, 3.09), Rotation2d.fromDegrees(120)), // trap that faces mid - new Pose2d(new Translation2d(10, 4), Rotation2d.fromDegrees(0)) + //DO THIS ONE FIRST + //brute force the X lol + new Pose2d(new Translation2d(10, 4.10), Rotation2d.fromDegrees(0)) }; private static Command trapAlign(DrivebaseSubsystem drivebaseSubsystem) { From 6fea8f8ddb60440dd96dbf00a719970490432e4d Mon Sep 17 00:00:00 2001 From: iamawesomecat Date: Tue, 12 Mar 2024 18:06:49 -0700 Subject: [PATCH 03/11] =?UTF-8?q?Spotless=204=EF=B8=8F=E2=83=A34=EF=B8=8F?= =?UTF-8?q?=E2=83=A34=EF=B8=8F=E2=83=A34=EF=B8=8F=E2=83=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/frc/team2412/robot/Controls.java | 3 +- .../commands/launcher/SetPivotCommand.java | 31 +++++++++---------- .../robot/subsystems/LauncherSubsystem.java | 2 +- .../frc/team2412/robot/util/TrapAlign.java | 10 +++--- 4 files changed, 21 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index f79742b4..4cb7cea8 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -150,8 +150,7 @@ private void bindLauncherControls() { * LauncherSubsystem.ANGLE_MAX_SPEED)); launcherLowerPresetButton.onTrue( - new SetPivotCommand(s.launcherSubsystem, LauncherSubsystem.RETRACTED_ANGLE) - ); + new SetPivotCommand(s.launcherSubsystem, LauncherSubsystem.RETRACTED_ANGLE)); launcherSubwooferPresetButton.onTrue( new SetAngleLaunchCommand( s.launcherSubsystem, diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java index 4cecc0cc..482a2111 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java @@ -1,29 +1,26 @@ package frc.team2412.robot.commands.launcher; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.units.Angle; import edu.wpi.first.wpilibj2.command.Command; import frc.team2412.robot.subsystems.LauncherSubsystem; -public class SetPivotCommand extends Command{ - private final LauncherSubsystem launcherSubsystem; - private final double angle; - public SetPivotCommand(LauncherSubsystem launcherSubsystem, double angle){ - this.launcherSubsystem = launcherSubsystem; - this.angle = angle; - addRequirements(launcherSubsystem); - } +public class SetPivotCommand extends Command { + private final LauncherSubsystem launcherSubsystem; + private final double angle; - @Override - public void initialize(){ - launcherSubsystem.setAngle(angle); - } + public SetPivotCommand(LauncherSubsystem launcherSubsystem, double angle) { + this.launcherSubsystem = launcherSubsystem; + this.angle = angle; + addRequirements(launcherSubsystem); + } @Override - public boolean isFinished() { - return MathUtil.isNear( - angle, launcherSubsystem.getAngle(), LauncherSubsystem.ANGLE_TOLERANCE); + public void initialize() { + launcherSubsystem.setAngle(angle); } - + @Override + public boolean isFinished() { + return MathUtil.isNear(angle, launcherSubsystem.getAngle(), LauncherSubsystem.ANGLE_TOLERANCE); + } } diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index cdcf2073..cf752e57 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -196,7 +196,7 @@ public void launch(double speed) { } public void ampLaunch(double speed) { - launcherTopPIDController.setReference(-speed, ControlType.kVelocity); + launcherTopPIDController.setReference(-speed, ControlType.kVelocity); launcherBottomMotor.disable(); } diff --git a/src/main/java/frc/team2412/robot/util/TrapAlign.java b/src/main/java/frc/team2412/robot/util/TrapAlign.java index 0abdedc6..368254c7 100644 --- a/src/main/java/frc/team2412/robot/util/TrapAlign.java +++ b/src/main/java/frc/team2412/robot/util/TrapAlign.java @@ -18,11 +18,11 @@ public class TrapAlign { private static final Pose2d[] BLUE_TRAP_POSES = { // trap that faces amp new Pose2d(new Translation2d(4.3, 5.14), Rotation2d.fromDegrees(-60)), - // trap that faces source + // trap that faces source new Pose2d(new Translation2d(4.3, 3.09), Rotation2d.fromDegrees(-300)), // trap that faces mid - //DO THIS ONE FIRST - //brute force the X lol + // DO THIS ONE FIRST + // brute force the X lol new Pose2d(new Translation2d(6, 4.10), Rotation2d.fromDegrees(180)) }; @@ -32,8 +32,8 @@ public class TrapAlign { // trap that faces source new Pose2d(new Translation2d(12.3, 3.09), Rotation2d.fromDegrees(120)), // trap that faces mid - //DO THIS ONE FIRST - //brute force the X lol + // DO THIS ONE FIRST + // brute force the X lol new Pose2d(new Translation2d(10, 4.10), Rotation2d.fromDegrees(0)) }; From 3e3bd8d3e99c820d7e85013dceeeb73c1d6ecee1 Mon Sep 17 00:00:00 2001 From: iamawesomecat Date: Tue, 12 Mar 2024 18:28:55 -0700 Subject: [PATCH 04/11] adjusted trap aim angle --- .../java/frc/team2412/robot/subsystems/LauncherSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index cf752e57..328b9609 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -29,7 +29,7 @@ public class LauncherSubsystem extends SubsystemBase { public static final int AMP_AIM_ANGLE = 335; public static final int SUBWOOFER_AIM_ANGLE = 298; public static final int PODIUM_AIM_ANGLE = 39; - public static final int TRAP_AIM_ANGLE = 290; + public static final int TRAP_AIM_ANGLE = 317; public static final double MANUAL_MODIFIER = 0.02; public static final double RETRACTED_ANGLE = 255; // offset for FF so parallel to floor is 0 From 86bf3dd636ab24d02a430144b49c48ae19134e7c Mon Sep 17 00:00:00 2001 From: iamawesomecat Date: Tue, 12 Mar 2024 18:53:19 -0700 Subject: [PATCH 05/11] FF fixes --- .../robot/subsystems/LauncherSubsystem.java | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 328b9609..c885621d 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -145,11 +145,7 @@ public void configMotors() { launcherAngleOnePIDController.setP(5.0); launcherAngleOnePIDController.setI(0); launcherAngleOnePIDController.setD(3); - launcherAngleOnePIDController.setFF( - launcherAngleFF.calculate( - Units.rotationsToRadians(launcherAngleEncoder.getPosition() - FF_PIVOT_OFFSET), - launcherAngleEncoder.getVelocity())); - launcherAngleOnePIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED); + launcherAngleOnePIDController.setFF(0); // launcherAngleTwoPIDController.setP(0.1); // launcherAngleTwoPIDController.setI(0); @@ -212,7 +208,12 @@ public double getAngle() { public void setAngle(double launcherAngle) { angleSetpoint = launcherAngle; launcherAngleOnePIDController.setReference( - Units.degreesToRotations(angleSetpoint), ControlType.kPosition); + Units.degreesToRotations(angleSetpoint), + ControlType.kPosition, + 0, + launcherAngleFF.calculate( + Units.rotationsToRadians(launcherAngleEncoder.getPosition() - FF_PIVOT_OFFSET), + launcherAngleEncoder.getVelocity())); // launcherAngleTwoPIDController.setReference( // Units.degreesToRotations(angleSetpoint), ControlType.kPosition); } @@ -243,7 +244,13 @@ public void setAngleManual(double joystickInput) { if (Units.degreesToRotations(getAngle()) > 0.71 && Units.degreesToRotations(getAngle()) < 0.95) { - launcherAngleOnePIDController.setReference(manualAngleSetpoint, ControlType.kPosition); + launcherAngleOnePIDController.setReference( + manualAngleSetpoint, + ControlType.kPosition, + 0, + launcherAngleFF.calculate( + Units.rotationsToRadians(launcherAngleEncoder.getPosition() - FF_PIVOT_OFFSET), + launcherAngleEncoder.getVelocity())); } } From 8e931a23dea8a42018ad5516bf37b19c225bed0f Mon Sep 17 00:00:00 2001 From: iamawesomecat Date: Thu, 14 Mar 2024 20:23:33 -0700 Subject: [PATCH 06/11] fixed trap align hear me out, trap scoring might actually be possible by sammy --- .../java/frc/team2412/robot/Controls.java | 9 ++++---- .../frc/team2412/robot/util/TrapAlign.java | 23 +++++++++++-------- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 4cb7cea8..0c792405 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -25,6 +25,7 @@ import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; import frc.team2412.robot.commands.launcher.SetPivotCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; +import frc.team2412.robot.util.TrapAlign; public class Controls { public static class ControlConstants { @@ -49,7 +50,7 @@ public static class ControlConstants { private final Trigger launcherSubwooferPresetButton; private final Trigger launcherLowerPresetButton; // private final Trigger launcherPodiumPresetButton; - // private final Trigger launcherTrapPresetButton; + private final Trigger launcherTrapPresetButton; private final Trigger launcherLaunchButton; private final Subsystems s; @@ -63,7 +64,7 @@ public Controls(Subsystems s) { launcherSubwooferPresetButton = codriveController.a(); launcherLowerPresetButton = codriveController.y(); // launcherPodiumPresetButton = codriveController.povLeft(); - // launcherTrapPresetButton = codriveController.y(); + launcherTrapPresetButton = codriveController.y(); launcherLaunchButton = codriveController.leftBumper(); // intake controls (confirmed with driveteam) driveIntakeInButton = driveController.a(); @@ -166,8 +167,8 @@ private void bindLauncherControls() { s.launcherSubsystem, LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, LauncherSubsystem.AMP_AIM_ANGLE)); - // launcherTrapPresetButton.onTrue( - // TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); + launcherTrapPresetButton.onTrue( + TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); codriveController.b().whileTrue(s.launcherSubsystem.run(s.launcherSubsystem::stopLauncher)); diff --git a/src/main/java/frc/team2412/robot/util/TrapAlign.java b/src/main/java/frc/team2412/robot/util/TrapAlign.java index 368254c7..82af00fd 100644 --- a/src/main/java/frc/team2412/robot/util/TrapAlign.java +++ b/src/main/java/frc/team2412/robot/util/TrapAlign.java @@ -23,7 +23,7 @@ public class TrapAlign { // trap that faces mid // DO THIS ONE FIRST // brute force the X lol - new Pose2d(new Translation2d(6, 4.10), Rotation2d.fromDegrees(180)) + new Pose2d(new Translation2d(5.8, 4.10), Rotation2d.fromDegrees(180)) }; private static final Pose2d[] RED_TRAP_POSES = { @@ -34,20 +34,18 @@ public class TrapAlign { // trap that faces mid // DO THIS ONE FIRST // brute force the X lol - new Pose2d(new Translation2d(10, 4.10), Rotation2d.fromDegrees(0)) + new Pose2d(new Translation2d(10.8, 4.10), Rotation2d.fromDegrees(0)) }; private static Command trapAlign(DrivebaseSubsystem drivebaseSubsystem) { Pose2d robotPose = drivebaseSubsystem.getPose(); - Pose2d trapPose = - robotPose.nearest( - List.of( - (DriverStation.getAlliance().get().equals(Alliance.Blue)) - ? BLUE_TRAP_POSES - : RED_TRAP_POSES)); - + boolean isBlue; + isBlue = DriverStation.getAlliance().get().equals(Alliance.Blue); + // figures out which trap to go to + Pose2d trapPose = robotPose.nearest(List.of((isBlue) ? BLUE_TRAP_POSES : RED_TRAP_POSES)); + // sets the point for the path to go to List bezierPoints = PathPlannerPath.bezierFromPoses(robotPose, trapPose); - + // this is flipped PathPlannerPath path = new PathPlannerPath( bezierPoints, @@ -57,6 +55,11 @@ private static Command trapAlign(DrivebaseSubsystem drivebaseSubsystem) { DrivebaseSubsystem.MAX_ANGULAR_VELOCITY, DrivebaseSubsystem.MAX_ANGULAR_ACCELERAITON), new GoalEndState(0.0, trapPose.getRotation())); + // path.flipPath(); Returns path except it's flipped + // this unflips it + if (!isBlue) { + path = path.flipPath(); + } return AutoBuilder.followPath(path); } From 4200de1581071e64a2adcf7de023ec56cf77370a Mon Sep 17 00:00:00 2001 From: iamawesomecat Date: Sat, 16 Mar 2024 14:25:46 -0700 Subject: [PATCH 07/11] fixes :) --- .../commands/launcher/SetAngleLaunchCommand.java | 1 - .../robot/commands/launcher/SetPivotCommand.java | 2 +- .../team2412/robot/subsystems/LauncherSubsystem.java | 7 +++---- src/main/java/frc/team2412/robot/util/TrapAlign.java | 11 ++++++++--- 4 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java index f69ffc2d..a791b948 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java @@ -22,7 +22,6 @@ public SetAngleLaunchCommand(LauncherSubsystem launcherSubsystem, double speed, public void initialize() { launcherSubsystem.setAngle(launcherAngle); launcherSubsystem.launch(launcherSpeed); - launcherSubsystem.manualSetpoint(launcherAngle); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java index 482a2111..96d27e3c 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java @@ -17,7 +17,7 @@ public SetPivotCommand(LauncherSubsystem launcherSubsystem, double angle) { @Override public void initialize() { launcherSubsystem.setAngle(angle); - } + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index c885621d..59748a31 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -212,8 +212,8 @@ public void setAngle(double launcherAngle) { ControlType.kPosition, 0, launcherAngleFF.calculate( - Units.rotationsToRadians(launcherAngleEncoder.getPosition() - FF_PIVOT_OFFSET), - launcherAngleEncoder.getVelocity())); + Units.degreesToRadians(launcherAngle - FF_PIVOT_OFFSET), 0)); + manualAngleSetpoint = launcherAngle; // launcherAngleTwoPIDController.setReference( // Units.degreesToRotations(angleSetpoint), ControlType.kPosition); } @@ -249,8 +249,7 @@ public void setAngleManual(double joystickInput) { ControlType.kPosition, 0, launcherAngleFF.calculate( - Units.rotationsToRadians(launcherAngleEncoder.getPosition() - FF_PIVOT_OFFSET), - launcherAngleEncoder.getVelocity())); + Units.degreesToRadians(Units.rotationsToDegrees(manualAngleSetpoint) - FF_PIVOT_OFFSET), 0)); } } diff --git a/src/main/java/frc/team2412/robot/util/TrapAlign.java b/src/main/java/frc/team2412/robot/util/TrapAlign.java index 82af00fd..089c75f3 100644 --- a/src/main/java/frc/team2412/robot/util/TrapAlign.java +++ b/src/main/java/frc/team2412/robot/util/TrapAlign.java @@ -28,9 +28,9 @@ public class TrapAlign { private static final Pose2d[] RED_TRAP_POSES = { // trap that faces amp - new Pose2d(new Translation2d(12.3, 5.14), Rotation2d.fromDegrees(-60)), + new Pose2d(new Translation2d(12.3, 5.14), Rotation2d.fromDegrees(-120)), // trap that faces source - new Pose2d(new Translation2d(12.3, 3.09), Rotation2d.fromDegrees(120)), + new Pose2d(new Translation2d(12.3, 3.09), Rotation2d.fromDegrees(300)), // trap that faces mid // DO THIS ONE FIRST // brute force the X lol @@ -40,7 +40,11 @@ public class TrapAlign { private static Command trapAlign(DrivebaseSubsystem drivebaseSubsystem) { Pose2d robotPose = drivebaseSubsystem.getPose(); boolean isBlue; - isBlue = DriverStation.getAlliance().get().equals(Alliance.Blue); + if (DriverStation.getAlliance().isEmpty()) { + isBlue = DriverStation.getAlliance().get().equals(Alliance.Blue); + } else { + isBlue = false; + } // figures out which trap to go to Pose2d trapPose = robotPose.nearest(List.of((isBlue) ? BLUE_TRAP_POSES : RED_TRAP_POSES)); // sets the point for the path to go to @@ -86,6 +90,7 @@ public void initialize() { trapCommand.initialize(); launcherSubsystem.setAngle(LauncherSubsystem.TRAP_AIM_ANGLE); launcherSubsystem.launch(LauncherSubsystem.TRAP_SHOOT_SPEED_RPM); + launcherSubsystem.manualSetpoint(LauncherSubsystem.TRAP_AIM_ANGLE); } @Override From 65b4df322552830e3347126cd35a2cf51eaac467 Mon Sep 17 00:00:00 2001 From: iamawesomecat Date: Sat, 16 Mar 2024 14:27:57 -0700 Subject: [PATCH 08/11] =?UTF-8?q?without=20spot=20=F0=9F=98=80=F0=9F=98=80?= =?UTF-8?q?=F0=9F=98=80=F0=9F=98=80=F0=9F=98=80=F0=9F=98=80=F0=9F=98=80?= =?UTF-8?q?=F0=9F=98=80=F0=9F=98=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../team2412/robot/commands/launcher/SetPivotCommand.java | 2 +- .../frc/team2412/robot/subsystems/LauncherSubsystem.java | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java index 96d27e3c..482a2111 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java @@ -17,7 +17,7 @@ public SetPivotCommand(LauncherSubsystem launcherSubsystem, double angle) { @Override public void initialize() { launcherSubsystem.setAngle(angle); - } + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 59748a31..f01e93a2 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -211,8 +211,7 @@ public void setAngle(double launcherAngle) { Units.degreesToRotations(angleSetpoint), ControlType.kPosition, 0, - launcherAngleFF.calculate( - Units.degreesToRadians(launcherAngle - FF_PIVOT_OFFSET), 0)); + launcherAngleFF.calculate(Units.degreesToRadians(launcherAngle - FF_PIVOT_OFFSET), 0)); manualAngleSetpoint = launcherAngle; // launcherAngleTwoPIDController.setReference( // Units.degreesToRotations(angleSetpoint), ControlType.kPosition); @@ -249,7 +248,9 @@ public void setAngleManual(double joystickInput) { ControlType.kPosition, 0, launcherAngleFF.calculate( - Units.degreesToRadians(Units.rotationsToDegrees(manualAngleSetpoint) - FF_PIVOT_OFFSET), 0)); + Units.degreesToRadians( + Units.rotationsToDegrees(manualAngleSetpoint) - FF_PIVOT_OFFSET), + 0)); } } From b89296b14a114b0ef3c5d6ba64e1ee9d69a5a924 Mon Sep 17 00:00:00 2001 From: iamawesomecat Date: Sun, 17 Mar 2024 16:34:06 -0700 Subject: [PATCH 09/11] fixes pt.2 --- src/main/java/frc/team2412/robot/Controls.java | 4 ++-- src/main/java/frc/team2412/robot/util/TrapAlign.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 0c792405..a5a2dee8 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -64,8 +64,8 @@ public Controls(Subsystems s) { launcherSubwooferPresetButton = codriveController.a(); launcherLowerPresetButton = codriveController.y(); // launcherPodiumPresetButton = codriveController.povLeft(); - launcherTrapPresetButton = codriveController.y(); - launcherLaunchButton = codriveController.leftBumper(); + launcherTrapPresetButton = codriveController.rightTrigger(); + launcherLaunchButton = codriveController.rightBumper(); // intake controls (confirmed with driveteam) driveIntakeInButton = driveController.a(); driveIntakeStopButton = driveController.b(); diff --git a/src/main/java/frc/team2412/robot/util/TrapAlign.java b/src/main/java/frc/team2412/robot/util/TrapAlign.java index 089c75f3..5b921fba 100644 --- a/src/main/java/frc/team2412/robot/util/TrapAlign.java +++ b/src/main/java/frc/team2412/robot/util/TrapAlign.java @@ -40,7 +40,7 @@ public class TrapAlign { private static Command trapAlign(DrivebaseSubsystem drivebaseSubsystem) { Pose2d robotPose = drivebaseSubsystem.getPose(); boolean isBlue; - if (DriverStation.getAlliance().isEmpty()) { + if (!DriverStation.getAlliance().isEmpty()) { isBlue = DriverStation.getAlliance().get().equals(Alliance.Blue); } else { isBlue = false; From 9692a60f61bac3ff7f40467334b92e39017324a5 Mon Sep 17 00:00:00 2001 From: iamawesomecat Date: Mon, 18 Mar 2024 17:50:15 -0700 Subject: [PATCH 10/11] 3/17/24 testing changes --- .../frc/team2412/robot/subsystems/IntakeSubsystem.java | 8 ++++++-- .../frc/team2412/robot/subsystems/LauncherSubsystem.java | 4 ++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 886b5d14..f3c2cdce 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -166,9 +166,13 @@ private void resetMotors() { configureMotor(intakeMotorRight, true); configureMotor(ingestMotor, false); - configureMotor(indexMotorUpper, 40, true); + configureMotor(indexMotorUpper, 40, false); - configureMotor(feederMotor, 40, true); + configureMotor(feederMotor, 40, false); + indexMotorUpper + .getForwardLimitSwitch(com.revrobotics.SparkLimitSwitch.Type.kNormallyOpen) + .enableLimitSwitch(false); + indexMotorUpper.burnFlash(); } public void intakeSet(double speed) { diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index f01e93a2..ed91fd37 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -134,8 +134,8 @@ public void configMotors() { launcherAngleOneMotor.setSmartCurrentLimit(60); launcherAngleTwoMotor.setSmartCurrentLimit(60); - launcherAngleOneMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, 0.95f); - launcherAngleOneMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, 0.71f); + launcherAngleOneMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, 0.96f); + launcherAngleOneMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, 0.80f); launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); From 94c6e58f9f04c7b8974f489b4e0c7a47c884619b Mon Sep 17 00:00:00 2001 From: iamawesomecat Date: Mon, 18 Mar 2024 18:05:17 -0700 Subject: [PATCH 11/11] fixed limits smore --- .../frc/team2412/robot/subsystems/LauncherSubsystem.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 721996b4..524568eb 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -249,10 +249,10 @@ public double getAngleSpeed() { public void setAngleManual(double joystickInput) { manualAngleSetpoint = - MathUtil.clamp(manualAngleSetpoint + joystickInput * MANUAL_MODIFIER, 0.71f, 0.95f); + MathUtil.clamp(manualAngleSetpoint + joystickInput * MANUAL_MODIFIER, 0.80f, 0.96f); - if (Units.degreesToRotations(getAngle()) > 0.71 - && Units.degreesToRotations(getAngle()) < 0.95) { + if (Units.degreesToRotations(getAngle()) > 0.80 + && Units.degreesToRotations(getAngle()) < 0.96) { launcherAngleOnePIDController.setReference( manualAngleSetpoint, ControlType.kPosition,