From 9ac1d206ec2987898ae354d94eeefcdc96e03769 Mon Sep 17 00:00:00 2001 From: Rowan Flood Date: Wed, 21 Feb 2024 22:35:37 -0600 Subject: [PATCH 01/22] changes to april tags to hopefully filter positions better --- src/main/java/frc/robot/settings/Constants.java | 4 +++- .../java/frc/robot/settings/LimelightFiducialData.java | 2 +- src/main/java/frc/robot/settings/LimelightValues.java | 10 ++++++++-- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 2e6046d3..5ad04491 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -426,7 +426,9 @@ public final class Vision{ public static final String APRILTAG_LIMELIGHT2_NAME = "limelight-aprill"; public static final String APRILTAG_LIMELIGHT3_NAME = "limelight-aprilr"; public static final String OBJ_DETECITON_LIMELIGHT_NAME = "limelight-neural"; - + + public static final double APRILTAG_CLOSENESS = 0.5; + public static final double MAX_TAG_DISTANCE = 4; public static final Translation2d fieldCorner = new Translation2d(16.54, 8.02); diff --git a/src/main/java/frc/robot/settings/LimelightFiducialData.java b/src/main/java/frc/robot/settings/LimelightFiducialData.java index c035b431..f72a829d 100644 --- a/src/main/java/frc/robot/settings/LimelightFiducialData.java +++ b/src/main/java/frc/robot/settings/LimelightFiducialData.java @@ -35,7 +35,7 @@ public Pose2d getbotPose(){ * @return true if the most recent vision pose estimation is inside the field and is close enough to the provided pose. */ public boolean isPoseTrustworthy(Pose2d robotPose){ - Pose2d poseEstimate = this.getbotPose(); + Pose2d poseEstimate = this.botPoseBlue; if ((poseEstimate.getX() Date: Wed, 21 Feb 2024 22:40:49 -0600 Subject: [PATCH 02/22] fixed compilation error --- .../frc/robot/settings/LimelightValues.java | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/settings/LimelightValues.java b/src/main/java/frc/robot/settings/LimelightValues.java index c02bfda5..8404cd3b 100644 --- a/src/main/java/frc/robot/settings/LimelightValues.java +++ b/src/main/java/frc/robot/settings/LimelightValues.java @@ -11,6 +11,9 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.LimelightHelpers; import frc.robot.LimelightHelpers.Results; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import static frc.robot.settings.Constants.Vision.MAX_TAG_DISTANCE; +import static frc.robot.settings.Constants.Vision.APRILTAG_CLOSENESS; import edu.wpi.first.networktables.NetworkTableInstance; @@ -41,9 +44,9 @@ public LimelightValues(Results llresults, boolean valid){ this.botPoseRed = llresults.getBotPose2d_wpiRed(); this.botPoseBlue = llresults.getBotPose2d_wpiBlue(); this.tagDistance = llresults.targets_Fiducials[0].getTargetPose_RobotSpace2D().getTranslation().getNorm(); - SmartDashboard.putNumber("VISION dist to apriltag [0]", tagDistance) - SmartDashboard.putNumber("VISION dist to apriltag [1]", llresults.targets_Fiducials[1].getTargetPose_RobotSpace2D().getTranslation().getNorm();) - SmartDashboard.putNumber("VISION dist to apriltag [2]", llresults.targets_Fiducials[2].getTargetPose_RobotSpace2D().getTranslation().getNorm();) + SmartDashboard.putNumber("VISION dist to apriltag [0]", tagDistance); + SmartDashboard.putNumber("VISION dist to apriltag [1]", llresults.targets_Fiducials[1].getTargetPose_RobotSpace2D().getTranslation().getNorm()); + SmartDashboard.putNumber("VISION dist to apriltag [2]", llresults.targets_Fiducials[2].getTargetPose_RobotSpace2D().getTranslation().getNorm()); } } public double gettx(int index){return tx[index];} @@ -63,10 +66,12 @@ public Pose2d getBotPoseBlue() { public boolean isPoseTrustworthy(Pose2d robotPose){ Pose2d poseEstimate = this.botPoseBlue; if ((poseEstimate.getX() Date: Thu, 22 Feb 2024 16:53:19 -0600 Subject: [PATCH 03/22] Speakre to Speaker --- src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java index cc9684df..b27aa250 100644 --- a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java @@ -99,7 +99,7 @@ public double calculateSpeakerAngle() { } double deltaY = Math.abs(dtvalues.getY() - Field.SPEAKER_Y); double speakerDist = Math.sqrt(Math.pow(deltaY, 2) + Math.pow(deltaX, 2)); - // SmartDashboard.putNumber("dist to speakre", speakerDist); + // SmartDashboard.putNumber("dist to speaker", speakerDist); Rotation2d unadjustedAngle = Rotation2d.fromRadians(Math.asin(deltaX/speakerDist)); double totalDistToSpeaker = Math.sqrt(Math.pow(Field.SPEAKER_Z-ShooterConstants.SHOOTER_HEIGHT, 2) + Math.pow(speakerDist, 2)); double shootingTime = totalDistToSpeaker / shootingSpeed; // calculates how long the note will take to reach the target From 5ec0ad7c03f8692dbd7ac504b0940b64ec0a59ad Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Tue, 20 Feb 2024 20:01:54 -0600 Subject: [PATCH 04/22] Added logging --- src/main/java/frc/robot/RobotContainer.java | 7 ++- vendordeps/URCL.json | 65 +++++++++++++++++++++ 2 files changed, 71 insertions(+), 1 deletion(-) create mode 100644 vendordeps/URCL.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 38cac0c3..be3076d9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,8 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import org.littletonrobotics.urcl.URCL; + import static frc.robot.settings.Constants.DriveConstants.*; import com.ctre.phoenix6.hardware.Pigeon2; @@ -77,6 +79,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.Angle; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PS4Controller; import frc.robot.commands.ManualShoot; @@ -138,7 +141,9 @@ public RobotContainer() { Preferences.initBoolean("Use Limelight", false); Preferences.initBoolean("Use 2 Limelights", false); Preferences.initDouble("wait # of seconds", 0); - + + DataLogManager.start(); + URCL.start(); driverController = new PS4Controller(DRIVE_CONTROLLER_ID); operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID); pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json new file mode 100644 index 00000000..998c2616 --- /dev/null +++ b/vendordeps/URCL.json @@ -0,0 +1,65 @@ +{ + "fileName": "URCL.json", + "name": "URCL", + "version": "2024.1.0", + "frcYear": "2024", + "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", + "mavenUrls": [ + "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.1.0" + ], + "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-cpp", + "version": "2024.1.0", + "libName": "URCL", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + }, + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "libName": "URCLDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From d3dfc3b4c0514632a5294bf0b9923fd645b495f5 Mon Sep 17 00:00:00 2001 From: NoMythic <2491nomythic@gmail.com> Date: Thu, 22 Feb 2024 17:05:54 -0600 Subject: [PATCH 05/22] Phoenix Logging --- src/main/java/frc/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index be3076d9..c38d1983 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import static frc.robot.settings.Constants.DriveConstants.*; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.hardware.Pigeon2; import com.fasterxml.jackson.core.sym.Name; import com.pathplanner.lib.auto.AutoBuilder; @@ -144,6 +145,8 @@ public RobotContainer() { DataLogManager.start(); URCL.start(); + SignalLogger.setPath("/media/sda1/ctre-logs/"); + SignalLogger.start(); driverController = new PS4Controller(DRIVE_CONTROLLER_ID); operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID); pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); From 27400b81b2fc34b91968967d8a171647bae79267 Mon Sep 17 00:00:00 2001 From: NoMythic <2491nomythic@gmail.com> Date: Thu, 22 Feb 2024 17:36:44 -0600 Subject: [PATCH 06/22] Power Usage to Smart Dashboard --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 15 ++++++++++++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 758db764..7562f56a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -45,6 +45,7 @@ public void robotPeriodic() { // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. + m_robotContainer.robotPeriodic(); CommandScheduler.getInstance().run(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c38d1983..33fa50dd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,6 +75,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import frc.robot.commands.Drive; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Translation2d; @@ -83,6 +84,7 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PS4Controller; +import edu.wpi.first.wpilibj.PowerDistribution; import frc.robot.commands.ManualShoot; import frc.robot.commands.AngleShooter; import frc.robot.commands.IntakeCommand; @@ -126,6 +128,8 @@ public class RobotContainer { private SendableChooser climbSpotChooser; private SendableChooser autoChooser; private DoubleSupplier angleSup; + private PowerDistribution PDP; + // Replace with CommandPS4Controller or CommandJoystick if needed /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -150,6 +154,7 @@ public RobotContainer() { driverController = new PS4Controller(DRIVE_CONTROLLER_ID); operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID); pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); + PDP = new PowerDistribution(0, ModuleType.kRev); // = new PathPlannerPath(null, DEFAUL_PATH_CONSTRAINTS, null, climberExists); limelightInit(); @@ -417,7 +422,15 @@ public void teleopPeriodic() { SmartDashboard.putBoolean("shooter in range", RobotState.getInstance().ShooterInRange); SmartDashboard.putBoolean("shooter ready", RobotState.getInstance().ShooterReady); } - + + public void logPower(){ + for(int i = 0; i < 16; i++) { + SmartDashboard.putNumber("PDP Current " + i, PDP.getCurrent(i)); + } + } + public void robotPeriodic() { + logPower(); + } public void disabledPeriodic() { } From e699f1827a30dca905060eed2ca59a662d9475c0 Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Thu, 22 Feb 2024 18:59:00 -0600 Subject: [PATCH 07/22] tuning for Amp --- .pathplanner/settings.json | 2 +- .../deploy/pathplanner/paths/ScoreAmp.path | 26 +++++++++---------- .../java/frc/robot/settings/Constants.java | 4 +-- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 3f82bc00..e6e704e4 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,6 +1,6 @@ { "robotWidth": 0.81, - "robotLength": 0.813, + "robotLength": 0.81, "holonomicMode": true, "pathFolders": [ "Collect Notes", diff --git a/src/main/deploy/pathplanner/paths/ScoreAmp.path b/src/main/deploy/pathplanner/paths/ScoreAmp.path index cf441a5e..872f02cd 100644 --- a/src/main/deploy/pathplanner/paths/ScoreAmp.path +++ b/src/main/deploy/pathplanner/paths/ScoreAmp.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.85, - "y": 6.7 + "x": 2.0, + "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 1.85, - "y": 6.889081499499318 + "x": 2.0, + "y": 7.189081499499318 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.8527875727163086, - "y": 7.7 + "x": 2.0, + "y": 7.9 }, "prevControl": { - "x": 1.8527875727163086, - "y": 7.6000000000000005 + "x": 2.0, + "y": 7.800000000000001 }, "nextControl": null, "isLocked": false, @@ -30,16 +30,16 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.0, + "waypointRelativePos": 0.25, "rotationDegrees": -90.0, - "rotateFast": false + "rotateFast": true } ], "constraintZones": [], "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.1, + "waypointRelativePos": 0.6, "command": { "type": "parallel", "data": { @@ -73,7 +73,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 2.0, "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 @@ -89,5 +89,5 @@ "rotation": -90.0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 2e6046d3..d7c1028e 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -270,9 +270,9 @@ public static final class ShooterConstants{ public static final double AUTO_AIM_ROBOT_kP = 0.125; public static final double AUTO_AIM_ROBOT_kI = 0.00; public static final double AUTO_AIM_ROBOT_kD = 0.00; - + public static final double SHOOTING_RPS = 90; - public static final double AMP_RPS = 9.5; + public static final double AMP_RPS = 6.5; //the PID values used on the PID loop on the motor controller that control the position of the shooter angle public static final double ANGLE_SHOOTER_POWER_KP = 0.026; From fa34e5f7de2840a4c8fa0423f44807afaa8d1d0b Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Thu, 22 Feb 2024 19:04:38 -0600 Subject: [PATCH 08/22] print tagDistance --- src/main/java/frc/robot/settings/LimelightValues.java | 9 ++++++--- .../java/frc/robot/subsystems/DrivetrainSubsystem.java | 6 ++++++ 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/settings/LimelightValues.java b/src/main/java/frc/robot/settings/LimelightValues.java index 8404cd3b..a692b7df 100644 --- a/src/main/java/frc/robot/settings/LimelightValues.java +++ b/src/main/java/frc/robot/settings/LimelightValues.java @@ -44,9 +44,9 @@ public LimelightValues(Results llresults, boolean valid){ this.botPoseRed = llresults.getBotPose2d_wpiRed(); this.botPoseBlue = llresults.getBotPose2d_wpiBlue(); this.tagDistance = llresults.targets_Fiducials[0].getTargetPose_RobotSpace2D().getTranslation().getNorm(); - SmartDashboard.putNumber("VISION dist to apriltag [0]", tagDistance); - SmartDashboard.putNumber("VISION dist to apriltag [1]", llresults.targets_Fiducials[1].getTargetPose_RobotSpace2D().getTranslation().getNorm()); - SmartDashboard.putNumber("VISION dist to apriltag [2]", llresults.targets_Fiducials[2].getTargetPose_RobotSpace2D().getTranslation().getNorm()); + // SmartDashboard.putNumber("VISION dist to apriltag [0]", tagDistance); + // SmartDashboard.putNumber("VISION dist to apriltag [1]", llresults.targets_Fiducials[1].getTargetPose_RobotSpace2D().getTranslation().getNorm()); + // SmartDashboard.putNumber("VISION dist to apriltag [2]", llresults.targets_Fiducials[2].getTargetPose_RobotSpace2D().getTranslation().getNorm()); } } public double gettx(int index){return tx[index];} @@ -63,6 +63,9 @@ public Pose2d getbotPose(){ public Pose2d getBotPoseBlue() { return botPoseBlue; } + public double getTagDistance() { + return tagDistance; + } public boolean isPoseTrustworthy(Pose2d robotPose){ Pose2d poseEstimate = this.botPoseBlue; if ((poseEstimate.getX() Date: Thu, 22 Feb 2024 16:53:19 -0600 Subject: [PATCH 09/22] Speakre to Speaker --- src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java index cc9684df..b27aa250 100644 --- a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java @@ -99,7 +99,7 @@ public double calculateSpeakerAngle() { } double deltaY = Math.abs(dtvalues.getY() - Field.SPEAKER_Y); double speakerDist = Math.sqrt(Math.pow(deltaY, 2) + Math.pow(deltaX, 2)); - // SmartDashboard.putNumber("dist to speakre", speakerDist); + // SmartDashboard.putNumber("dist to speaker", speakerDist); Rotation2d unadjustedAngle = Rotation2d.fromRadians(Math.asin(deltaX/speakerDist)); double totalDistToSpeaker = Math.sqrt(Math.pow(Field.SPEAKER_Z-ShooterConstants.SHOOTER_HEIGHT, 2) + Math.pow(speakerDist, 2)); double shootingTime = totalDistToSpeaker / shootingSpeed; // calculates how long the note will take to reach the target From f11063358668de2373c0877082cbc8105e3e7e67 Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Tue, 20 Feb 2024 20:01:54 -0600 Subject: [PATCH 10/22] Added logging --- src/main/java/frc/robot/RobotContainer.java | 7 ++- vendordeps/URCL.json | 65 +++++++++++++++++++++ 2 files changed, 71 insertions(+), 1 deletion(-) create mode 100644 vendordeps/URCL.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0188a2c1..ff78c915 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,8 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import org.littletonrobotics.urcl.URCL; + import static frc.robot.settings.Constants.DriveConstants.*; import com.ctre.phoenix6.hardware.Pigeon2; @@ -77,6 +79,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.Angle; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PS4Controller; import frc.robot.commands.ManualShoot; @@ -138,7 +141,9 @@ public RobotContainer() { Preferences.initBoolean("Use Limelight", false); Preferences.initBoolean("Use 2 Limelights", false); Preferences.initDouble("wait # of seconds", 0); - + + DataLogManager.start(); + URCL.start(); driverController = new PS4Controller(DRIVE_CONTROLLER_ID); operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID); pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json new file mode 100644 index 00000000..998c2616 --- /dev/null +++ b/vendordeps/URCL.json @@ -0,0 +1,65 @@ +{ + "fileName": "URCL.json", + "name": "URCL", + "version": "2024.1.0", + "frcYear": "2024", + "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", + "mavenUrls": [ + "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.1.0" + ], + "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-cpp", + "version": "2024.1.0", + "libName": "URCL", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + }, + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "libName": "URCLDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From aedb14cdc6f8a0e35a5a4407eac0f256922e9e79 Mon Sep 17 00:00:00 2001 From: NoMythic <2491nomythic@gmail.com> Date: Thu, 22 Feb 2024 17:05:54 -0600 Subject: [PATCH 11/22] Phoenix Logging --- src/main/java/frc/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ff78c915..fa3c1efb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import static frc.robot.settings.Constants.DriveConstants.*; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.hardware.Pigeon2; import com.fasterxml.jackson.core.sym.Name; import com.pathplanner.lib.auto.AutoBuilder; @@ -144,6 +145,8 @@ public RobotContainer() { DataLogManager.start(); URCL.start(); + SignalLogger.setPath("/media/sda1/ctre-logs/"); + SignalLogger.start(); driverController = new PS4Controller(DRIVE_CONTROLLER_ID); operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID); pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); From 14df64a248f6513faa91b82e50fa1952e741b6d6 Mon Sep 17 00:00:00 2001 From: Liam Sykes <78829557+trixydevs@users.noreply.github.com> Date: Wed, 21 Feb 2024 15:47:22 -0600 Subject: [PATCH 12/22] added manual intake, configured intake more --- src/main/java/frc/robot/settings/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 5ad04491..8ac9d117 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -327,7 +327,7 @@ public static final class ClimberConstants{ } public static final class IndexerConstants{ public static final int INDEXER_MOTOR = 11; - public static final double INDEXER_INTAKE_SPEED = -0.25;//should be 0.5 TODO change to positive + public static final double INDEXER_INTAKE_SPEED = 0.5;//should be 0.5 TODO change to positive public static final double HUMAN_PLAYER_INDEXER_SPEED = -0.5;//should be 0.5 TODO change to positive public static final double INDEXER_SHOOTING_SPEED = 1; } From 6f109b4fdfd601c1b8a529bc395f93cbb69d4f75 Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Thu, 22 Feb 2024 21:59:14 -0600 Subject: [PATCH 13/22] changed som econstants for amp shooting --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 10 +++++----- src/main/java/frc/robot/commands/IndexCommand.java | 11 +++++++++-- src/main/java/frc/robot/settings/Constants.java | 11 ++++++----- src/main/java/frc/robot/settings/LimelightValues.java | 2 +- 5 files changed, 22 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 758db764..1feb78e3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,6 +29,7 @@ public class Robot extends TimedRobot { public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. + Preferences.setBoolean("Intake", true); m_robotContainer = new RobotContainer(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fa3c1efb..a41b0dde 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -143,10 +143,10 @@ public RobotContainer() { Preferences.initBoolean("Use 2 Limelights", false); Preferences.initDouble("wait # of seconds", 0); - DataLogManager.start(); - URCL.start(); - SignalLogger.setPath("/media/sda1/ctre-logs/"); - SignalLogger.start(); + // DataLogManager.start(); + // URCL.start(); + // SignalLogger.setPath("/media/sda1/ctre-logs/"); + // SignalLogger.start(); driverController = new PS4Controller(DRIVE_CONTROLLER_ID); operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID); pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); @@ -212,7 +212,7 @@ private void indexInit() { indexer = new IndexerSubsystem(); } private void indexCommandInst() { - defaulNoteHandlingCommand = new IndexCommand(indexer, driverController::getR2Button, driverController::getL2Button, shooter, intake, driveTrain, angleShooterSubsystem, driverController::getR1Button); + defaulNoteHandlingCommand = new IndexCommand(indexer, driverController::getR2Button, driverController::getL2Button, shooter, intake, driveTrain, angleShooterSubsystem, driverController::getR1Button, driverController::getPOV); indexer.setDefaultCommand(defaulNoteHandlingCommand); } diff --git a/src/main/java/frc/robot/commands/IndexCommand.java b/src/main/java/frc/robot/commands/IndexCommand.java index a47b0040..e04143f3 100644 --- a/src/main/java/frc/robot/commands/IndexCommand.java +++ b/src/main/java/frc/robot/commands/IndexCommand.java @@ -30,6 +30,7 @@ public class IndexCommand extends Command { BooleanSupplier revUpSupplier; Boolean revUp; BooleanSupplier shootIfReadySupplier; + DoubleSupplier ampSupplier;; Boolean shootIfReady; // DoubleSupplier POVSupplier; BooleanSupplier humanPlayerSupplier; @@ -42,16 +43,18 @@ public class IndexCommand extends Command { double runsEmpty = 0; /** Creates a new IndexCommand. */ - public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier) { + public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier, DoubleSupplier ampSupplier) { this.m_Indexer = m_IndexerSubsystem; this.shootIfReadySupplier = shootIfReadySupplier; this.revUpSupplier = revUpSupplier; this.shooter = shooter; this.intake = intake; this.drivetrain = drivetrain; + this.ampSupplier = ampSupplier; this.angleShooterSubsytem = angleShooterSubsystem; this.humanPlayerSupplier = humanPlaySupplier; SmartDashboard.putNumber("amp RPS", AMP_RPS); + SmartDashboard.putNumber("indexer amp speed", IndexerConstants.AMP_SPEED); SmartDashboard.putNumber("amp angle", Field.AMPLIFIER_ANGLE); // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_IndexerSubsystem, shooter, intake); @@ -106,7 +109,11 @@ public void execute() { indexer = true; } if (indexer) { - m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED); + if(ampSupplier.getAsDouble() == 90) { + m_Indexer.set(SmartDashboard.getNumber("indexer amp speed", IndexerConstants.AMP_SPEED)); + } else { + m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED); + } } else { m_Indexer.off(); } diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 8ac9d117..e3547f09 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -108,7 +108,7 @@ private DriveConstants() { /** * The diameter of the module's wheel in meters. */ - public static final double DRIVETRAIN_WHEEL_DIAMETER = 0.097; + public static final double DRIVETRAIN_WHEEL_DIAMETER = 0.098; /** * The overall drive reduction of the module. Multiplying motor rotations by @@ -272,7 +272,7 @@ public static final class ShooterConstants{ public static final double AUTO_AIM_ROBOT_kD = 0.00; public static final double SHOOTING_RPS = 90; - public static final double AMP_RPS = 9.5; + public static final double AMP_RPS = 7.5; //the PID values used on the PID loop on the motor controller that control the position of the shooter angle public static final double ANGLE_SHOOTER_POWER_KP = 0.026; @@ -291,7 +291,7 @@ public static final class ShooterConstants{ public static final double DISTANCE_MULTIPLIER = 0.15; public static final double OFFSET_MULTIPLIER = 1; public static final double MINIMUM_SHOOTER_ANGLE = 10;//still has to be found - public static final double MAXIMUM_SHOOTER_ANGLE = 98;//still has to be found + public static final double MAXIMUM_SHOOTER_ANGLE = 104;//still has to be found public static final double HUMAN_PLAYER_ANGLE = 97;//still has to be found public static final double HUMAN_PLAYER_RPS = -10;//still has to be found @@ -330,6 +330,7 @@ public static final class IndexerConstants{ public static final double INDEXER_INTAKE_SPEED = 0.5;//should be 0.5 TODO change to positive public static final double HUMAN_PLAYER_INDEXER_SPEED = -0.5;//should be 0.5 TODO change to positive public static final double INDEXER_SHOOTING_SPEED = 1; + public static final double AMP_SPEED = 0.1; } public static final class IntakeConstants{ public static final int INTAKE_1_MOTOR = 20; @@ -418,7 +419,7 @@ public final class Field{ public static final double SPEAKER_Z = 2.08; //height of opening public static final double MAX_SHOOTING_DISTANCE = 2491; - public static final double AMPLIFIER_ANGLE = 97; + public static final double AMPLIFIER_ANGLE = 103; //angle at 60 for bounce techinque, didn't work } @@ -428,7 +429,7 @@ public final class Vision{ public static final String OBJ_DETECITON_LIMELIGHT_NAME = "limelight-neural"; public static final double APRILTAG_CLOSENESS = 0.5; - public static final double MAX_TAG_DISTANCE = 4; + public static final double MAX_TAG_DISTANCE = 3.05; public static final Translation2d fieldCorner = new Translation2d(16.54, 8.02); diff --git a/src/main/java/frc/robot/settings/LimelightValues.java b/src/main/java/frc/robot/settings/LimelightValues.java index a692b7df..53151067 100644 --- a/src/main/java/frc/robot/settings/LimelightValues.java +++ b/src/main/java/frc/robot/settings/LimelightValues.java @@ -43,7 +43,7 @@ public LimelightValues(Results llresults, boolean valid){ } this.botPoseRed = llresults.getBotPose2d_wpiRed(); this.botPoseBlue = llresults.getBotPose2d_wpiBlue(); - this.tagDistance = llresults.targets_Fiducials[0].getTargetPose_RobotSpace2D().getTranslation().getNorm(); + // this.tagDistance = llresults.targets_Fiducials[0].getTargetPose_RobotSpace2D().getTranslation().getNorm(); // SmartDashboard.putNumber("VISION dist to apriltag [0]", tagDistance); // SmartDashboard.putNumber("VISION dist to apriltag [1]", llresults.targets_Fiducials[1].getTargetPose_RobotSpace2D().getTranslation().getNorm()); // SmartDashboard.putNumber("VISION dist to apriltag [2]", llresults.targets_Fiducials[2].getTargetPose_RobotSpace2D().getTranslation().getNorm()); From 8177706256e52d95722047fc0cdfbf6c3eacda3b Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Thu, 22 Feb 2024 22:31:20 -0600 Subject: [PATCH 14/22] tuned amp speed and indexer speed for amp added --- .../deploy/pathplanner/paths/ScoreAmp.path | 26 +++++++++---------- .../java/frc/robot/commands/IndexCommand.java | 4 +-- .../java/frc/robot/commands/ManualShoot.java | 2 +- .../java/frc/robot/settings/Constants.java | 4 +-- .../frc/robot/settings/LimelightValues.java | 2 +- 5 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/ScoreAmp.path b/src/main/deploy/pathplanner/paths/ScoreAmp.path index cf441a5e..942994fa 100644 --- a/src/main/deploy/pathplanner/paths/ScoreAmp.path +++ b/src/main/deploy/pathplanner/paths/ScoreAmp.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.85, - "y": 6.7 + "x": 2.0, + "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 1.85, - "y": 6.889081499499318 + "x": 2.0, + "y": 7.189081499499318 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.8527875727163086, - "y": 7.7 + "x": 2.0, + "y": 6.9 }, "prevControl": { - "x": 1.8527875727163086, - "y": 7.6000000000000005 + "x": 2.0, + "y": 6.800000000000001 }, "nextControl": null, "isLocked": false, @@ -30,16 +30,16 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.0, + "waypointRelativePos": 0.25, "rotationDegrees": -90.0, - "rotateFast": false + "rotateFast": true } ], "constraintZones": [], "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.1, + "waypointRelativePos": 0.6, "command": { "type": "parallel", "data": { @@ -73,7 +73,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 2.0, "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 @@ -89,5 +89,5 @@ "rotation": -90.0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/IndexCommand.java b/src/main/java/frc/robot/commands/IndexCommand.java index e04143f3..6893b29e 100644 --- a/src/main/java/frc/robot/commands/IndexCommand.java +++ b/src/main/java/frc/robot/commands/IndexCommand.java @@ -54,7 +54,7 @@ public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIf this.angleShooterSubsytem = angleShooterSubsystem; this.humanPlayerSupplier = humanPlaySupplier; SmartDashboard.putNumber("amp RPS", AMP_RPS); - SmartDashboard.putNumber("indexer amp speed", IndexerConstants.AMP_SPEED); + SmartDashboard.putNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED); SmartDashboard.putNumber("amp angle", Field.AMPLIFIER_ANGLE); // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_IndexerSubsystem, shooter, intake); @@ -110,7 +110,7 @@ public void execute() { } if (indexer) { if(ampSupplier.getAsDouble() == 90) { - m_Indexer.set(SmartDashboard.getNumber("indexer amp speed", IndexerConstants.AMP_SPEED)); + m_Indexer.set(SmartDashboard.getNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED)); } else { m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED); } diff --git a/src/main/java/frc/robot/commands/ManualShoot.java b/src/main/java/frc/robot/commands/ManualShoot.java index e28135c0..acadbed6 100644 --- a/src/main/java/frc/robot/commands/ManualShoot.java +++ b/src/main/java/frc/robot/commands/ManualShoot.java @@ -24,7 +24,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED); + indexer.set(IndexerConstants.INDEXER_AMP_SPEED); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index e3547f09..53507247 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -330,7 +330,7 @@ public static final class IndexerConstants{ public static final double INDEXER_INTAKE_SPEED = 0.5;//should be 0.5 TODO change to positive public static final double HUMAN_PLAYER_INDEXER_SPEED = -0.5;//should be 0.5 TODO change to positive public static final double INDEXER_SHOOTING_SPEED = 1; - public static final double AMP_SPEED = 0.1; + public static final double INDEXER_AMP_SPEED = 0.4; } public static final class IntakeConstants{ public static final int INTAKE_1_MOTOR = 20; @@ -419,7 +419,7 @@ public final class Field{ public static final double SPEAKER_Z = 2.08; //height of opening public static final double MAX_SHOOTING_DISTANCE = 2491; - public static final double AMPLIFIER_ANGLE = 103; + public static final double AMPLIFIER_ANGLE = 101; //angle at 60 for bounce techinque, didn't work } diff --git a/src/main/java/frc/robot/settings/LimelightValues.java b/src/main/java/frc/robot/settings/LimelightValues.java index 53151067..5e577d8c 100644 --- a/src/main/java/frc/robot/settings/LimelightValues.java +++ b/src/main/java/frc/robot/settings/LimelightValues.java @@ -40,10 +40,10 @@ public LimelightValues(Results llresults, boolean valid){ this.tx[i] = llresults.targets_Fiducials[i].tx; this.ty[i] = llresults.targets_Fiducials[i].ty; this.ta[i] = llresults.targets_Fiducials[i].ta; + this.tagDistance = llresults.targets_Fiducials[0].getTargetPose_RobotSpace2D().getTranslation().getNorm(); } this.botPoseRed = llresults.getBotPose2d_wpiRed(); this.botPoseBlue = llresults.getBotPose2d_wpiBlue(); - // this.tagDistance = llresults.targets_Fiducials[0].getTargetPose_RobotSpace2D().getTranslation().getNorm(); // SmartDashboard.putNumber("VISION dist to apriltag [0]", tagDistance); // SmartDashboard.putNumber("VISION dist to apriltag [1]", llresults.targets_Fiducials[1].getTargetPose_RobotSpace2D().getTranslation().getNorm()); // SmartDashboard.putNumber("VISION dist to apriltag [2]", llresults.targets_Fiducials[2].getTargetPose_RobotSpace2D().getTranslation().getNorm()); From acf95c084a109f294afe8d50813b433f9146eb52 Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Thu, 22 Feb 2024 22:46:55 -0600 Subject: [PATCH 15/22] clenaup --- src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 649bcd55..407af1c7 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -432,10 +432,10 @@ public void periodic() { Boolean isLL2VisionValid = ll2.isResultValid; Boolean isLL3VisionValid = ll3.isResultValid; if(isLL2VisionValid) { - SmartDashboard.putNumber("VISION left LL tag [0] distnace", ll2.getTagDistance()); + SmartDashboard.putNumber("VISION left LL tag [0] distance", ll2.getTagDistance()); } if(isLL3VisionValid) { - SmartDashboard.putNumber("VISION right LL tag [0] distance", LOOPS_VALID_FOR_SHOT); + SmartDashboard.putNumber("VISION right LL tag [0] distance", ll3.getTagDistance()); } Boolean isLL2VisionTrustworthy = isLL2VisionValid && ll2.isPoseTrustworthy(odometer.getEstimatedPosition()); Boolean isLL3VisionTrustworthy = isLL3VisionValid && ll3.isPoseTrustworthy(odometer.getEstimatedPosition()); From 339015b3336c07680020eb3eb78f5c12208485ce Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Thu, 22 Feb 2024 22:57:25 -0600 Subject: [PATCH 16/22] added amp speed to manualShoot --- src/main/java/frc/robot/RobotContainer.java | 3 +-- src/main/java/frc/robot/commands/ManualShoot.java | 14 ++++++++++++-- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a41b0dde..173f2f6e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -83,7 +83,6 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PS4Controller; -import frc.robot.commands.ManualShoot; import frc.robot.commands.AngleShooter; import frc.robot.commands.IntakeCommand; import frc.robot.settings.IntakeDirection; @@ -262,7 +261,7 @@ private void configureBindings() { SmartDashboard.putData("Manual Angle Shooter Up", new AngleShooter(angleShooterSubsystem, ()->ShooterConstants.MAXIMUM_SHOOTER_ANGLE)); } if(indexerExists) { - new Trigger(driverController::getL1Button).whileTrue(new ManualShoot(indexer)); + new Trigger(driverController::getL1Button).whileTrue(new ManualShoot(indexer, driverController::getPOV)); } if(climberExists) { // new Trigger(driverController::getCrossButton).whileTrue(new AutoClimb(climber)).onFalse(new InstantCommand(()-> climber.climberStop())); diff --git a/src/main/java/frc/robot/commands/ManualShoot.java b/src/main/java/frc/robot/commands/ManualShoot.java index acadbed6..8c800f14 100644 --- a/src/main/java/frc/robot/commands/ManualShoot.java +++ b/src/main/java/frc/robot/commands/ManualShoot.java @@ -6,15 +6,21 @@ import frc.robot.settings.Constants.IndexerConstants; import frc.robot.settings.Constants.ShooterConstants; import frc.robot.subsystems.IndexerSubsystem; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + import edu.wpi.first.wpilibj2.command.Command; public class ManualShoot extends Command { private IndexerSubsystem indexer; + DoubleSupplier ampSupplier; /** Creates a new ManualShoot. */ - public ManualShoot(IndexerSubsystem indexer) { + public ManualShoot(IndexerSubsystem indexer, DoubleSupplier ampSupplier) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(indexer); this.indexer = indexer; + this.ampSupplier = ampSupplier; } // Called when the command is initially scheduled. @@ -24,7 +30,11 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - indexer.set(IndexerConstants.INDEXER_AMP_SPEED); + if(ampSupplier.getAsDouble() == 90 || ampSupplier.getAsDouble() == 45|| ampSupplier.getAsDouble() == 135) { + indexer.set(IndexerConstants.INDEXER_AMP_SPEED); + } else { + indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED); + } } // Called once the command ends or is interrupted. From 7524c752211983d7d68a852c61c0a55838d041fd Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Thu, 22 Feb 2024 23:57:03 -0600 Subject: [PATCH 17/22] hold down options to force set odometyr --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 173f2f6e..1434cb85 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -249,7 +249,7 @@ private void configureBindings() { () -> modifyAxis(-driverController.getRawAxis(Y_AXIS), DEADBAND_NORMAL), () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL), driverController::getL2Button)); - new Trigger(driverController::getOptionsButton).whileTrue(new InstantCommand(driveTrain::forceUpdateOdometryWithVision)); + new Trigger(driverController::getOptionsButton).onTrue(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", true))).onFalse(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", false))); new Trigger(driverController::getSquareButton).onTrue(new SequentialCommandGroup( new CollectNote(driveTrain, limelight), new DriveTimeCommand(-2, 0, 0, 0.5, driveTrain) From d4a27e8c9232122b4c9824f07b30a5ddcee3beb4 Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Fri, 23 Feb 2024 00:17:39 -0600 Subject: [PATCH 18/22] cleanup --- src/main/java/frc/robot/Robot.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1feb78e3..758db764 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,7 +29,6 @@ public class Robot extends TimedRobot { public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. - Preferences.setBoolean("Intake", true); m_robotContainer = new RobotContainer(); } From 4f66c6b7638b8fc9d3586054a17d63cbc5a6e75e Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Fri, 23 Feb 2024 00:38:12 -0600 Subject: [PATCH 19/22] added voltage limits to indexer and shooter --- src/main/java/frc/robot/settings/Constants.java | 3 +++ .../java/frc/robot/subsystems/IndexerSubsystem.java | 10 ++++++++++ .../java/frc/robot/subsystems/ShooterSubsystem.java | 8 ++++++++ 3 files changed, 21 insertions(+) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 91b8f54d..2c2b0d4f 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -267,6 +267,8 @@ public static final class ShooterConstants{ public static final double ALLOWED_ANGLE_ERROR = 1.5; public static final double ALLOWED_SPEED_ERROR = 4; + public static final double CURRENT_LIMIT = 200; //amps the motor is limited to + public static final double AUTO_AIM_ROBOT_kP = 0.125; public static final double AUTO_AIM_ROBOT_kI = 0.00; public static final double AUTO_AIM_ROBOT_kD = 0.00; @@ -324,6 +326,7 @@ public static final class ClimberConstants{ } public static final class IndexerConstants{ public static final int INDEXER_MOTOR = 11; + public static final int CURRENT_LIMIT = 200; public static final double INDEXER_INTAKE_SPEED = -0.5;//should be 0.5 TODO change to positive public static final double INDEXER_SHOOTING_SPEED = 1; } diff --git a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java index 75eb8436..2596c612 100644 --- a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAnalogSensor; @@ -10,14 +12,22 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.settings.Constants.IndexerConstants; +import frc.robot.settings.Constants.ShooterConstants; public class IndexerSubsystem extends SubsystemBase { TalonFX m_IndexerMotor; SparkAnalogSensor m_DistanceSensor; + CurrentLimitsConfigs currentLimitsConfigs; + TalonFXConfigurator configurator; public IndexerSubsystem() { m_IndexerMotor = new TalonFX(IndexerConstants.INDEXER_MOTOR); m_IndexerMotor.setInverted(false); + + currentLimitsConfigs = new CurrentLimitsConfigs(); + currentLimitsConfigs.SupplyCurrentLimit = IndexerConstants.CURRENT_LIMIT; + currentLimitsConfigs.SupplyCurrentLimitEnable = true; + configurator.apply(currentLimitsConfigs); } public void on() { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 7e4f4d5a..a2662a57 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -4,6 +4,7 @@ package frc.robot.subsystems; import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; @@ -41,6 +42,7 @@ public class ShooterSubsystem extends SubsystemBase { double currentHeading; double m_DesiredShooterAngle; + CurrentLimitsConfigs currentLimitConfigs; Slot0Configs PIDconfigs = new Slot0Configs(); double kP = Constants.ShooterConstants.kP; double kI = Constants.ShooterConstants.kI; @@ -75,6 +77,12 @@ public ShooterSubsystem(double runSpeed) { configuratorR = shooterR.getConfigurator(); configuratorL = shooterL.getConfigurator(); + currentLimitConfigs = new CurrentLimitsConfigs(); + currentLimitConfigs.SupplyCurrentLimit = ShooterConstants.CURRENT_LIMIT; + currentLimitConfigs.SupplyCurrentLimitEnable = true; + configuratorL.apply(currentLimitConfigs); + configuratorR.apply(currentLimitConfigs); + PIDconfigs.kP = kP; PIDconfigs.kI = kI; PIDconfigs.kD = kD; From 11e6c403c2af01618bdda38784e615d87fb9e010 Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Fri, 23 Feb 2024 07:29:11 -0600 Subject: [PATCH 20/22] added current limit to driveMotorConfic --- src/main/java/frc/robot/settings/Constants.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 2c2b0d4f..37ca1af6 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -209,6 +209,8 @@ private DriveConstants() { public static final double k_DRIVE_FF_V = 0; public static final double DRIVE_DEADBAND_MPS = 0.01; public static final double DRIVE_MOTOR_RAMP = 0.1; + public static final double DRIVE_CURRENT_LIMIT = 200; + // Steer Motor /** * The maximum velocity of the steer motor.

@@ -377,6 +379,8 @@ public CTREConfigs() { driveMotorConfig.Voltage.PeakForwardVoltage = 12; driveMotorConfig.Voltage.PeakReverseVoltage = -12; driveMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveMotorConfig.CurrentLimits.SupplyCurrentLimit = DriveConstants.DRIVE_CURRENT_LIMIT; + driveMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; // Steer encoder. steerEncoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; From f0e6daf33db79e3f6ae77854244212fdf581995c Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Fri, 23 Feb 2024 07:32:18 -0600 Subject: [PATCH 21/22] added values for testing --- src/main/java/frc/robot/subsystems/IndexerSubsystem.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java index 2596c612..fd66f9dd 100644 --- a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java @@ -46,5 +46,6 @@ public void set(double speed) { } @Override public void periodic() { + SmartDashboard.putNumber("indexer current", m_IndexerMotor.getSupplyCurrent().getValueAsDouble()); } } From c78425398f0bbeeb83f7456a1c36c9a85f353d6f Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Fri, 23 Feb 2024 17:20:27 -0600 Subject: [PATCH 22/22] added current limits that are good --- src/main/java/frc/robot/RobotContainer.java | 10 ++++++---- src/main/java/frc/robot/settings/Constants.java | 2 +- .../java/frc/robot/subsystems/IndexerSubsystem.java | 1 + .../java/frc/robot/subsystems/ShooterSubsystem.java | 7 +++++-- 4 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 33fa50dd..b7bad8fc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -154,7 +154,7 @@ public RobotContainer() { driverController = new PS4Controller(DRIVE_CONTROLLER_ID); operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID); pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); - PDP = new PowerDistribution(0, ModuleType.kRev); + PDP = new PowerDistribution(1, ModuleType.kRev); // = new PathPlannerPath(null, DEFAUL_PATH_CONSTRAINTS, null, climberExists); limelightInit(); @@ -256,10 +256,12 @@ private void configureBindings() { () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL), driverController::getL2Button)); - new Trigger(driverController::getR1Button).onTrue(new SequentialCommandGroup( - new CollectNote(driveTrain, limelight), - new DriveTimeCommand(-2, 0, 0, 0.5, driveTrain) + if(Preferences.getBoolean("Detector Limelight", false)) { + new Trigger(driverController::getR1Button).onTrue(new SequentialCommandGroup( + new CollectNote(driveTrain, limelight), + new DriveTimeCommand(-2, 0, 0, 0.5, driveTrain) )); + } new Trigger(driverController::getTouchpadPressed).onTrue(new InstantCommand(driveTrain::stop, driveTrain)); SmartDashboard.putData("force update limelight position", new InstantCommand(()->driveTrain.forceUpdateOdometryWithVision(), driveTrain)); if(angleShooterExists) { diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 37ca1af6..e16879bc 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -328,7 +328,7 @@ public static final class ClimberConstants{ } public static final class IndexerConstants{ public static final int INDEXER_MOTOR = 11; - public static final int CURRENT_LIMIT = 200; + public static final int CURRENT_LIMIT = 50; public static final double INDEXER_INTAKE_SPEED = -0.5;//should be 0.5 TODO change to positive public static final double INDEXER_SHOOTING_SPEED = 1; } diff --git a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java index fd66f9dd..ff65a191 100644 --- a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java @@ -27,6 +27,7 @@ public IndexerSubsystem() { currentLimitsConfigs = new CurrentLimitsConfigs(); currentLimitsConfigs.SupplyCurrentLimit = IndexerConstants.CURRENT_LIMIT; currentLimitsConfigs.SupplyCurrentLimitEnable = true; + configurator = m_IndexerMotor.getConfigurator(); configurator.apply(currentLimitsConfigs); } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index a2662a57..c2a10719 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; @@ -132,8 +133,8 @@ public boolean validShot() { return runsValid >= Constants.LOOPS_VALID_FOR_SHOT; } public void turnOff(){ - shooterR.set(0); - shooterL.set(0); + shooterR.setControl(new DutyCycleOut(0)); + shooterL.setControl(new DutyCycleOut(0)); } // public double getSpeedRPS() { // return shooterR.getVelocity().asSupplier().get(); @@ -141,6 +142,8 @@ public void turnOff(){ @Override public void periodic() { SmartDashboard.putNumber("TESTING shooter speed error", getError()); + SmartDashboard.putNumber("shooter current right", shooterR.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("shooter current left", shooterL.getSupplyCurrent().getValueAsDouble()); if(getError()