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 1/7] 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 2/7] 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 3/7] 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 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 4/7] 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 5/7] 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 6/7] 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 7/7] 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()