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 a9160c2..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,17 @@ 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; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; @@ -13,9 +21,9 @@ 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 { - private final SlapdownIOInputsAutoLogged inputs = new SlapdownIOInputsAutoLogged(); private final SlapdownIO io; public final PIDController pid = new PIDController(SlapdownConstants.KP, SlapdownConstants.KI, @@ -34,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); } @@ -64,7 +75,7 @@ public Command intakeCommand() { return Commands.runEnd( () -> { if (canRunIntake()) { - io.runIntakeVoltage(SlapdownConstants.INTAKE_VOLTAGE); + io.runIntakeVoltage(relativeIntakeSpeed()); } }, () -> io.runIntakeVoltage(0.0) @@ -143,4 +154,35 @@ public Command setAngleDegrees(double angle) { public double getAngle() { return inputs.absolutePosition; } -} \ No newline at end of file + + public double getDotProduct() { + ChassisSpeeds speeds = speedsSupplier.get(); + + double xVelo = speeds.vxMetersPerSecond; + double yVelo = speeds.vyMetersPerSecond; + + double dotProduct = xVelo + 0.5 * Math.abs(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; + } + 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;