From c1a2ebfb79e6b30fafbca52f63f1f73c2d7a6f7e Mon Sep 17 00:00:00 2001 From: KPatel008 <122328545+KPatel008@users.noreply.github.com> Date: Thu, 14 Mar 2024 22:02:08 -0400 Subject: [PATCH 1/2] fix setpoints --- src/main/java/frc/robot/Robot.java | 15 ++++++++------- src/main/java/frc/robot/Superstructure.java | 9 ++++----- src/main/java/frc/robot/arm/ArmIOTalonFX.java | 1 + 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b012ef1..430cf7b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.arm.Arm; -import frc.robot.arm.ArmSetpoints; import frc.robot.climber.Climber; import frc.robot.drivetrain.Drivetrain; import frc.robot.feeder.Feeder; @@ -110,7 +109,7 @@ public Robot() { NamedCommands.registerCommand("feederOnTest", m_feeder.feed()); NamedCommands.registerCommand("feederOff", m_feeder.idle().withTimeout(1)); NamedCommands.registerCommand("sensorIntake", Superstructure.sensorIntake(m_feeder, m_intake)); - NamedCommands.registerCommand("armDown", m_arm.goToSetpoint(ArmSetpoints.kStowed)); + NamedCommands.registerCommand("armDown", m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); m_autoChooser = new LoggedDashboardChooser<>("Auto Chooser", AutoBuilder.buildAutoChooser()); m_autoChooser.addOption( @@ -182,21 +181,23 @@ public Robot() { .leftTrigger() .whileTrue( Commands.parallel( - m_intake.feed(), m_shooter.ampShot(), m_arm.goToSetpoint(ArmSetpoints.kAmp))) - .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed)); - m_operatorController.rightTrigger().onTrue(m_arm.goToSetpoint(ArmSetpoints.kStowed)); + m_intake.feed(), + m_shooter.ampShot(), + m_arm.goToSetpoint(1.49 + 0.0873, -2.307, 0.0, 0.0))) + .onFalse(m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); + m_operatorController.rightTrigger().onTrue(m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); m_operatorController .povUp() .onTrue( Commands.parallel(m_arm.goToSetpoint(-0.52, 2.083, 0.0, 0.0), m_shooter.runShooter())) - .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed)); + .onFalse(m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); m_operatorController.y().whileTrue(m_shooter.runShooter()); m_operatorController.a().whileTrue(m_shooter.ampShot()); m_operatorController .leftBumper() .onTrue(Superstructure.sensorCatch(m_shooter, m_feeder, m_intake, m_arm)) - .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed)); + .onFalse(m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); } /** diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index ea18906..b2a73dc 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.arm.Arm; -import frc.robot.arm.ArmSetpoints; import frc.robot.drivetrain.Drivetrain; import frc.robot.feeder.Feeder; import frc.robot.intake.Intake; @@ -36,8 +35,9 @@ public static Command sensorCatch(Shooter shooter, Feeder feeder, Intake intake, } public static Command ampShot(Arm arm, Shooter shooter) { - return Commands.parallel(arm.goToSetpoint(ArmSetpoints.kAmp), shooter.runShooter()) - .finallyDo(() -> arm.goToSetpoint(ArmSetpoints.kStowed)); + return Commands.parallel( + arm.goToSetpoint(1.49 + 0.0873, -2.307, 0.0, 0.0), shooter.runShooter()) + .finallyDo(() -> arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); } public static Command spit(Shooter shooter, Feeder feeder, Intake intake) { @@ -52,8 +52,7 @@ public static Command aimAtGoal(Drivetrain drivetrain, Shooter shooter, Lights l () -> PoseEstimation.getInstance().getAimingParameters().driveHeading()), drivetrain::clearHeadingGoal), shooter.runShooter(), - lights.showReadyToShootStatus( - drivetrain.atHeadingGoal.and(shooter.readyToShoot).and(drivetrain.inRangeOfGoal))) + lights.showReadyToShootStatus(drivetrain.atHeadingGoal.and(shooter.readyToShoot))) .finallyDo(() -> lights.getCurrentCommand().cancel()) .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); } diff --git a/src/main/java/frc/robot/arm/ArmIOTalonFX.java b/src/main/java/frc/robot/arm/ArmIOTalonFX.java index 8e1e7e2..0d8a0d1 100644 --- a/src/main/java/frc/robot/arm/ArmIOTalonFX.java +++ b/src/main/java/frc/robot/arm/ArmIOTalonFX.java @@ -70,6 +70,7 @@ public ArmIOTalonFX() { elbowLeftMotorConfig.CurrentLimits = currentLimitsConfig; elbowLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + elbowLeftMotorConfig.Voltage.PeakReverseVoltage = -4.0; m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig); final TalonFXConfiguration wristLeftMotorConfig = new TalonFXConfiguration(); From ddf3f5f29f7eb6f33086c9042f2be2aa1d8f8306 Mon Sep 17 00:00:00 2001 From: KPatel008 <122328545+KPatel008@users.noreply.github.com> Date: Sat, 16 Mar 2024 08:18:00 -0400 Subject: [PATCH 2/2] make uri changes --- build.gradle | 2 +- src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java | 5 +++-- .../frc/robot/drivetrain/PhoenixOdometryThread.java | 3 ++- src/main/java/frc/robot/shooter/Shooter.java | 2 +- vendordeps/AdvantageKit.json | 10 +++++----- vendordeps/photonlib.json | 10 +++++----- 6 files changed, 17 insertions(+), 15 deletions(-) diff --git a/build.gradle b/build.gradle index 8f49c33..6c233b5 100755 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" } diff --git a/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java b/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java index df8726f..0d703ca 100644 --- a/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java @@ -17,8 +17,9 @@ public class ImuIOPigeon2 implements ImuIO { private final Queue m_yawPositionQueue, m_yawTimestampQueue; public ImuIOPigeon2() { - m_imu.getConfigurator().apply(new Pigeon2Configuration()); - m_imu.getConfigurator().setYaw(0.0); + final Pigeon2Configuration imuConfig = new Pigeon2Configuration(); + m_imu.getConfigurator().apply(imuConfig); + m_imu.setYaw(0.0); m_yawSignal.setUpdateFrequency(PhoenixOdometryThread.kOdometryFrequencyHz); m_yawVelocitySignal.setUpdateFrequency(100.0); m_imu.optimizeBusUtilization(); diff --git a/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java b/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java index 9456340..6fa3d7e 100644 --- a/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java @@ -9,6 +9,7 @@ import java.util.concurrent.ArrayBlockingQueue; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; public class PhoenixOdometryThread extends Thread { @@ -71,7 +72,7 @@ public void run() { while (true) { m_signalsLock.lock(); try { - BaseStatusSignal.waitForAll(2.0 / kOdometryFrequencyHz, m_signals); + BaseStatusSignal.waitForAll(LoggedRobot.defaultPeriodSecs, m_signals); } finally { m_signalsLock.unlock(); } diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index 706e2ea..c3832e2 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -14,7 +14,7 @@ public class Shooter extends SubsystemBase { - private final double kFarShotVelocityRpm = 6000.0; + private final double kFarShotVelocityRpm = 5600.0; private final double kAmpshot = 5000.0; private final double kReadyToShootToleranceRps = 3.0; diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 1b8f4df..55a634d 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.1.1", + "version": "3.2.0", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.1.1" + "version": "3.2.0" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.1.1" + "version": "3.2.0" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.1.1" + "version": "3.2.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.1.1", + "version": "3.2.0", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 5850529..cba2533 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.9", + "version": "v2024.2.10", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.9", + "version": "v2024.2.10", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.9", + "version": "v2024.2.10", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.9" + "version": "v2024.2.10" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.9" + "version": "v2024.2.10" } ] }