diff --git a/src/main/deploy/pathplanner/paths/TurnNinety.path b/src/main/deploy/pathplanner/paths/TurnNinety.path index e75bda6..41bceba 100644 --- a/src/main/deploy/pathplanner/paths/TurnNinety.path +++ b/src/main/deploy/pathplanner/paths/TurnNinety.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 2.42775, + "y": 4.0055 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 3.4277499999999996, + "y": 4.0055 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 2.42775, + "y": 4.0055 }, "prevControl": { - "x": 1.0, - "y": 7.0 + "x": 1.42775, + "y": 4.0055 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 334d3c4..f565921 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -119,6 +119,9 @@ public class RobotContainer { DeferredBlock.addBlock(() -> { TimesNegativeOne.update(); FieldPositions.update(); + + m_robotIntake.io.updateGains(); + m_robotShooter.io.updateGains(); }, true); @@ -269,7 +272,16 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> { m_robotShooter.setShooterReady(); + m_robotIntake.setMode(IntakeMode.Idle); })); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotShooter.setShooterShoot(); + })).onFalse(new InstantCommand(() -> { + m_robotShooter.setShooterNOTShoot(); + })); + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> { diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index f18e71c..a85bcb5 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 71; - public static final String GIT_SHA = "32ede81ffaf43d4e730ff646db98627385e80ba1"; - public static final String GIT_DATE = "2026-02-20 16:24:05 MST"; + public static final int GIT_REVISION = 72; + public static final String GIT_SHA = "9c7159ba3b2fd669f9781f1315cb481b7a1b8d57"; + public static final String GIT_DATE = "2026-02-20 21:59:06 MST"; public static final String GIT_BRANCH = "operator-controls"; - public static final String BUILD_DATE = "2026-02-20 21:42:37 MST"; - public static final long BUILD_UNIX_TIME = 1771648957538L; + public static final String BUILD_DATE = "2026-02-21 13:54:02 MST"; + public static final long BUILD_UNIX_TIME = 1771707242280L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index b596e5a..2231807 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -78,10 +78,10 @@ public final class Constants { public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; public static final Transform3d LEFT_CAMERA_POS = new Transform3d( - new Translation3d(Units.inchesToMeters(-13-9.134-1.5), -Units.inchesToMeters(10.75), Units.inchesToMeters(9.5) + new Translation3d(Units.inchesToMeters(-13-9.134), -Units.inchesToMeters(10.75), Units.inchesToMeters(9.5) ), new Rotation3d(0,25.*(Math.PI/180.),Math.PI)); public static final Transform3d RIGHT_CAMERA_POS = new Transform3d( - new Translation3d(Units.inchesToMeters(-13-9.134-1.5), Units.inchesToMeters(10.75), Units.inchesToMeters(9.5)), + new Translation3d(Units.inchesToMeters(-13-9.134), Units.inchesToMeters(10.75), Units.inchesToMeters(9.5)), new Rotation3d(0,25.*(Math.PI/180.),Math.PI) ); @@ -99,7 +99,7 @@ public final class Constants { public static final class LEDConstants { public static final int LED_SPARK_ID = 9; - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.SOLID_RED_ORANGE; + public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_TWINKLES; // LED color for when the intake is out public static final LEDPatterns INTAKE_OUT = LEDPatterns.SOLID_RED; diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 55986e5..28e1dcf 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -45,7 +45,7 @@ public class LED extends SubsystemBase implements Queryable { // Don't stall the main thread every time the setMode function is called if(this.mode != pattern) { this.mode = pattern; - setTo5V(); + update(); } } diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index e9e5868..99c06fc 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -31,7 +31,7 @@ public class Intake extends SubsystemBase { Idle } - private IntakeMode mode = IntakeMode.Retracted; + private IntakeMode mode = IntakeMode.Idle; public void setMode(IntakeMode mode) { this.mode = mode; @@ -67,27 +67,27 @@ public class Intake extends SubsystemBase { io.updateInputs(state); - // switch (mode) { - // case Extended: - // io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); - // io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); - // break; - // case Retracted: - // io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); - // io.setRollerOutput(state, 0); - // break; - // case Extending: - // io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); - // io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); - // break; - // case Retracting: - // io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); - // io.setRollerOutput(state, 0); - // break; - // case Idle: - // io.stopArm(); - // break; - // } + switch (mode) { + case Extended: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); + io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); + break; + case Retracted: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); + io.setRollerOutput(state, 0); + break; + case Extending: + io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); + io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); + break; + case Retracting: + io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); + io.setRollerOutput(state, 0); + break; + case Idle: + io.stopArm(); + break; + } // if (state.retractedLimit){ // this.mode = IntakeMode.Retracted; // } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 94b5289..b46648a 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -33,7 +33,7 @@ public class IntakeConstants { // public static final Angle ARM_LIMIT_LOWER = Degrees.of(90); // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); - public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0); + public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0.1); public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.3); public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.2); public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.4); @@ -45,13 +45,13 @@ public class IntakeConstants { public static final Slot0Configs ARM_PID = new Slot0Configs() - .withKP(0.2) + .withKP(0.1) .withKI(0.0) .withKD(0.0); - public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.1); + public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.05); public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0); public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index 9ea9072..e096062 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -83,6 +83,7 @@ public class IntakeReal implements IntakeIO { .withPosition(motorAngle) .withLimitReverseMotion(!m_armLimitSwitch.get()) ); + } @Override diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 24b1c27..8e1c255 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -61,6 +61,7 @@ public class Shooter extends SubsystemBase { } private ShooterMode mode = ShooterMode.NotReady; + private boolean shooterButtonReady = false; public void setShooterReady() { if(this.mode == ShooterMode.NotReady) { @@ -72,6 +73,15 @@ public class Shooter extends SubsystemBase { this.mode = ShooterMode.NotReady; } + public void setShooterShoot() { + shooterButtonReady = true; + } + + + public void setShooterNOTShoot() { + shooterButtonReady = false; + } + @AutoLogOutput public ShooterMode getMode() { return mode; @@ -100,11 +110,11 @@ public class Shooter extends SubsystemBase { if(this.mode != ShooterMode.NotReady) { // TODO: get if the robot is within the angle of the hub - boolean driverError = false; + boolean driverError = // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | - // distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | - // distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get(); + distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | + distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get(); double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; double shooterSpeedTarget = Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond) + state.motor2TargetVelocity.in(RotationsPerSecond)) / 2; @@ -148,16 +158,23 @@ public class Shooter extends SubsystemBase { ShooterMode.Ready ); - } + } + + switch (mode) { case Shooting: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); - io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); + if(shooterButtonReady) { + io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); + io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); + } else { + io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + } break; case Ready: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); + io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); break; case NotReady: diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index f6ff699..22ef12a 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -18,7 +18,7 @@ public class ShooterConstants { public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5; public static final double INDEXER_GEAR_RATIO = 1.; - public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40); + // public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40); public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60); public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); @@ -28,23 +28,26 @@ public class ShooterConstants { public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); // Shoot mode tolerances - public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 0); - public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 99); + public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); + public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8); public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360); public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1); public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance DEG", 3); - public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 5); + public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3); // public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) { // Model derived from points + // double speed = + // 1.11576*hubDistMeters*hubDistMeters + + // 0.318464*hubDistMeters + + // 30.6293; double speed = - 1.11576*hubDistMeters*hubDistMeters + - 0.318464*hubDistMeters + - 30.6293; + 5.6939*hubDistMeters + + 22.76545; double max = SHOOTER_MAX_VELOCITY.get(); @@ -54,6 +57,8 @@ public class ShooterConstants { } else if(speed < -max) { speed = -max; } + + // double speed = SHOOTER_MAX_VELOCITY.get(); return RotationsPerSecond.of(-speed); } @@ -62,7 +67,7 @@ public class ShooterConstants { public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) .withKP(0.08) - .withKI(0.1) + .withKI(0.15) .withKD(0.0); @@ -71,7 +76,7 @@ public class ShooterConstants { public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08); - public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.1); + public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15); public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index cb7fcd1..9e0b918 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -50,7 +50,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public double lastOdomSpeed; - public Pose2d initalPose2d = new Pose2d(); + public Pose2d initalPose2d = new Pose2d(); // Mira aqui public double rotTarget = 0.0; @@ -330,7 +330,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { Rotation2d heading = new Rotation2d(rightStick.getX(), rightStick.getY());//.r otateBy(Rotation2d.fromDegrees(90)); - heading = heading.rotateBy(Rotation2d.fromDegrees(90)); + heading = heading.rotateBy(Rotation2d.fromDegrees(-90)); rotTarget = heading.getDegrees(); driveFieldAngle(leftStick, heading);