From 83a38c2d2eff83be3200a6fd133e0af4775390a9 Mon Sep 17 00:00:00 2001 From: EmptyDreadbot Date: Sat, 28 Mar 2026 14:28:17 -0400 Subject: [PATCH 1/2] Made the relative intake speed and dot product functions. Used this to make relative intake speed for robot --- .../robot/subsystems/slapdown/Slapdown.java | 38 ++++++++++++++++++- 1 file changed, 36 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/slapdown/Slapdown.java b/src/main/java/frc/robot/subsystems/slapdown/Slapdown.java index a9160c2..2d55627 100644 --- a/src/main/java/frc/robot/subsystems/slapdown/Slapdown.java +++ b/src/main/java/frc/robot/subsystems/slapdown/Slapdown.java @@ -4,6 +4,8 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; @@ -13,8 +15,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants.SlapdownConstants; +import frc.robot.subsystems.drive.DriveConstants; public class Slapdown extends SubsystemBase { + Twist2d positions = new Twist2d(); + ChassisSpeeds speeds = new ChassisSpeeds(); private final SlapdownIOInputsAutoLogged inputs = new SlapdownIOInputsAutoLogged(); private final SlapdownIO io; @@ -64,7 +69,7 @@ public Command intakeCommand() { return Commands.runEnd( () -> { if (canRunIntake()) { - io.runIntakeVoltage(SlapdownConstants.INTAKE_VOLTAGE); + io.runIntakeVoltage(relativeIntakeSpeed()); } }, () -> io.runIntakeVoltage(0.0) @@ -143,4 +148,33 @@ public Command setAngleDegrees(double angle) { public double getAngle() { return inputs.absolutePosition; } -} \ No newline at end of file + + public double getDotProduct() { + double headingAngle = positions.dtheta; + double xVelo = speeds.vxMetersPerSecond; + double yVelo = speeds.vyMetersPerSecond; + + double dotProduct = Math.cos(headingAngle) * xVelo + Math.sin(headingAngle) * yVelo; + return dotProduct; + } + + public double relativeIntakeSpeed() { + // i is the intake voltage + // v is the dot product range + double dotProduct = getDotProduct(); + double iMin = SlapdownConstants.INTAKE_VOLTAGE/2; + double iMax = SlapdownConstants.INTAKE_VOLTAGE; + double vMin = 0; + double vMax = DriveConstants.maxSpeedMetersPerSec;; + double i; + + if (dotProduct < vMin){ + i = iMin; + } else if (dotProduct > vMax) { + i = iMax; + } else { + i = ((dotProduct - vMin)/(vMax - vMin))*(iMax - iMin) + iMin; + } + return i; + } + } From 460b197e3cbaae03e516aa3db59fb282de04f343 Mon Sep 17 00:00:00 2001 From: jeejDev <156706451+Butter-Stik@users.noreply.github.com> Date: Fri, 3 Apr 2026 18:12:14 -0400 Subject: [PATCH 2/2] fixed robot relative --- simgui-ds.json | 8 ++++--- src/main/java/frc/robot/BuildConstants.java | 12 +++++----- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 6 ++--- .../robot/subsystems/slapdown/Slapdown.java | 22 +++++++++++++------ .../subsystems/slapdown/SlapdownIOSim.java | 4 ++-- 6 files changed, 33 insertions(+), 23 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 6d1455f..d9b1f67 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -99,13 +99,15 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true }, { - "guid": "Keyboard1" + "guid": "78696e70757401000000000000000000", + "useGamepad": true }, { - "guid": "Keyboard2" + "useGamepad": true } ] } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 4b1ea64..7b0009f 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Dreadbots2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 320; - public static final String GIT_SHA = "947c63f762dc7da313d3accb4446bedf85d3da40"; - public static final String GIT_DATE = "2026-03-18 18:28:57 EDT"; - public static final String GIT_BRANCH = "dev"; - public static final String BUILD_DATE = "2026-03-27 16:31:19 EDT"; - public static final long BUILD_UNIX_TIME = 1774643479924L; + public static final int GIT_REVISION = 325; + public static final String GIT_SHA = "83a38c2d2eff83be3200a6fd133e0af4775390a9"; + public static final String GIT_DATE = "2026-03-28 14:28:17 EDT"; + public static final String GIT_BRANCH = "Mathew-IntakeSpeedBasedOnRobotSpeed"; + public static final String BUILD_DATE = "2026-04-03 18:01:51 EDT"; + public static final long BUILD_UNIX_TIME = 1775253711273L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8041b2a..3e0e0cd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -143,8 +143,8 @@ public static class SlapdownConstants { public static final double KG = 0.0; public static final double MAX_VELOCITY = 540; public static final double MAX_ACCELERATION = 540; - public static final int SLAPDOWN_GEARING = 1; - public static final int INTAKE_GEARING = 1; + public static final int SLAPDOWN_GEARING = 27; + public static final double INTAKE_GEARING = 1.4; public static final double ARM_LENGTH = Units.inchesToMeters(11.75); public static final double MIN_ANGLE_RAD = Units.degreesToRadians(-20); public static final double MAX_ANGLE_RAD = Units.degreesToRadians(131.5); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 51bc3d5..ca5e0c4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -99,7 +99,7 @@ public RobotContainer() { flywheel = new Flywheel(new FlywheelIOSparkFlex(), turret); hood = new Hood(new HoodIOSparkMax()); indexer = new Indexer(new IndexerIOSparkFlex(), operator); - slapdown = new Slapdown(new SlapdownIOSparkFlex()); + slapdown = new Slapdown(new SlapdownIOSparkFlex(), drive::getChassisSpeeds); climb = new Climb(new ClimbIOSparkFlex(), slapdown); break; case SIM: @@ -120,7 +120,7 @@ public RobotContainer() { flywheel = new Flywheel(new FlywheelIOSim(), turret); hood = new Hood(new HoodIOSim()); indexer = new Indexer(new IndexerIOSim(), operator); - slapdown = new Slapdown(new SlapdownIOSim()); + slapdown = new Slapdown(new SlapdownIOSim(), drive::getChassisSpeeds); climb = new Climb(new ClimbIOSim(), slapdown); break; @@ -140,7 +140,7 @@ public RobotContainer() { flywheel = new Flywheel(new FlywheelIOSparkFlex(), turret); hood = new Hood(new HoodIOSparkMax()); indexer = new Indexer(new IndexerIOSparkFlex(), operator); - slapdown = new Slapdown(new SlapdownIOSparkFlex()); + slapdown = new Slapdown(new SlapdownIOSparkFlex(), drive::getChassisSpeeds); climb = new Climb(new ClimbIOSparkFlex(), slapdown); break; } diff --git a/src/main/java/frc/robot/subsystems/slapdown/Slapdown.java b/src/main/java/frc/robot/subsystems/slapdown/Slapdown.java index 2d55627..f799ffa 100644 --- a/src/main/java/frc/robot/subsystems/slapdown/Slapdown.java +++ b/src/main/java/frc/robot/subsystems/slapdown/Slapdown.java @@ -1,9 +1,15 @@ package frc.robot.subsystems.slapdown; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + import org.littletonrobotics.junction.Logger; +import org.opencv.core.RotatedRect; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -18,9 +24,6 @@ import frc.robot.subsystems.drive.DriveConstants; public class Slapdown extends SubsystemBase { - Twist2d positions = new Twist2d(); - ChassisSpeeds speeds = new ChassisSpeeds(); - private final SlapdownIOInputsAutoLogged inputs = new SlapdownIOInputsAutoLogged(); private final SlapdownIO io; public final PIDController pid = new PIDController(SlapdownConstants.KP, SlapdownConstants.KI, @@ -39,8 +42,11 @@ public class Slapdown extends SubsystemBase { private TrapezoidProfile.State goal = new TrapezoidProfile.State(0, 0); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); - public Slapdown(SlapdownIO io) { + private final Supplier speedsSupplier; + + public Slapdown(SlapdownIO io, Supplier speedsSupplier) { this.io = io; + this.speedsSupplier = speedsSupplier; SmartDashboard.putData("SlapdownPID", pid); } @@ -150,11 +156,12 @@ public double getAngle() { } public double getDotProduct() { - double headingAngle = positions.dtheta; + ChassisSpeeds speeds = speedsSupplier.get(); + double xVelo = speeds.vxMetersPerSecond; double yVelo = speeds.vyMetersPerSecond; - double dotProduct = Math.cos(headingAngle) * xVelo + Math.sin(headingAngle) * yVelo; + double dotProduct = xVelo + 0.5 * Math.abs(yVelo); return dotProduct; } @@ -175,6 +182,7 @@ public double relativeIntakeSpeed() { } else { i = ((dotProduct - vMin)/(vMax - vMin))*(iMax - iMin) + iMin; } + Logger.recordOutput("Intake/relativeVoltage", i); return i; } - } +} diff --git a/src/main/java/frc/robot/subsystems/slapdown/SlapdownIOSim.java b/src/main/java/frc/robot/subsystems/slapdown/SlapdownIOSim.java index a6d5ba3..43ea256 100644 --- a/src/main/java/frc/robot/subsystems/slapdown/SlapdownIOSim.java +++ b/src/main/java/frc/robot/subsystems/slapdown/SlapdownIOSim.java @@ -28,8 +28,8 @@ public SlapdownIOSim() { Units.degreesToRadians(0) ); this.intakeMotor = new DCMotorSim( - LinearSystemId.createDCMotorSystem(DCMotor.getNEO(1), SlapdownConstants.SIM_INTAKE_MOI, SlapdownConstants.INTAKE_GEARING), - DCMotor.getNEO(1) + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60Foc(1), SlapdownConstants.SIM_INTAKE_MOI, SlapdownConstants.INTAKE_GEARING), + DCMotor.getKrakenX60Foc(1) ); pivotVolts = 0.0; intakeVolts = 0.0;