From 45f028d0c44f2b025580fef5cdf9ff3e6d09acd5 Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Thu, 20 Feb 2025 17:47:37 -0300 Subject: [PATCH 01/21] Intake and Puncher --- .../java/frc/robot/subsystems/Intake.java | 0 .../java/frc/robot/subsystems/Puncher.java | 0 .../java/frc/robot/subsystems/Shooter.java | 21 ------------------- 3 files changed, 21 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/Puncher.java delete mode 100644 src/main/java/frc/robot/subsystems/Shooter.java diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/subsystems/Puncher.java b/src/main/java/frc/robot/subsystems/Puncher.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java deleted file mode 100644 index d490556..0000000 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.swervedrive.Vision; - -public class Shooter extends SubsystemBase { - Vision vision; - - public Shooter(Vision vision){ - this.vision = vision; - } - - @Override - public void periodic(){ - SmartDashboard.putNumber("Distance From Apriltag", vision.getDistanceFromAprilTag(1)); - SmartDashboard.putNumber("Apriltag X", vision.getXApriltag(1)); - SmartDashboard.putNumber("Apriltag Y", vision.getYApriltag(1)); - - } -} From 1be293895d4897c075c7a9fad61ff4e24c71513d Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Thu, 20 Feb 2025 23:57:49 -0300 Subject: [PATCH 02/21] sendable implementation --- simgui-ds.json | 11 ++- simgui.json | 30 +++++++ src/main/java/frc/robot/RobotContainer.java | 41 +++++++++- .../java/frc/robot/subsystems/Intake.java | 24 ++++++ src/main/java/frc/robot/subsystems/Joint.java | 41 ++++++++++ .../java/frc/robot/subsystems/Puncher.java | 81 +++++++++++++++++++ 6 files changed, 223 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Joint.java diff --git a/simgui-ds.json b/simgui-ds.json index 179e064..4b9f566 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -4,11 +4,15 @@ "visible": true } }, + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ { - "decKey": 65, "incKey": 68 }, { @@ -29,10 +33,11 @@ "axisCount": 5, "buttonCount": 4, "buttonKeys": [ - 90, + 65, 88, 67, - 86 + 86, + -1 ], "povConfig": [ { diff --git a/simgui.json b/simgui.json index 18af23d..ab8f202 100644 --- a/simgui.json +++ b/simgui.json @@ -60,6 +60,7 @@ "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", "/LiveWindow/Ungrouped/Pigeon 2 [13]": "Gyro", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", + "/SmartDashboard/Alerts": "Alerts", "/SmartDashboard/Encoders": "Alerts", "/SmartDashboard/Field": "Field2d", "/SmartDashboard/IMU": "Alerts", @@ -118,6 +119,14 @@ } }, "NetworkTables": { + "retained": { + "Shuffleboard": { + "Main": { + "open": true + }, + "open": true + } + }, "transitory": { "FMSInfo": { "open": true @@ -134,6 +143,27 @@ } }, "NetworkTables Info": { + "Clients": { + "open": true + }, + "Connections": { + "open": true + }, + "Server": { + "Subscribers": { + "open": true + }, + "open": true + }, + "shuffleboard@1": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, "visible": true } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18f7d10..def887f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,17 +8,27 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; -import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Joint; +import frc.robot.subsystems.Puncher; import frc.robot.subsystems.swervedrive.SwerveSubsystem; + import java.io.File; +import java.util.Map; + import swervelib.SwerveInputStream; /** @@ -31,12 +41,18 @@ */ public class RobotContainer { + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + private NetworkTable table; + // Replace with CommandPS4Controller or CommandJoystick if needed final CommandXboxController driverXbox = new CommandXboxController(0); // The robot's subsystems and commands are defined here... private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); - private Shooter shooter; + private final Intake intake; + private final Puncher puncher; + private final Joint joint; + //private Shooter shooter; /** * Converts driver input into a field-relative ChassisSpeeds that is controlled * by angular velocity. @@ -103,7 +119,14 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + setupNetworkTable(); + table = inst.getTable("Shuffleboard"); + //shooter = new Shooter(drivebase.getVision()); + intake = new Intake(); + puncher = new Puncher(); + joint = new Joint(); + configureBindings(); DriverStation.silenceJoystickConnectionWarning(true); NamedCommands.registerCommand("test", Commands.print("I EXIST")); @@ -166,6 +189,8 @@ private void configureBindings() { driverXbox.rightBumper().onTrue(Commands.none()); } + driverXbox.povUp().whileTrue(Commands.startEnd(() -> joint.percentOut(table.getEntry("Intake IN").getDouble(0)), () -> joint.stop(), joint)); + } /** @@ -181,4 +206,16 @@ public Command getAutonomousCommand() { public void setMotorBrake(boolean brake) { drivebase.setMotorBrake(brake); } + + public void setupNetworkTable() { + ShuffleboardTab tab = Shuffleboard.getTab("Main"); + addTab(tab, "Intake IN"); + } + + public void addTab(ShuffleboardTab tab, String title) { + tab.add(title, 0) + .withWidget(BuiltInWidgets.kNumberSlider) + .withSize(2, 1) + .withProperties(Map.of("min", -1, "max", 1)); + } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index e69de29..1868314 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase { + private final TalonFX IntakeMotor; + private final DutyCycleOut percentOutCycle = new DutyCycleOut(0); + + public Intake() { + IntakeMotor = new TalonFX(12); + IntakeMotor.getConfigurator().apply(new TalonFXConfiguration()); + } + + public void percentOut(double speed) { + IntakeMotor.setControl(percentOutCycle.withOutput(speed)); + } + + public void stop() { + IntakeMotor.stopMotor(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Joint.java b/src/main/java/frc/robot/subsystems/Joint.java new file mode 100644 index 0000000..55e72a8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Joint.java @@ -0,0 +1,41 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.InvertType; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; +import com.ctre.phoenix6.controls.Follower; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Joint extends SubsystemBase { + private final TalonSRX IntakeLeftMotor; + private final TalonSRX IntakeRightMotor; + + public Joint() { + IntakeLeftMotor = new TalonSRX(13); + IntakeRightMotor = new TalonSRX(14); + + TalonSRXConfiguration configs = new TalonSRXConfiguration(); + + IntakeLeftMotor.configAllSettings(configs); + IntakeRightMotor.configAllSettings(configs); + + IntakeLeftMotor.setNeutralMode(NeutralMode.Brake); + IntakeRightMotor.setNeutralMode(NeutralMode.Brake); + + IntakeLeftMotor.setInverted(false); + IntakeRightMotor.setInverted(true); + + IntakeRightMotor.follow(IntakeLeftMotor); + } + + public void percentOut(double speed) { + IntakeLeftMotor.set(TalonSRXControlMode.PercentOutput, speed); + } + + public void stop() { + IntakeLeftMotor.set(TalonSRXControlMode.PercentOutput, 0); + } +} diff --git a/src/main/java/frc/robot/subsystems/Puncher.java b/src/main/java/frc/robot/subsystems/Puncher.java index e69de29..1d3e8a8 100644 --- a/src/main/java/frc/robot/subsystems/Puncher.java +++ b/src/main/java/frc/robot/subsystems/Puncher.java @@ -0,0 +1,81 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Puncher extends SubsystemBase { + private final TalonFX puncherLeftMotor; + private final TalonFX puncherRightMotor; + private final Follower follower; + + private final DutyCycleOut percentOutCycle = new DutyCycleOut(0); + private final PositionDutyCycle positionCycle = new PositionDutyCycle(0); + + public Puncher() { + puncherLeftMotor = new TalonFX(10); + puncherRightMotor = new TalonFX(11); + follower = new Follower(10, false); + + puncherLeftMotor.getConfigurator().apply(new TalonFXConfiguration()); + puncherRightMotor.getConfigurator().apply(new TalonFXConfiguration()); + + TalonFXConfigurator masterConfig = puncherLeftMotor.getConfigurator(); + TalonFXConfigurator slaveConfig = puncherRightMotor.getConfigurator(); + + Slot0Configs positionPIDConfigs = new Slot0Configs(); + positionPIDConfigs.kP = 0; + positionPIDConfigs.kI = 0; + positionPIDConfigs.kD = 0; + positionPIDConfigs.kA = 0; + masterConfig.apply(positionPIDConfigs); + slaveConfig.apply(positionPIDConfigs); + + MotorOutputConfigs outputConfigs = new MotorOutputConfigs(); + outputConfigs.PeakForwardDutyCycle = 1; + outputConfigs.PeakReverseDutyCycle = -1; + outputConfigs.withNeutralMode(NeutralModeValue.Coast); + //outputConfigs.withInverted(InvertedValue.Clockwise_Positive); + masterConfig.apply(outputConfigs); + slaveConfig.apply(outputConfigs); + + SoftwareLimitSwitchConfigs limitSwitchConfigs = new SoftwareLimitSwitchConfigs(); + limitSwitchConfigs.ForwardSoftLimitEnable = false; + limitSwitchConfigs.ReverseSoftLimitEnable = false; + limitSwitchConfigs.ForwardSoftLimitThreshold = 0; + limitSwitchConfigs.ReverseSoftLimitThreshold = 0; + masterConfig.apply(limitSwitchConfigs); + slaveConfig.apply(limitSwitchConfigs); + } + + public double getPosition() { + return puncherLeftMotor.getPosition().getValueAsDouble(); + } + + public void goToPosition(double position) { + double clampedPosition = MathUtil.clamp(position, 0, 10); + puncherLeftMotor.setControl(positionCycle.withPosition(clampedPosition)); + puncherRightMotor.setControl(follower); + } + + public void percentOut(double speed) { + puncherLeftMotor.setControl(percentOutCycle.withOutput(speed)); + puncherRightMotor.setControl(follower); + } + + public void stop() { + puncherLeftMotor.stopMotor(); + puncherRightMotor.stopMotor(); + } +} \ No newline at end of file From 121b002221fd7344bbf093a1696ad72f0facd066 Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Fri, 21 Feb 2025 21:26:29 -0300 Subject: [PATCH 03/21] intake --- simgui-ds.json | 4 +- simgui.json | 32 +++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 25 +++-------- src/main/java/frc/robot/subsystems/Joint.java | 41 +++++++++++++------ 4 files changed, 68 insertions(+), 34 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 4b9f566..8f80438 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,7 +1,7 @@ { - "Keyboard 0 Settings": { + "Joysticks": { "window": { - "visible": true + "visible": false } }, "System Joysticks": { diff --git a/simgui.json b/simgui.json index ab8f202..c03b499 100644 --- a/simgui.json +++ b/simgui.json @@ -1,6 +1,11 @@ { "HALProvider": { "Other Devices": { + "SPARK MAX [1] RELATIVE ENCODER": { + "header": { + "open": true + } + }, "SPARK MAX [2]": { "header": { "open": true @@ -21,6 +26,16 @@ "open": true } }, + "Talon FX (v6)[10]": { + "header": { + "open": true + } + }, + "Talon FX (v6)[12]": { + "header": { + "open": true + } + }, "Talon FX (v6)[2]/Rotor Sensor": { "header": { "open": true @@ -51,6 +66,14 @@ "open": true } } + }, + "RoboRIO": { + "6V Rail": { + "open": true + }, + "RoboRIO Input": { + "open": true + } } }, "NTProvider": { @@ -119,6 +142,15 @@ } }, "NetworkTables": { + "Persistent Values": { + "open": false + }, + "Retained Values": { + "open": false + }, + "Transitory Values": { + "open": false + }, "retained": { "Shuffleboard": { "Main": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index def887f..89860a6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; @@ -41,9 +42,6 @@ */ public class RobotContainer { - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - private NetworkTable table; - // Replace with CommandPS4Controller or CommandJoystick if needed final CommandXboxController driverXbox = new CommandXboxController(0); // The robot's subsystems and commands are defined here... @@ -119,8 +117,6 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - setupNetworkTable(); - table = inst.getTable("Shuffleboard"); //shooter = new Shooter(drivebase.getVision()); intake = new Intake(); @@ -189,8 +185,11 @@ private void configureBindings() { driverXbox.rightBumper().onTrue(Commands.none()); } - driverXbox.povUp().whileTrue(Commands.startEnd(() -> joint.percentOut(table.getEntry("Intake IN").getDouble(0)), () -> joint.stop(), joint)); - + driverXbox.povRight().whileTrue(Commands.startEnd(() -> intake.percentOut(0), () -> intake.stop(), intake)); + driverXbox.povLeft().whileTrue(Commands.startEnd(() -> intake.percentOut(0), () -> intake.stop(), intake)); + driverXbox.povUp().whileTrue(Commands.startEnd(() -> joint.percentOut(0), () -> joint.stop(), joint)); + driverXbox.povDown().whileTrue(Commands.startEnd(() -> joint.percentOut(0), () -> joint.stop(), joint)); + driverXbox.y().whileTrue(Commands.startEnd(() -> puncher.percentOut(0), () -> puncher.stop(), puncher)); } /** @@ -206,16 +205,4 @@ public Command getAutonomousCommand() { public void setMotorBrake(boolean brake) { drivebase.setMotorBrake(brake); } - - public void setupNetworkTable() { - ShuffleboardTab tab = Shuffleboard.getTab("Main"); - addTab(tab, "Intake IN"); - } - - public void addTab(ShuffleboardTab tab, String title) { - tab.add(title, 0) - .withWidget(BuiltInWidgets.kNumberSlider) - .withSize(2, 1) - .withProperties(Map.of("min", -1, "max", 1)); - } } diff --git a/src/main/java/frc/robot/subsystems/Joint.java b/src/main/java/frc/robot/subsystems/Joint.java index 55e72a8..5fb489d 100644 --- a/src/main/java/frc/robot/subsystems/Joint.java +++ b/src/main/java/frc/robot/subsystems/Joint.java @@ -1,41 +1,56 @@ package frc.robot.subsystems; +import java.beans.Encoder; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.TalonSRXFeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DigitalSource; +import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Joint extends SubsystemBase { - private final TalonSRX IntakeLeftMotor; - private final TalonSRX IntakeRightMotor; + private final TalonSRX intakeLeftMotor; + private final TalonSRX intakeRightMotor; + private final DutyCycleEncoder absoluteEncoder; public Joint() { - IntakeLeftMotor = new TalonSRX(13); - IntakeRightMotor = new TalonSRX(14); + intakeLeftMotor = new TalonSRX(13); + intakeRightMotor = new TalonSRX(14); + + absoluteEncoder = new DutyCycleEncoder(new DigitalInput(2)); + + intakeLeftMotor.configSelectedFeedbackSensor(FeedbackDevice.PulseWidthEncodedPosition, 0, 0); + intakeRightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 0); TalonSRXConfiguration configs = new TalonSRXConfiguration(); - IntakeLeftMotor.configAllSettings(configs); - IntakeRightMotor.configAllSettings(configs); + intakeLeftMotor.configAllSettings(configs); + intakeRightMotor.configAllSettings(configs); - IntakeLeftMotor.setNeutralMode(NeutralMode.Brake); - IntakeRightMotor.setNeutralMode(NeutralMode.Brake); + intakeLeftMotor.setNeutralMode(NeutralMode.Brake); + intakeRightMotor.setNeutralMode(NeutralMode.Brake); - IntakeLeftMotor.setInverted(false); - IntakeRightMotor.setInverted(true); + intakeLeftMotor.setInverted(false); + intakeRightMotor.setInverted(true); - IntakeRightMotor.follow(IntakeLeftMotor); + intakeRightMotor.follow(intakeLeftMotor); } public void percentOut(double speed) { - IntakeLeftMotor.set(TalonSRXControlMode.PercentOutput, speed); + intakeLeftMotor.set(TalonSRXControlMode.PercentOutput, speed); } public void stop() { - IntakeLeftMotor.set(TalonSRXControlMode.PercentOutput, 0); + intakeLeftMotor.set(TalonSRXControlMode.PercentOutput, 0); } } From 033bd772508c14be905658570085deed8f41fa49 Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Fri, 21 Feb 2025 18:27:40 -0300 Subject: [PATCH 04/21] intake --- .../java/frc/robot/subsystems/Intake.java | 25 +++++++++++-------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 1868314..59d77b8 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,24 +1,29 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Intake extends SubsystemBase { - private final TalonFX IntakeMotor; - private final DutyCycleOut percentOutCycle = new DutyCycleOut(0); - + private final TalonSRX intakeMotor; + public Intake() { - IntakeMotor = new TalonFX(12); - IntakeMotor.getConfigurator().apply(new TalonFXConfiguration()); + intakeMotor = new TalonSRX(12); + + TalonSRXConfiguration configs = new TalonSRXConfiguration(); + + intakeMotor.configAllSettings(configs); + intakeMotor.setNeutralMode(NeutralMode.Coast); + intakeMotor.setInverted(false); } public void percentOut(double speed) { - IntakeMotor.setControl(percentOutCycle.withOutput(speed)); + intakeMotor.set(TalonSRXControlMode.PercentOutput, speed); } public void stop() { - IntakeMotor.stopMotor(); + intakeMotor.set(TalonSRXControlMode.PercentOutput, 0); } } \ No newline at end of file From 88c3b0456e9f391140bb06b981e31bcf1d6716c5 Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Sat, 22 Feb 2025 08:22:33 -0300 Subject: [PATCH 05/21] Subsystems --- src/main/java/frc/robot/RobotContainer.java | 38 +++++++++++++------ .../{Intake.java => IntakeSubsystem.java} | 4 +- .../{Joint.java => JointSubsystem.java} | 4 +- .../{Puncher.java => PuncherSubsystem.java} | 4 +- 4 files changed, 33 insertions(+), 17 deletions(-) rename src/main/java/frc/robot/subsystems/{Intake.java => IntakeSubsystem.java} (90%) rename src/main/java/frc/robot/subsystems/{Joint.java => JointSubsystem.java} (95%) rename src/main/java/frc/robot/subsystems/{Puncher.java => PuncherSubsystem.java} (97%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 89860a6..6b2a6e6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,14 +17,17 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; -import frc.robot.subsystems.Intake; -import frc.robot.subsystems.Joint; -import frc.robot.subsystems.Puncher; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.JointSubsystem; +import frc.robot.subsystems.PuncherSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; @@ -35,7 +38,7 @@ /** * This class is where the bulk of the robot should be declared. Since * Command-based is a "declarative" paradigm, very - * little robot logic should actually be handled in the {@link Robot} periodic + * little robot logic should actually be handled in the {@link Robot} periodic * methods (other than the scheduler calls). * Instead, the structure of the robot (including subsystems, commands, and * trigger mappings) should be declared here. @@ -47,10 +50,13 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); - private final Intake intake; - private final Puncher puncher; - private final Joint joint; + private final IntakeSubsystem intake; + private final PuncherSubsystem puncher; + private final JointSubsystem joint; //private Shooter shooter; + + SendableChooser autoChooser = new SendableChooser (); + /** * Converts driver input into a field-relative ChassisSpeeds that is controlled * by angular velocity. @@ -118,10 +124,12 @@ public class RobotContainer { */ public RobotContainer() { + setupAuto(); + //shooter = new Shooter(drivebase.getVision()); - intake = new Intake(); - puncher = new Puncher(); - joint = new Joint(); + intake = new IntakeSubsystem(); + puncher = new PuncherSubsystem(); + joint = new JointSubsystem(); configureBindings(); DriverStation.silenceJoystickConnectionWarning(true); @@ -199,10 +207,18 @@ private void configureBindings() { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return drivebase.getAutonomousCommand("testauto"); + return autoChooser.getSelected(); + //return drivebase.getAutonomousCommand("testauto"); } public void setMotorBrake(boolean brake) { drivebase.setMotorBrake(brake); } + + public void setupAuto() { + SmartDashboard.putData(autoChooser); + autoChooser.setDefaultOption("No auto", new PrintCommand("No Auto Selected")); + autoChooser.addOption("Also no auto", new PrintCommand("Also No Auto Selected")); + } + } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java similarity index 90% rename from src/main/java/frc/robot/subsystems/Intake.java rename to src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 59d77b8..854f913 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -6,10 +6,10 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Intake extends SubsystemBase { +public class IntakeSubsystem extends SubsystemBase { private final TalonSRX intakeMotor; - public Intake() { + public IntakeSubsystem() { intakeMotor = new TalonSRX(12); TalonSRXConfiguration configs = new TalonSRXConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/Joint.java b/src/main/java/frc/robot/subsystems/JointSubsystem.java similarity index 95% rename from src/main/java/frc/robot/subsystems/Joint.java rename to src/main/java/frc/robot/subsystems/JointSubsystem.java index 5fb489d..fde998f 100644 --- a/src/main/java/frc/robot/subsystems/Joint.java +++ b/src/main/java/frc/robot/subsystems/JointSubsystem.java @@ -18,12 +18,12 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Joint extends SubsystemBase { +public class JointSubsystem extends SubsystemBase { private final TalonSRX intakeLeftMotor; private final TalonSRX intakeRightMotor; private final DutyCycleEncoder absoluteEncoder; - public Joint() { + public JointSubsystem() { intakeLeftMotor = new TalonSRX(13); intakeRightMotor = new TalonSRX(14); diff --git a/src/main/java/frc/robot/subsystems/Puncher.java b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java similarity index 97% rename from src/main/java/frc/robot/subsystems/Puncher.java rename to src/main/java/frc/robot/subsystems/PuncherSubsystem.java index 1d3e8a8..4d3c729 100644 --- a/src/main/java/frc/robot/subsystems/Puncher.java +++ b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java @@ -15,7 +15,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Puncher extends SubsystemBase { +public class PuncherSubsystem extends SubsystemBase { private final TalonFX puncherLeftMotor; private final TalonFX puncherRightMotor; private final Follower follower; @@ -23,7 +23,7 @@ public class Puncher extends SubsystemBase { private final DutyCycleOut percentOutCycle = new DutyCycleOut(0); private final PositionDutyCycle positionCycle = new PositionDutyCycle(0); - public Puncher() { + public PuncherSubsystem() { puncherLeftMotor = new TalonFX(10); puncherRightMotor = new TalonFX(11); follower = new Follower(10, false); From 457fda79f5f83a99af8226d6a4f7ea6ad519ac7d Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Mon, 24 Feb 2025 14:16:41 -0300 Subject: [PATCH 06/21] ButtonBindings --- src/main/java/frc/robot/RobotContainer.java | 217 +++--------------- src/main/java/frc/robot/Utils.java | 5 - .../buttonBindings/DriverButtonBindings.java | 157 +++++++++++++ .../OperatorButtonBindings.java | 15 ++ 4 files changed, 208 insertions(+), 186 deletions(-) delete mode 100644 src/main/java/frc/robot/Utils.java create mode 100644 src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java create mode 100644 src/main/java/frc/robot/buttonBindings/OperatorButtonBindings.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6b2a6e6..a90ce02 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,220 +5,75 @@ package frc.robot; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.OperatorConstants; +import frc.robot.buttonBindings.DriverButtonBindings; +import frc.robot.buttonBindings.OperatorButtonBindings; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.JointSubsystem; import frc.robot.subsystems.PuncherSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; -import java.util.Map; - -import swervelib.SwerveInputStream; - -/** - * This class is where the bulk of the robot should be declared. Since - * Command-based is a "declarative" paradigm, very - * little robot logic should actually be handled in the {@link Robot} periodic - * methods (other than the scheduler calls). - * Instead, the structure of the robot (including subsystems, commands, and - * trigger mappings) should be declared here. - */ + public class RobotContainer { + private final CommandXboxController driverXbox; + private final CommandXboxController operatorXbox; - // Replace with CommandPS4Controller or CommandJoystick if needed - final CommandXboxController driverXbox = new CommandXboxController(0); - // The robot's subsystems and commands are defined here... - private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), - "swerve")); - private final IntakeSubsystem intake; - private final PuncherSubsystem puncher; - private final JointSubsystem joint; - //private Shooter shooter; - - SendableChooser autoChooser = new SendableChooser (); - - /** - * Converts driver input into a field-relative ChassisSpeeds that is controlled - * by angular velocity. - */ - SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> driverXbox.getLeftY() * -1, - () -> driverXbox.getLeftX() * -1) - // Modificando para usar os triggers para controlar a rotação - .withControllerRotationAxis(() -> { - double leftTrigger = driverXbox.getLeftTriggerAxis(); // Valor entre 0 e 1 - double rightTrigger = driverXbox.getRightTriggerAxis(); // Valor entre 0 e 1 - - // Definindo a rotação com base nos triggers - double rotation = rightTrigger - leftTrigger; - - return rotation; // Retorna o valor da rotação - }) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - - /** - * Clone's the angular velocity input stream and converts it to a fieldRelative - * input stream. - */ - SwerveInputStream driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis(driverXbox::getRightX, - driverXbox::getRightY) - .headingWhile(true); - - /** - * Clone's the angular velocity input stream and converts it to a robotRelative - * input stream. - */ - SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(true) - .allianceRelativeControl(false); - - SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX()) - .withControllerRotationAxis(() -> driverXbox.getRawAxis( - 2)) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - // Derive the heading axis with math! - SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() - .withControllerHeadingAxis(() -> Math.sin( - driverXbox.getRawAxis( - 2) * - Math.PI) - * - (Math.PI * - 2), - () -> Math.cos( - driverXbox.getRawAxis( - 2) * - Math.PI) - * - (Math.PI * - 2)) - .headingWhile(true); - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { + private final SwerveSubsystem swerveSubsystem; + private final IntakeSubsystem intakeSubsystem; + private final PuncherSubsystem puncherSubsystem; + private final JointSubsystem jointSubsystem; - setupAuto(); + private final DriverButtonBindings driverButtonBindings; + private final OperatorButtonBindings operatorButtonBindings; + SendableChooser autoChooser; + + public RobotContainer() { + driverXbox = new CommandXboxController(0); + operatorXbox = new CommandXboxController(1); - //shooter = new Shooter(drivebase.getVision()); - intake = new IntakeSubsystem(); - puncher = new PuncherSubsystem(); - joint = new JointSubsystem(); + swerveSubsystem = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),"swerve")); + intakeSubsystem = new IntakeSubsystem(); + puncherSubsystem = new PuncherSubsystem(); + jointSubsystem = new JointSubsystem(); + driverButtonBindings = new DriverButtonBindings(driverXbox, swerveSubsystem, intakeSubsystem, + jointSubsystem, puncherSubsystem); + operatorButtonBindings = new OperatorButtonBindings(operatorXbox); configureBindings(); + + autoChooser = new SendableChooser (); + autoSetup(); + DriverStation.silenceJoystickConnectionWarning(true); - NamedCommands.registerCommand("test", Commands.print("I EXIST")); } - /** - * Use this method to define your trigger->command mappings. Triggers can be - * created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with - * an arbitrary predicate, or via the - * named factories in - * {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses - * for - * {@link CommandXboxController - * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4} - * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick - * Flight joysticks}. - */ private void configureBindings() { - - Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle); - Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); - Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); - Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative( - driveDirectAngle); - Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard); - Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard); - Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative( - driveDirectAngleKeyboard); - - if (RobotBase.isSimulation()) { - drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard); - } else { - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); - } - - if (Robot.isSimulation()) { - driverXbox.start().onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d())))); - driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand()); - - } - if (DriverStation.isTest()) { - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command above! - - driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); - driverXbox.y().whileTrue(drivebase.driveToDistanceCommand(1.0, 0.2)); - driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro))); - driverXbox.back().whileTrue(drivebase.centerModulesCommand()); - driverXbox.leftBumper().onTrue(Commands.none()); - driverXbox.rightBumper().onTrue(Commands.none()); - } else { - driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); - driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); - driverXbox.b().whileTrue( - drivebase.driveToPose( - new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))); - driverXbox.start().whileTrue(Commands.none()); - driverXbox.back().whileTrue(Commands.none()); - driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); - driverXbox.rightBumper().onTrue(Commands.none()); - } - - driverXbox.povRight().whileTrue(Commands.startEnd(() -> intake.percentOut(0), () -> intake.stop(), intake)); - driverXbox.povLeft().whileTrue(Commands.startEnd(() -> intake.percentOut(0), () -> intake.stop(), intake)); - driverXbox.povUp().whileTrue(Commands.startEnd(() -> joint.percentOut(0), () -> joint.stop(), joint)); - driverXbox.povDown().whileTrue(Commands.startEnd(() -> joint.percentOut(0), () -> joint.stop(), joint)); - driverXbox.y().whileTrue(Commands.startEnd(() -> puncher.percentOut(0), () -> puncher.stop(), puncher)); + driverButtonBindings.configureBindings(); + operatorButtonBindings.configureBindings(); } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ public Command getAutonomousCommand() { - // An example command will be run in autonomous return autoChooser.getSelected(); - //return drivebase.getAutonomousCommand("testauto"); } - public void setMotorBrake(boolean brake) { - drivebase.setMotorBrake(brake); - } - - public void setupAuto() { + public void autoSetup() { SmartDashboard.putData(autoChooser); + autoChooser.setDefaultOption("No auto", new PrintCommand("No Auto Selected")); autoChooser.addOption("Also no auto", new PrintCommand("Also No Auto Selected")); + + NamedCommands.registerCommand("test", Commands.print("I EXIST")); } + public void setMotorBrake(boolean brake) { + swerveSubsystem.setMotorBrake(brake); + } } diff --git a/src/main/java/frc/robot/Utils.java b/src/main/java/frc/robot/Utils.java deleted file mode 100644 index e077134..0000000 --- a/src/main/java/frc/robot/Utils.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot; - -public class Utils { - -} diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java new file mode 100644 index 0000000..6ffee62 --- /dev/null +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -0,0 +1,157 @@ +package frc.robot.buttonBindings; + +import org.dyn4j.dynamics.joint.Joint; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants.OperatorConstants; +import frc.robot.Robot; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.JointSubsystem; +import frc.robot.subsystems.PuncherSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import swervelib.SwerveInputStream; + +public class DriverButtonBindings { + private final CommandXboxController driverXbox; + + private final SwerveSubsystem swerveSubsystem; + private final IntakeSubsystem intakeSubsystem; + private final JointSubsystem jointSubsystem; + private final PuncherSubsystem puncherSubsystem; + + private final SwerveInputStream driveDirectAngle; + private final SwerveInputStream driveRobotOriented; + private final SwerveInputStream driveAngularVelocityKeyboard; + private final SwerveInputStream driveDirectAngleKeyboard; + private final SwerveInputStream driveAngularVelocity; + + private final Command driveFieldOrientedDirectAngle; + private final Command driveFieldOrientedAnglularVelocity; + private final Command driveRobotOrientedAngularVelocity; + private final Command driveSetpointGen; + private final Command driveFieldOrientedDirectAngleKeyboard; + private final Command driveFieldOrientedAnglularVelocityKeyboard; + private final Command driveSetpointGenKeyboard; + + + public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem drivebase, + IntakeSubsystem intake, JointSubsystem joint, PuncherSubsystem puncher) { + this.driverXbox = driverXbox; + + this.swerveSubsystem = drivebase; + this.intakeSubsystem = intake; + this.jointSubsystem = joint; + this.puncherSubsystem = puncher; + + driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), + () -> driverXbox.getLeftY() * -1, + () -> driverXbox.getLeftX() * -1) + .withControllerRotationAxis(() -> { + double leftTrigger = driverXbox.getLeftTriggerAxis(); + double rightTrigger = driverXbox.getRightTriggerAxis(); + double rotation = rightTrigger - leftTrigger; + return rotation; + }) + .deadband(OperatorConstants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); + + driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis + (driverXbox::getRightX, driverXbox::getRightY).headingWhile(true); + + driveRobotOriented = driveAngularVelocity.copy().robotRelative(true) + .allianceRelativeControl(false); + + driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), + () -> -driverXbox.getLeftY(), + () -> -driverXbox.getLeftX()) + .withControllerRotationAxis(() -> driverXbox.getRawAxis( + 2)) + .deadband(OperatorConstants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); + + driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() + .withControllerHeadingAxis(() -> Math.sin( + driverXbox.getRawAxis(2) * Math.PI) * (Math.PI * 2), + () -> Math.cos(driverXbox.getRawAxis(2) * Math.PI) * (Math.PI * 2)) + .headingWhile(true); + + driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle); + driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); + driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); + driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngle); + driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard); + driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard); + driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngleKeyboard); + } + + public void configureBindings() { + drivebaseDefaultButtonBindings(); + drivebaseSimulationButtonBindings(); + drivebaseTestButtonBindings(); + intakeButtonBindings(); + jointButtonBindings(); + puncherButtonBindings(); + } + + public void drivebaseDefaultButtonBindings() { + if (!RobotBase.isSimulation()) { + swerveSubsystem.setDefaultCommand(driveFieldOrientedAnglularVelocity); + } + + if (!DriverStation.isTest()) { + driverXbox.a().onTrue((Commands.runOnce(swerveSubsystem::zeroGyro))); + driverXbox.x().onTrue(Commands.runOnce(swerveSubsystem::addFakeVisionReading)); + driverXbox.b().whileTrue(swerveSubsystem.driveToPose(new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))); + driverXbox.start().whileTrue(Commands.none()); + driverXbox.back().whileTrue(Commands.none()); + driverXbox.leftBumper().whileTrue(Commands.runOnce(swerveSubsystem::lock, swerveSubsystem).repeatedly()); + driverXbox.rightBumper().onTrue(Commands.none()); + } + } + + public void drivebaseSimulationButtonBindings() { + if (RobotBase.isSimulation()) { + swerveSubsystem.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard); + } + + if (Robot.isSimulation()) { + driverXbox.start().onTrue(Commands.runOnce(() -> swerveSubsystem.resetOdometry(new Pose2d(3, 3, new Rotation2d())))); + driverXbox.button(1).whileTrue(swerveSubsystem.sysIdDriveMotorCommand()); + } + } + + public void drivebaseTestButtonBindings() { + if (DriverStation.isTest()) { + swerveSubsystem.setDefaultCommand(driveFieldOrientedAnglularVelocity); + driverXbox.x().whileTrue(Commands.runOnce(swerveSubsystem::lock, swerveSubsystem).repeatedly()); + driverXbox.y().whileTrue(swerveSubsystem.driveToDistanceCommand(1.0, 0.2)); + driverXbox.start().onTrue((Commands.runOnce(swerveSubsystem::zeroGyro))); + driverXbox.back().whileTrue(swerveSubsystem.centerModulesCommand()); + driverXbox.leftBumper().onTrue(Commands.none()); + driverXbox.rightBumper().onTrue(Commands.none()); + } + } + + public void intakeButtonBindings() { + driverXbox.povRight().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(0), () -> intakeSubsystem.stop(), intakeSubsystem)); + driverXbox.povLeft().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(0), () -> intakeSubsystem.stop(), intakeSubsystem)); + } + + public void jointButtonBindings() { + driverXbox.povUp().whileTrue(Commands.startEnd(() -> jointSubsystem.percentOut(0), () -> jointSubsystem.stop(), jointSubsystem)); + driverXbox.povDown().whileTrue(Commands.startEnd(() -> jointSubsystem.percentOut(0), () -> jointSubsystem.stop(), jointSubsystem)); + } + + public void puncherButtonBindings() { + driverXbox.y().whileTrue(Commands.startEnd(() -> puncherSubsystem.percentOut(0), () -> puncherSubsystem.stop(), puncherSubsystem)); + } +} diff --git a/src/main/java/frc/robot/buttonBindings/OperatorButtonBindings.java b/src/main/java/frc/robot/buttonBindings/OperatorButtonBindings.java new file mode 100644 index 0000000..42334d2 --- /dev/null +++ b/src/main/java/frc/robot/buttonBindings/OperatorButtonBindings.java @@ -0,0 +1,15 @@ +package frc.robot.buttonBindings; + +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +public class OperatorButtonBindings { + private final CommandXboxController operatorXbox; + + public OperatorButtonBindings(CommandXboxController operatorXbox) { + this.operatorXbox = operatorXbox; + } + + public void configureBindings() { + + } +} From c32b39d8a989bc192a2b91a79a66153912825e7c Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Mon, 24 Feb 2025 15:53:05 -0300 Subject: [PATCH 07/21] RobotState --- src/main/java/frc/robot/Constants.java | 39 ++---- src/main/java/frc/robot/Robot.java | 119 ++++-------------- src/main/java/frc/robot/RobotState.java | 8 ++ .../buttonBindings/DriverButtonBindings.java | 6 +- .../swervedrive/auto/AutoBalanceCommand.java | 86 ------------- .../drivebase/AbsoluteDriveAdv.java | 2 +- 6 files changed, 45 insertions(+), 215 deletions(-) create mode 100644 src/main/java/frc/robot/RobotState.java delete mode 100644 src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b5b7e09..0183fa5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,44 +8,21 @@ import edu.wpi.first.math.util.Units; import swervelib.math.Matter; -/** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean constants. This - * class should not be used for any other purpose. All constants should be declared globally (i.e. public static). Do - * not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ -public final class Constants -{ - +public final class Constants { public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound - public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); - public static final double LOOP_TIME = 0.13; //s, 20ms + 110ms sprk max velocity lag - public static final double MAX_SPEED = Units.feetToMeters(14.5); + public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); + public static final double LOOP_TIME = 0.13; //s, 20ms + 110ms sprk max velocity lag + public static final double MAX_SPEED = Units.feetToMeters(14.5); // Maximum speed of the robot in meters per second, used to limit acceleration. -// public static final class AutonConstants -// { -// -// public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); -// public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); -// } - - public static final class DrivebaseConstants - { - - // Hold time on motor brakes when disabled + public static final class DrivebaseConstants { public static final double WHEEL_LOCK_TIME = 10; // seconds } - public static class OperatorConstants - { - - // Joystick Deadband - public static final double DEADBAND = 0.1; + public static class HIDConstants { + public static final double DEADBAND = 0.1; public static final double LEFT_Y_DEADBAND = 0.1; public static final double RIGHT_X_DEADBAND = 0.1; - public static final double TURN_CONSTANT = 6; + public static final double TURN_CONSTANT = 6; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b301879..4669628 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,166 +10,97 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -/** - * The VM is configured to automatically run this class, and to call the functions corresponding to each mode, as - * described in the TimedRobot documentation. If you change the name of this class or the package after creating this - * project, you must also update the build.gradle file in the project. - */ -public class Robot extends TimedRobot -{ - - private static Robot instance; - private Command m_autonomousCommand; +public class Robot extends TimedRobot { + private static Robot instance; + private Command m_autonomousCommand; private RobotContainer m_robotContainer; private Timer disabledTimer; - public Robot() - { + public Robot() { instance = this; } - public static Robot getInstance() - { + public static Robot getInstance() { return instance; } - /** - * This function is run when the robot is first started up and should be used for any initialization code. - */ @Override - public void robotInit() - { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. + public void robotInit() { m_robotContainer = new RobotContainer(); - - // Create a timer to disable motor brake a few seconds after disable. This will let the robot stop - // immediately when disabled, but then also let it be pushed more disabledTimer = new Timer(); - if (isSimulation()) - { + if (isSimulation()) { DriverStation.silenceJoystickConnectionWarning(true); } } - /** - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics that you want ran - * during disabled, autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - */ @Override - public void robotPeriodic() - { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // 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. + public void robotPeriodic() { CommandScheduler.getInstance().run(); } - /** - * This function is called once each time the robot enters Disabled mode. - */ @Override - public void disabledInit() - { + public void disabledInit() { m_robotContainer.setMotorBrake(true); disabledTimer.reset(); disabledTimer.start(); } @Override - public void disabledPeriodic() - { - if (disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) - { + public void disabledPeriodic() { + RobotState.alliance = DriverStation.getAlliance(); + if (disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) { m_robotContainer.setMotorBrake(false); disabledTimer.stop(); disabledTimer.reset(); } } - /** - * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. - */ @Override - public void autonomousInit() - { + public void autonomousInit() { + RobotState.alliance = DriverStation.getAlliance(); m_robotContainer.setMotorBrake(true); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - // schedule the autonomous command (example) - if (m_autonomousCommand != null) - { + if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } } - /** - * This function is called periodically during autonomous. - */ @Override - public void autonomousPeriodic() - { + public void autonomousPeriodic() { } @Override - public void teleopInit() - { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (m_autonomousCommand != null) - { + public void teleopInit() { + RobotState.alliance = DriverStation.getAlliance(); + if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); - } else - { + } else { CommandScheduler.getInstance().cancelAll(); } } - /** - * This function is called periodically during operator control. - */ @Override - public void teleopPeriodic() - { + public void teleopPeriodic() { } @Override - public void testInit() - { - // Cancels all running commands at the start of test mode. + public void testInit() { CommandScheduler.getInstance().cancelAll(); } - /** - * This function is called periodically during test mode. - */ @Override - public void testPeriodic() - { + public void testPeriodic() { } - /** - * This function is called once when the robot is first started up. - */ @Override - public void simulationInit() - { + public void simulationInit() { } - /** - * This function is called periodically whilst in simulation. - */ @Override - public void simulationPeriodic() - { + public void simulationPeriodic() { } } diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java new file mode 100644 index 0000000..a90c354 --- /dev/null +++ b/src/main/java/frc/robot/RobotState.java @@ -0,0 +1,8 @@ +package frc.robot; + +import java.util.Optional; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +public class RobotState { + public static Optional alliance = Optional.empty(); +} diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java index 6ffee62..d3bb9c7 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Constants.OperatorConstants; +import frc.robot.Constants.HIDConstants; import frc.robot.Robot; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.JointSubsystem; @@ -59,7 +59,7 @@ public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem dr double rotation = rightTrigger - leftTrigger; return rotation; }) - .deadband(OperatorConstants.DEADBAND) + .deadband(HIDConstants.DEADBAND) .scaleTranslation(0.8) .allianceRelativeControl(true); @@ -74,7 +74,7 @@ public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem dr () -> -driverXbox.getLeftX()) .withControllerRotationAxis(() -> driverXbox.getRawAxis( 2)) - .deadband(OperatorConstants.DEADBAND) + .deadband(HIDConstants.DEADBAND) .scaleTranslation(0.8) .allianceRelativeControl(true); diff --git a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java deleted file mode 100644 index 6919e45..0000000 --- a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java +++ /dev/null @@ -1,86 +0,0 @@ -package frc.robot.commands.swervedrive.auto; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; - - -/** - * Auto Balance command using a simple PID controller. Created by Team 3512 - * ... - */ -public class AutoBalanceCommand extends Command -{ - - private final SwerveSubsystem swerveSubsystem; - private final PIDController controller; - - public AutoBalanceCommand(SwerveSubsystem swerveSubsystem) - { - this.swerveSubsystem = swerveSubsystem; - controller = new PIDController(1.0, 0.0, 0.0); - controller.setTolerance(1); - controller.setSetpoint(0.0); - // each subsystem used by the command must be passed into the - // addRequirements() method (which takes a vararg of Subsystem) - addRequirements(this.swerveSubsystem); - } - - /** - * The initial subroutine of a command. Called once when the command is initially scheduled. - */ - @Override - public void initialize() - { - - } - - /** - * The main body of a command. Called repeatedly while the command is scheduled. (That is, it is called repeatedly - * until {@link #isFinished()}) returns true.) - */ - @Override - public void execute() - { - SmartDashboard.putBoolean("At Tolerance", controller.atSetpoint()); - - double translationVal = MathUtil.clamp(controller.calculate(swerveSubsystem.getPitch().getDegrees(), 0.0), -0.5, - 0.5); - swerveSubsystem.drive(new Translation2d(translationVal, 0.0), 0.0, true); - } - - /** - *

- * Returns whether this command has finished. Once a command finishes -- indicated by this method returning true -- - * the scheduler will call its {@link #end(boolean)} method. - *

- * Returning false will result in the command never ending automatically. It may still be cancelled manually or - * interrupted by another command. Hard coding this command to always return true will result in the command executing - * once and finishing immediately. It is recommended to use * - * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} for such an operation. - *

- * - * @return whether this command has finished. - */ - @Override - public boolean isFinished() - { - return controller.atSetpoint(); - } - - /** - * The action to take when the command ends. Called when either the command finishes normally -- that is it is called - * when {@link #isFinished()} returns true -- or when it is interrupted/canceled. This is where you may want to wrap - * up loose ends, like shutting off a motor that was being used in the command. - * - * @param interrupted whether the command was interrupted/canceled - */ - @Override - public void end(boolean interrupted) - { - swerveSubsystem.lock(); - } -} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java index a92904f..0ec9d2a 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -130,7 +130,7 @@ public void execute() if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { resetHeading = true; - swerve.drive(translation, (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true); + swerve.drive(translation, (Constants.HIDConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true); } else { swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); From bad9820ac67a2563e3e911c9c77e1563be03563b Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Wed, 26 Feb 2025 13:54:28 -0300 Subject: [PATCH 08/21] ElevatorSubsystem --- src/main/java/frc/robot/RobotContainer.java | 6 +- .../buttonBindings/DriverButtonBindings.java | 19 ++++-- .../robot/subsystems/ElevatorSubsystem.java | 51 +++++++++++++++ .../frc/robot/subsystems/JointSubsystem.java | 64 +++++++++++-------- 4 files changed, 108 insertions(+), 32 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/ElevatorSubsystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a90ce02..10623f2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.buttonBindings.DriverButtonBindings; import frc.robot.buttonBindings.OperatorButtonBindings; +import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.JointSubsystem; import frc.robot.subsystems.PuncherSubsystem; @@ -30,6 +31,7 @@ public class RobotContainer { private final IntakeSubsystem intakeSubsystem; private final PuncherSubsystem puncherSubsystem; private final JointSubsystem jointSubsystem; + private final ElevatorSubsystem elevatorSubsystem; private final DriverButtonBindings driverButtonBindings; private final OperatorButtonBindings operatorButtonBindings; @@ -43,9 +45,11 @@ public RobotContainer() { intakeSubsystem = new IntakeSubsystem(); puncherSubsystem = new PuncherSubsystem(); jointSubsystem = new JointSubsystem(); + elevatorSubsystem = new ElevatorSubsystem(); + driverButtonBindings = new DriverButtonBindings(driverXbox, swerveSubsystem, intakeSubsystem, - jointSubsystem, puncherSubsystem); + jointSubsystem, puncherSubsystem, elevatorSubsystem); operatorButtonBindings = new OperatorButtonBindings(operatorXbox); configureBindings(); diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java index d3bb9c7..91db2aa 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.HIDConstants; import frc.robot.Robot; +import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.JointSubsystem; import frc.robot.subsystems.PuncherSubsystem; @@ -25,6 +26,7 @@ public class DriverButtonBindings { private final IntakeSubsystem intakeSubsystem; private final JointSubsystem jointSubsystem; private final PuncherSubsystem puncherSubsystem; + private final ElevatorSubsystem elevatorSubsystem; private final SwerveInputStream driveDirectAngle; private final SwerveInputStream driveRobotOriented; @@ -42,13 +44,14 @@ public class DriverButtonBindings { public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem drivebase, - IntakeSubsystem intake, JointSubsystem joint, PuncherSubsystem puncher) { + IntakeSubsystem intake, JointSubsystem joint, PuncherSubsystem puncher, ElevatorSubsystem elevatorSubsystem) { this.driverXbox = driverXbox; this.swerveSubsystem = drivebase; this.intakeSubsystem = intake; this.jointSubsystem = joint; this.puncherSubsystem = puncher; + this.elevatorSubsystem = elevatorSubsystem; driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), () -> driverXbox.getLeftY() * -1, @@ -100,6 +103,7 @@ public void configureBindings() { intakeButtonBindings(); jointButtonBindings(); puncherButtonBindings(); + elevatorButtonBindings(); } public void drivebaseDefaultButtonBindings() { @@ -142,16 +146,21 @@ public void drivebaseTestButtonBindings() { } public void intakeButtonBindings() { - driverXbox.povRight().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(0), () -> intakeSubsystem.stop(), intakeSubsystem)); - driverXbox.povLeft().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(0), () -> intakeSubsystem.stop(), intakeSubsystem)); + driverXbox.povRight().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(0.5), () -> intakeSubsystem.stop(), intakeSubsystem)); + driverXbox.povLeft().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(0.5), () -> intakeSubsystem.stop(), intakeSubsystem)); } public void jointButtonBindings() { - driverXbox.povUp().whileTrue(Commands.startEnd(() -> jointSubsystem.percentOut(0), () -> jointSubsystem.stop(), jointSubsystem)); - driverXbox.povDown().whileTrue(Commands.startEnd(() -> jointSubsystem.percentOut(0), () -> jointSubsystem.stop(), jointSubsystem)); + driverXbox.povUp().whileTrue(Commands.startEnd(() -> jointSubsystem.percentOut(0.5), () -> jointSubsystem.stop(), jointSubsystem)); + driverXbox.povDown().whileTrue(Commands.startEnd(() -> jointSubsystem.percentOut(0.5), () -> jointSubsystem.stop(), jointSubsystem)); } public void puncherButtonBindings() { driverXbox.y().whileTrue(Commands.startEnd(() -> puncherSubsystem.percentOut(0), () -> puncherSubsystem.stop(), puncherSubsystem)); } + + public void elevatorButtonBindings() { + driverXbox.a().whileTrue(Commands.startEnd(elevatorSubsystem::moveUp, elevatorSubsystem::stop, elevatorSubsystem)); + driverXbox.b().whileTrue(Commands.startEnd(elevatorSubsystem::moveDown, elevatorSubsystem::stop, elevatorSubsystem)); + } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java new file mode 100644 index 0000000..d5a9804 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -0,0 +1,51 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; +import com.ctre.phoenix.motorcontrol.ControlMode; + +public class ElevatorSubsystem extends SubsystemBase { + private final VictorSPX motor; + private final DigitalInput minLimitSwitch; + private final DigitalInput maxLimitSwitch; + + private static final double SPEED_UP = 0.0; + private static final double SPEED_DOWN = 0.0; + + public ElevatorSubsystem() { + motor = new VictorSPX(15); + minLimitSwitch = new DigitalInput(0); + maxLimitSwitch = new DigitalInput(1); + } + + public void moveUp() { + if (!this.isAtMax()) { + motor.set(ControlMode.PercentOutput, SPEED_UP); + } else { + stop(); + } + } + + public void moveDown() { + if (!this.isAtMin()) { + motor.set(ControlMode.PercentOutput, SPEED_DOWN); + } else { + stop(); + } + } + + public void stop() { + motor.set(ControlMode.PercentOutput, 0); + } + + public boolean isAtMin() { + return !minLimitSwitch.get(); + } + + public boolean isAtMax() { + return !maxLimitSwitch.get(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/JointSubsystem.java b/src/main/java/frc/robot/subsystems/JointSubsystem.java index fde998f..535e9b4 100644 --- a/src/main/java/frc/robot/subsystems/JointSubsystem.java +++ b/src/main/java/frc/robot/subsystems/JointSubsystem.java @@ -1,56 +1,68 @@ package frc.robot.subsystems; -import java.beans.Encoder; - import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; -import com.ctre.phoenix.motorcontrol.TalonSRXFeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DigitalSource; import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class JointSubsystem extends SubsystemBase { - private final TalonSRX intakeLeftMotor; - private final TalonSRX intakeRightMotor; + private final TalonSRX JointLeftMotor; + private final TalonSRX JointRightMotor; private final DutyCycleEncoder absoluteEncoder; + private boolean absoluteEncoderConnected; + private int absoluteEncoderFrequency; + private double absoluteEncoderOutput; + private double relativeEncoderPosition; + public JointSubsystem() { - intakeLeftMotor = new TalonSRX(13); - intakeRightMotor = new TalonSRX(14); + JointLeftMotor = new TalonSRX(13); + JointRightMotor = new TalonSRX(14); - absoluteEncoder = new DutyCycleEncoder(new DigitalInput(2)); + absoluteEncoder = new DutyCycleEncoder(3, 1, 0); + absoluteEncoder.setAssumedFrequency(975.6); - intakeLeftMotor.configSelectedFeedbackSensor(FeedbackDevice.PulseWidthEncodedPosition, 0, 0); - intakeRightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 0); + JointRightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10); + JointRightMotor.getSensorCollection().setQuadraturePosition(0, 10); TalonSRXConfiguration configs = new TalonSRXConfiguration(); - intakeLeftMotor.configAllSettings(configs); - intakeRightMotor.configAllSettings(configs); + JointLeftMotor.configAllSettings(configs); + JointRightMotor.configAllSettings(configs); - intakeLeftMotor.setNeutralMode(NeutralMode.Brake); - intakeRightMotor.setNeutralMode(NeutralMode.Brake); + JointLeftMotor.setNeutralMode(NeutralMode.Brake); + JointRightMotor.setNeutralMode(NeutralMode.Brake); - intakeLeftMotor.setInverted(false); - intakeRightMotor.setInverted(true); + JointLeftMotor.setInverted(false); + JointRightMotor.setInverted(false); - intakeRightMotor.follow(intakeLeftMotor); + JointRightMotor.follow(JointLeftMotor); } public void percentOut(double speed) { - intakeLeftMotor.set(TalonSRXControlMode.PercentOutput, speed); + JointLeftMotor.set(TalonSRXControlMode.PercentOutput, speed); } public void stop() { - intakeLeftMotor.set(TalonSRXControlMode.PercentOutput, 0); + JointLeftMotor.set(TalonSRXControlMode.PercentOutput, 0); + } + + @Override + public void periodic() { + absoluteEncoderConnected = absoluteEncoder.isConnected(); + absoluteEncoderFrequency = absoluteEncoder.getFrequency(); + absoluteEncoderOutput = absoluteEncoder.get(); + + relativeEncoderPosition = JointRightMotor.getSelectedSensorPosition(0); + + SmartDashboard.putBoolean("Joint Subsystem/Absolute Encoder/Connected", absoluteEncoderConnected); + SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Frequency", absoluteEncoderFrequency); + SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Output", absoluteEncoderOutput); + + SmartDashboard.putNumber("Joint Subsystem/Relative Encoder/Position", relativeEncoderPosition); } } From ff1d0ef85d28d0550d6fb4d5a04090027079f47b Mon Sep 17 00:00:00 2001 From: Pedro Henrique Date: Fri, 28 Feb 2025 15:35:55 -0300 Subject: [PATCH 09/21] thing --- .../buttonBindings/DriverButtonBindings.java | 17 +++++++++-------- .../frc/robot/subsystems/PuncherSubsystem.java | 4 ++-- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java index 91db2aa..b31945d 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -97,13 +97,13 @@ public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem dr } public void configureBindings() { - drivebaseDefaultButtonBindings(); - drivebaseSimulationButtonBindings(); - drivebaseTestButtonBindings(); + //drivebaseDefaultButtonBindings(); + //drivebaseSimulationButtonBindings(); + //drivebaseTestButtonBindings(); intakeButtonBindings(); - jointButtonBindings(); + //jointButtonBindings(); puncherButtonBindings(); - elevatorButtonBindings(); + //elevatorButtonBindings(); } public void drivebaseDefaultButtonBindings() { @@ -146,8 +146,8 @@ public void drivebaseTestButtonBindings() { } public void intakeButtonBindings() { - driverXbox.povRight().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(0.5), () -> intakeSubsystem.stop(), intakeSubsystem)); - driverXbox.povLeft().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(0.5), () -> intakeSubsystem.stop(), intakeSubsystem)); + driverXbox.povRight().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(0.6), () -> intakeSubsystem.stop(), intakeSubsystem)); + driverXbox.povLeft().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(-0.6), () -> intakeSubsystem.stop(), intakeSubsystem)); } public void jointButtonBindings() { @@ -156,7 +156,8 @@ public void jointButtonBindings() { } public void puncherButtonBindings() { - driverXbox.y().whileTrue(Commands.startEnd(() -> puncherSubsystem.percentOut(0), () -> puncherSubsystem.stop(), puncherSubsystem)); + driverXbox.y().whileTrue(Commands.startEnd(() -> puncherSubsystem.percentOut(0.6), () -> puncherSubsystem.percentOut(0.1), puncherSubsystem)); + driverXbox.a().whileTrue(Commands.startEnd(() -> puncherSubsystem.percentOut(-1), () -> puncherSubsystem.stop(), puncherSubsystem)); } public void elevatorButtonBindings() { diff --git a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java index 4d3c729..6557d0b 100644 --- a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java @@ -35,8 +35,8 @@ public PuncherSubsystem() { TalonFXConfigurator slaveConfig = puncherRightMotor.getConfigurator(); Slot0Configs positionPIDConfigs = new Slot0Configs(); - positionPIDConfigs.kP = 0; - positionPIDConfigs.kI = 0; + positionPIDConfigs.kP = 0.2; + positionPIDConfigs.kI = 0.005; positionPIDConfigs.kD = 0; positionPIDConfigs.kA = 0; masterConfig.apply(positionPIDConfigs); From 2ea8cbededd8aab5e9acce45693c174aca6ddf45 Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Sat, 1 Mar 2025 19:36:07 -0300 Subject: [PATCH 10/21] commit --- .../buttonBindings/DriverButtonBindings.java | 9 ++++++--- .../robot/subsystems/ElevatorSubsystem.java | 6 ++++-- .../frc/robot/subsystems/JointSubsystem.java | 2 +- .../frc/robot/subsystems/PuncherSubsystem.java | 18 +++++++++++++++++- 4 files changed, 28 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java index b31945d..a02914b 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -100,8 +100,8 @@ public void configureBindings() { //drivebaseDefaultButtonBindings(); //drivebaseSimulationButtonBindings(); //drivebaseTestButtonBindings(); - intakeButtonBindings(); - //jointButtonBindings(); + //intakeButtonBindings(); + jointButtonBindings(); puncherButtonBindings(); //elevatorButtonBindings(); } @@ -156,8 +156,11 @@ public void jointButtonBindings() { } public void puncherButtonBindings() { - driverXbox.y().whileTrue(Commands.startEnd(() -> puncherSubsystem.percentOut(0.6), () -> puncherSubsystem.percentOut(0.1), puncherSubsystem)); + driverXbox.y().whileTrue(Commands.startEnd(() -> puncherSubsystem.percentOut(0.6), () -> puncherSubsystem.stop(), puncherSubsystem)); driverXbox.a().whileTrue(Commands.startEnd(() -> puncherSubsystem.percentOut(-1), () -> puncherSubsystem.stop(), puncherSubsystem)); + //driverXbox.povLeft().whileTrue(Commands.startEnd(() -> puncherSubsystem.goToPosition(25), () -> puncherSubsystem.goToPosition(0), puncherSubsystem)); + driverXbox.povLeft().whileTrue(Commands.startEnd(() -> puncherSubsystem.setBaixo(0.1), () -> puncherSubsystem.setBaixo(0), puncherSubsystem)); + driverXbox.povRight().whileTrue(Commands.startEnd(() -> puncherSubsystem.setBaixo(-0.1), () -> puncherSubsystem.setBaixo(0), puncherSubsystem)); } public void elevatorButtonBindings() { diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index d5a9804..ddec7de 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -4,11 +4,13 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.VictorSPX; import com.ctre.phoenix.motorcontrol.ControlMode; public class ElevatorSubsystem extends SubsystemBase { - private final VictorSPX motor; + private final TalonSRX motor; private final DigitalInput minLimitSwitch; private final DigitalInput maxLimitSwitch; @@ -16,7 +18,7 @@ public class ElevatorSubsystem extends SubsystemBase { private static final double SPEED_DOWN = 0.0; public ElevatorSubsystem() { - motor = new VictorSPX(15); + motor = new TalonSRX(15); minLimitSwitch = new DigitalInput(0); maxLimitSwitch = new DigitalInput(1); } diff --git a/src/main/java/frc/robot/subsystems/JointSubsystem.java b/src/main/java/frc/robot/subsystems/JointSubsystem.java index 535e9b4..77afbff 100644 --- a/src/main/java/frc/robot/subsystems/JointSubsystem.java +++ b/src/main/java/frc/robot/subsystems/JointSubsystem.java @@ -37,7 +37,7 @@ public JointSubsystem() { JointLeftMotor.setNeutralMode(NeutralMode.Brake); JointRightMotor.setNeutralMode(NeutralMode.Brake); - JointLeftMotor.setInverted(false); + JointLeftMotor.setInverted(true); JointRightMotor.setInverted(false); JointRightMotor.follow(JointLeftMotor); diff --git a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java index 6557d0b..01794fa 100644 --- a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java @@ -1,5 +1,9 @@ package frc.robot.subsystems; +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; @@ -13,6 +17,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class PuncherSubsystem extends SubsystemBase { @@ -20,6 +25,10 @@ public class PuncherSubsystem extends SubsystemBase { private final TalonFX puncherRightMotor; private final Follower follower; + private final TalonSRX baixo; + + private final Servo servo; + private final DutyCycleOut percentOutCycle = new DutyCycleOut(0); private final PositionDutyCycle positionCycle = new PositionDutyCycle(0); @@ -28,6 +37,9 @@ public PuncherSubsystem() { puncherRightMotor = new TalonFX(11); follower = new Follower(10, false); + servo = new Servo(9); + baixo = new TalonSRX(16); + puncherLeftMotor.getConfigurator().apply(new TalonFXConfiguration()); puncherRightMotor.getConfigurator().apply(new TalonFXConfiguration()); @@ -64,7 +76,7 @@ public double getPosition() { } public void goToPosition(double position) { - double clampedPosition = MathUtil.clamp(position, 0, 10); + double clampedPosition = MathUtil.clamp(position, 0, 100); puncherLeftMotor.setControl(positionCycle.withPosition(clampedPosition)); puncherRightMotor.setControl(follower); } @@ -78,4 +90,8 @@ public void stop() { puncherLeftMotor.stopMotor(); puncherRightMotor.stopMotor(); } + + public void setBaixo(double speed) { + baixo.set(TalonSRXControlMode.PercentOutput,speed); + } } \ No newline at end of file From 938c9aaa5952d4d1b7bd0f86ae01eba9948b1435 Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Mon, 3 Mar 2025 05:39:14 -0300 Subject: [PATCH 11/21] commit --- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 4 ++ .../buttonBindings/DriverButtonBindings.java | 20 ++++--- .../robot/subsystems/ElevatorSubsystem.java | 4 ++ .../frc/robot/subsystems/JointSubsystem.java | 53 ++++++++++++------- .../robot/subsystems/PuncherSubsystem.java | 16 +++--- 6 files changed, 61 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4669628..2aab5b6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -56,6 +56,8 @@ public void disabledPeriodic() { disabledTimer.stop(); disabledTimer.reset(); } + + m_robotContainer.resetEncoders(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 10623f2..7320b18 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,4 +80,8 @@ public void autoSetup() { public void setMotorBrake(boolean brake) { swerveSubsystem.setMotorBrake(brake); } + + public void resetEncoders() { + puncherSubsystem.resetEncoders(); + } } diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java index a02914b..f05d7f8 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -100,7 +100,7 @@ public void configureBindings() { //drivebaseDefaultButtonBindings(); //drivebaseSimulationButtonBindings(); //drivebaseTestButtonBindings(); - //intakeButtonBindings(); + intakeButtonBindings(); jointButtonBindings(); puncherButtonBindings(); //elevatorButtonBindings(); @@ -146,25 +146,23 @@ public void drivebaseTestButtonBindings() { } public void intakeButtonBindings() { - driverXbox.povRight().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(0.6), () -> intakeSubsystem.stop(), intakeSubsystem)); - driverXbox.povLeft().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(-0.6), () -> intakeSubsystem.stop(), intakeSubsystem)); + driverXbox.y().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(0.6), () -> intakeSubsystem.stop(), intakeSubsystem)); + driverXbox.x().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(-0.6), () -> intakeSubsystem.stop(), intakeSubsystem)); } public void jointButtonBindings() { driverXbox.povUp().whileTrue(Commands.startEnd(() -> jointSubsystem.percentOut(0.5), () -> jointSubsystem.stop(), jointSubsystem)); - driverXbox.povDown().whileTrue(Commands.startEnd(() -> jointSubsystem.percentOut(0.5), () -> jointSubsystem.stop(), jointSubsystem)); + driverXbox.povDown().whileTrue(Commands.startEnd(() -> jointSubsystem.percentOut(-0.5), () -> jointSubsystem.stop(), jointSubsystem)); } public void puncherButtonBindings() { - driverXbox.y().whileTrue(Commands.startEnd(() -> puncherSubsystem.percentOut(0.6), () -> puncherSubsystem.stop(), puncherSubsystem)); - driverXbox.a().whileTrue(Commands.startEnd(() -> puncherSubsystem.percentOut(-1), () -> puncherSubsystem.stop(), puncherSubsystem)); - //driverXbox.povLeft().whileTrue(Commands.startEnd(() -> puncherSubsystem.goToPosition(25), () -> puncherSubsystem.goToPosition(0), puncherSubsystem)); - driverXbox.povLeft().whileTrue(Commands.startEnd(() -> puncherSubsystem.setBaixo(0.1), () -> puncherSubsystem.setBaixo(0), puncherSubsystem)); - driverXbox.povRight().whileTrue(Commands.startEnd(() -> puncherSubsystem.setBaixo(-0.1), () -> puncherSubsystem.setBaixo(0), puncherSubsystem)); + driverXbox.b().whileTrue(Commands.startEnd(() -> puncherSubsystem.goToPosition(100), () -> puncherSubsystem.goToPosition(0), puncherSubsystem)); + driverXbox.povLeft().onTrue(Commands.startEnd(() -> puncherSubsystem.setBaixo(0.15), () -> puncherSubsystem.setBaixo(0), puncherSubsystem).withTimeout(2)); + driverXbox.povRight().onTrue(Commands.startEnd(() -> puncherSubsystem.setBaixo(-0.15), () -> puncherSubsystem.setBaixo(0), puncherSubsystem).withTimeout(2)); } public void elevatorButtonBindings() { - driverXbox.a().whileTrue(Commands.startEnd(elevatorSubsystem::moveUp, elevatorSubsystem::stop, elevatorSubsystem)); - driverXbox.b().whileTrue(Commands.startEnd(elevatorSubsystem::moveDown, elevatorSubsystem::stop, elevatorSubsystem)); + driverXbox.x().whileTrue(Commands.startEnd(() -> elevatorSubsystem.percentOut(0.5), elevatorSubsystem::stop, elevatorSubsystem)); + driverXbox.b().whileTrue(Commands.startEnd(() -> elevatorSubsystem.percentOut(-0.5), elevatorSubsystem::stop, elevatorSubsystem)); } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index ddec7de..8a20abc 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -50,4 +50,8 @@ public boolean isAtMin() { public boolean isAtMax() { return !maxLimitSwitch.get(); } + + public void percentOut(double speed) { + motor.set(ControlMode.PercentOutput, speed); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/JointSubsystem.java b/src/main/java/frc/robot/subsystems/JointSubsystem.java index 77afbff..e973ba0 100644 --- a/src/main/java/frc/robot/subsystems/JointSubsystem.java +++ b/src/main/java/frc/robot/subsystems/JointSubsystem.java @@ -1,67 +1,82 @@ package frc.robot.subsystems; +import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; +import com.ctre.phoenix.motorcontrol.can.VictorSPXConfiguration; + +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class JointSubsystem extends SubsystemBase { - private final TalonSRX JointLeftMotor; - private final TalonSRX JointRightMotor; + private final TalonSRX jointLeftMotor; + private final VictorSPX jointRightMotor; private final DutyCycleEncoder absoluteEncoder; + //private final PIDController pidController; private boolean absoluteEncoderConnected; private int absoluteEncoderFrequency; private double absoluteEncoderOutput; + private double absoluteEncoderPosition; + private double relativeEncoderPosition; public JointSubsystem() { - JointLeftMotor = new TalonSRX(13); - JointRightMotor = new TalonSRX(14); + jointLeftMotor = new TalonSRX(13); + jointRightMotor = new VictorSPX(14); absoluteEncoder = new DutyCycleEncoder(3, 1, 0); absoluteEncoder.setAssumedFrequency(975.6); - JointRightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10); - JointRightMotor.getSensorCollection().setQuadraturePosition(0, 10); + jointLeftMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 10); + //jointRightMotor.configSelectedFeedbackSensor(FeedbackDevice.PulseWidthEncodedPosition, 0, 10); + + jointLeftMotor.getSensorCollection().setQuadraturePosition((int) Math.round(absoluteEncoder.get()), 0); + + //pidController = new PIDController(relativeEncoderPosition, absoluteEncoderPosition, absoluteEncoderOutput, absoluteEncoderFrequency); - TalonSRXConfiguration configs = new TalonSRXConfiguration(); + TalonSRXConfiguration masterConfigs = new TalonSRXConfiguration(); + VictorSPXConfiguration slaveConfigs = new VictorSPXConfiguration(); - JointLeftMotor.configAllSettings(configs); - JointRightMotor.configAllSettings(configs); + jointLeftMotor.configAllSettings(masterConfigs); + jointRightMotor.configAllSettings(slaveConfigs); - JointLeftMotor.setNeutralMode(NeutralMode.Brake); - JointRightMotor.setNeutralMode(NeutralMode.Brake); + jointLeftMotor.setNeutralMode(NeutralMode.Brake); + jointRightMotor.setNeutralMode(NeutralMode.Brake); - JointLeftMotor.setInverted(true); - JointRightMotor.setInverted(false); + jointLeftMotor.setInverted(true); + jointRightMotor.setInverted(false); - JointRightMotor.follow(JointLeftMotor); + jointRightMotor.follow(jointLeftMotor); } public void percentOut(double speed) { - JointLeftMotor.set(TalonSRXControlMode.PercentOutput, speed); + jointLeftMotor.set(ControlMode.PercentOutput, speed); } public void stop() { - JointLeftMotor.set(TalonSRXControlMode.PercentOutput, 0); + jointLeftMotor.set(ControlMode.PercentOutput, 0); } @Override public void periodic() { absoluteEncoderConnected = absoluteEncoder.isConnected(); absoluteEncoderFrequency = absoluteEncoder.getFrequency(); - absoluteEncoderOutput = absoluteEncoder.get(); + absoluteEncoderOutput = (((absoluteEncoder.get()*100)-74)*36.7) - 308; + //absoluteEncoderOutput = absoluteEncoder.get(); + absoluteEncoderPosition = jointRightMotor.getSelectedSensorPosition(0); - relativeEncoderPosition = JointRightMotor.getSelectedSensorPosition(0); + relativeEncoderPosition = jointLeftMotor.getSelectedSensorPosition(0)*(-1); SmartDashboard.putBoolean("Joint Subsystem/Absolute Encoder/Connected", absoluteEncoderConnected); SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Frequency", absoluteEncoderFrequency); SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Output", absoluteEncoderOutput); + SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Position", absoluteEncoderPosition); SmartDashboard.putNumber("Joint Subsystem/Relative Encoder/Position", relativeEncoderPosition); } diff --git a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java index 01794fa..7a0ef83 100644 --- a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java @@ -1,9 +1,7 @@ package frc.robot.subsystems; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; -import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.can.VictorSPX; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; @@ -13,11 +11,9 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class PuncherSubsystem extends SubsystemBase { @@ -27,8 +23,6 @@ public class PuncherSubsystem extends SubsystemBase { private final TalonSRX baixo; - private final Servo servo; - private final DutyCycleOut percentOutCycle = new DutyCycleOut(0); private final PositionDutyCycle positionCycle = new PositionDutyCycle(0); @@ -36,10 +30,11 @@ public PuncherSubsystem() { puncherLeftMotor = new TalonFX(10); puncherRightMotor = new TalonFX(11); follower = new Follower(10, false); - - servo = new Servo(9); baixo = new TalonSRX(16); + puncherLeftMotor.setPosition(0); + puncherRightMotor.setPosition(0); + puncherLeftMotor.getConfigurator().apply(new TalonFXConfiguration()); puncherRightMotor.getConfigurator().apply(new TalonFXConfiguration()); @@ -94,4 +89,9 @@ public void stop() { public void setBaixo(double speed) { baixo.set(TalonSRXControlMode.PercentOutput,speed); } + + public void resetEncoders() { + puncherLeftMotor.setPosition(0); + puncherRightMotor.setPosition(0); + } } \ No newline at end of file From 83e835db2a2b9471400827b654318300a666da41 Mon Sep 17 00:00:00 2001 From: Pedro Henrique Date: Tue, 4 Mar 2025 10:28:32 -0300 Subject: [PATCH 12/21] commit --- .../frc/robot/buttonBindings/DriverButtonBindings.java | 7 ++++--- src/main/java/frc/robot/subsystems/PuncherSubsystem.java | 4 ++++ 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java index f05d7f8..ee5a754 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -97,7 +97,7 @@ public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem dr } public void configureBindings() { - //drivebaseDefaultButtonBindings(); + drivebaseDefaultButtonBindings(); //drivebaseSimulationButtonBindings(); //drivebaseTestButtonBindings(); intakeButtonBindings(); @@ -157,8 +157,9 @@ public void jointButtonBindings() { public void puncherButtonBindings() { driverXbox.b().whileTrue(Commands.startEnd(() -> puncherSubsystem.goToPosition(100), () -> puncherSubsystem.goToPosition(0), puncherSubsystem)); - driverXbox.povLeft().onTrue(Commands.startEnd(() -> puncherSubsystem.setBaixo(0.15), () -> puncherSubsystem.setBaixo(0), puncherSubsystem).withTimeout(2)); - driverXbox.povRight().onTrue(Commands.startEnd(() -> puncherSubsystem.setBaixo(-0.15), () -> puncherSubsystem.setBaixo(0), puncherSubsystem).withTimeout(2)); + driverXbox.povLeft().onTrue(Commands.startEnd(() -> puncherSubsystem.setBaixo(0.15), () -> puncherSubsystem.setBaixo(0), puncherSubsystem).withTimeout(1.5)); + driverXbox.povRight().onTrue(Commands.startEnd(() -> puncherSubsystem.setBaixo(-0.15), () -> puncherSubsystem.setBaixo(0), puncherSubsystem).withTimeout(1.5) + .andThen(Commands.startEnd(() -> puncherSubsystem.setBaixo(0.15), () -> puncherSubsystem.setBaixo(0), puncherSubsystem).withTimeout(1.5))); } public void elevatorButtonBindings() { diff --git a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java index 7a0ef83..d0056dd 100644 --- a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java @@ -70,6 +70,10 @@ public double getPosition() { return puncherLeftMotor.getPosition().getValueAsDouble(); } + public void resetPosition(){ + puncherLeftMotor.setPosition(null); + } + public void goToPosition(double position) { double clampedPosition = MathUtil.clamp(position, 0, 100); puncherLeftMotor.setControl(positionCycle.withPosition(clampedPosition)); From 8a757eace39d0d363f52686c1369174beb19ef7c Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Sat, 8 Mar 2025 07:10:13 -0300 Subject: [PATCH 13/21] commit --- simgui.json | 1 + src/main/deploy/swerve/modules/backleft.json | 2 +- src/main/deploy/swerve/modules/backright.json | 2 +- src/main/deploy/swerve/modules/frontleft.json | 2 +- .../deploy/swerve/modules/frontright.json | 2 +- .../deploy/swerve/modules/pidfproperties.json | 2 +- src/main/deploy/swerve/swervedrive.json | 3 +- src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 23 +- src/main/java/frc/robot/RobotState.java | 1 + .../buttonBindings/DriverButtonBindings.java | 219 ++++++++++++------ .../robot/subsystems/ElevatorSubsystem.java | 74 +++--- .../frc/robot/subsystems/IntakeSubsystem.java | 43 ++++ .../frc/robot/subsystems/JointSubsystem.java | 176 +++++++++++--- .../robot/subsystems/PuncherSubsystem.java | 93 +++++++- .../swervedrive/SwerveSubsystem.java | 4 +- 16 files changed, 493 insertions(+), 160 deletions(-) diff --git a/simgui.json b/simgui.json index c03b499..ff49fb0 100644 --- a/simgui.json +++ b/simgui.json @@ -83,6 +83,7 @@ "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", "/LiveWindow/Ungrouped/Pigeon 2 [13]": "Gyro", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", + "/Shuffleboard/Joint Subsystem/Closed Loop Command": "Command", "/SmartDashboard/Alerts": "Alerts", "/SmartDashboard/Encoders": "Alerts", "/SmartDashboard/Field": "Field2d", diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index 66bf66f..0850304 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -3,7 +3,7 @@ "front": -10.531, "left": 10.531 }, - "absoluteEncoderOffset": 82, + "absoluteEncoderOffset": 259.7, "drive": { "type": "sparkmax_neo", "id": 7, diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index f2af4ba..84f5270 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -3,7 +3,7 @@ "front": -10.531, "left": -10.531 }, - "absoluteEncoderOffset": 148.7, + "absoluteEncoderOffset": 275.8, "drive": { "type": "sparkmax_neo", "id": 5, diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index 63c41d4..4f98b16 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -3,7 +3,7 @@ "front": 10.531, "left": 10.531 }, - "absoluteEncoderOffset": 251.0, + "absoluteEncoderOffset": 255.1, "drive": { "type": "sparkmax_neo", "id": 1, diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index e84afa5..5e5ade0 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -3,7 +3,7 @@ "front": 10.531, "left": -10.531 }, - "absoluteEncoderOffset": 329.1, + "absoluteEncoderOffset": 320.1, "drive": { "type": "sparkmax_neo", "id": 3, diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json index 9b20fa1..25bf468 100644 --- a/src/main/deploy/swerve/modules/pidfproperties.json +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -7,7 +7,7 @@ "iz": 0 }, "angle": { - "p": 0.015, + "p": 0.025, "i": 0, "d": 0.2, "f": 0, diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json index e3f1854..1aacdbe 100644 --- a/src/main/deploy/swerve/swervedrive.json +++ b/src/main/deploy/swerve/swervedrive.json @@ -1,8 +1,9 @@ { "imu": { - "type": "navx_usb", + "type": "adxrs450", "id": 0, "canbus": null + }, "invertedIMU": false, "modules": [ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2aab5b6..0afaa06 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,6 +20,7 @@ public class Robot extends TimedRobot { public Robot() { instance = this; + enableLiveWindowInTest(true); } public static Robot getInstance() { @@ -43,6 +44,7 @@ public void robotPeriodic() { @Override public void disabledInit() { + m_robotContainer.resetPuncherSubsystemEncoders(); m_robotContainer.setMotorBrake(true); disabledTimer.reset(); disabledTimer.start(); @@ -56,8 +58,6 @@ public void disabledPeriodic() { disabledTimer.stop(); disabledTimer.reset(); } - - m_robotContainer.resetEncoders(); } @Override @@ -77,6 +77,8 @@ public void autonomousPeriodic() { @Override public void teleopInit() { + m_robotContainer.resetPuncherSubsystemEncoders(); + m_robotContainer.zeroGyro(); RobotState.alliance = DriverStation.getAlliance(); if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7320b18..b4da29b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -35,7 +36,7 @@ public class RobotContainer { private final DriverButtonBindings driverButtonBindings; private final OperatorButtonBindings operatorButtonBindings; - SendableChooser autoChooser; + SendableChooser autonomousChooser; public RobotContainer() { driverXbox = new CommandXboxController(0); @@ -53,8 +54,8 @@ public RobotContainer() { operatorButtonBindings = new OperatorButtonBindings(operatorXbox); configureBindings(); - autoChooser = new SendableChooser (); - autoSetup(); + autonomousChooser = new SendableChooser (); + autonomousChooserSetup(); DriverStation.silenceJoystickConnectionWarning(true); } @@ -65,14 +66,14 @@ private void configureBindings() { } public Command getAutonomousCommand() { - return autoChooser.getSelected(); + return autonomousChooser.getSelected(); } - public void autoSetup() { - SmartDashboard.putData(autoChooser); + public void autonomousChooserSetup() { + Shuffleboard.getTab("Autonomous").add("Choose Autonomous Routine", autonomousChooser); - autoChooser.setDefaultOption("No auto", new PrintCommand("No Auto Selected")); - autoChooser.addOption("Also no auto", new PrintCommand("Also No Auto Selected")); + autonomousChooser.setDefaultOption("No auto", new PrintCommand("No Auto Selected")); + autonomousChooser.addOption("Also no auto", new PrintCommand("Also No Auto Selected")); NamedCommands.registerCommand("test", Commands.print("I EXIST")); } @@ -81,7 +82,11 @@ public void setMotorBrake(boolean brake) { swerveSubsystem.setMotorBrake(brake); } - public void resetEncoders() { + public void resetPuncherSubsystemEncoders() { puncherSubsystem.resetEncoders(); } + + public void zeroGyro() { + swerveSubsystem.zeroGyro(); + } } diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index a90c354..d8c2f29 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -5,4 +5,5 @@ public class RobotState { public static Optional alliance = Optional.empty(); + public static boolean debugging = true; } diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java index ee5a754..bbc77cb 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -1,17 +1,16 @@ package frc.robot.buttonBindings; -import org.dyn4j.dynamics.joint.Joint; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.HIDConstants; import frc.robot.Robot; + import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.JointSubsystem; @@ -28,37 +27,60 @@ public class DriverButtonBindings { private final PuncherSubsystem puncherSubsystem; private final ElevatorSubsystem elevatorSubsystem; - private final SwerveInputStream driveDirectAngle; - private final SwerveInputStream driveRobotOriented; - private final SwerveInputStream driveAngularVelocityKeyboard; - private final SwerveInputStream driveDirectAngleKeyboard; - private final SwerveInputStream driveAngularVelocity; - - private final Command driveFieldOrientedDirectAngle; - private final Command driveFieldOrientedAnglularVelocity; - private final Command driveRobotOrientedAngularVelocity; - private final Command driveSetpointGen; - private final Command driveFieldOrientedDirectAngleKeyboard; - private final Command driveFieldOrientedAnglularVelocityKeyboard; - private final Command driveSetpointGenKeyboard; - - - public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem drivebase, - IntakeSubsystem intake, JointSubsystem joint, PuncherSubsystem puncher, ElevatorSubsystem elevatorSubsystem) { + private final SwerveInputStream driveDirectAngleSwerveInputStream; + private final SwerveInputStream driveRobotOrientedSwerveInputStream; + private final SwerveInputStream driveAngularVelocityKeyboardSwerveInputStream; + private final SwerveInputStream driveDirectAngleKeyboardSwerveInputStream; + private final SwerveInputStream driveAngularVelocitySwerveInputStream; + + private final Command driveFieldOrientedDirectAngleCommand; + private final Command driveFieldOrientedAngularVelocityCommand; + private final Command driveRobotOrientedAngularVelocityCommand; + private final Command driveSetpointGenCommand; + private final Command driveFieldOrientedDirectAngleKeyboardCommand; + private final Command driveFieldOrientedAngularVelocityKeyboardCommand; + private final Command driveSetpointGenKeyboardCommand; + + private final Command swerveZeroGyroCommand; + private final Command swerveLockPoseCommand; + private final Command swerveAddFakeVisionReadingCommand; + private final Command swerveCenterModulesCommand; + + private final Command intakeCommand; + private final Command outtakeCommand; + private final Command jointAscendCommand; + private final Command jointDescendCommand; + private final Command puncherBuildUpCommand; + private final Command puncherReleaseCommand; + private final Command elevatorAscendCommand; + private final Command elevatorDescendCommand; + + private final Command testPuncherBuildUpCommand; + private final Command testPuncherReleaseCommand; + private final Command testPuncherPrintCommand; + private final Command testIntakeCommand; + private final Command testOuttakeCommand; + private final Command testElevatorAscendCommand; + private final Command testElevatorDescendCommand; + private final Command testJointCommand; + + + public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem swerveSubsystem, + IntakeSubsystem intakeSubsystem, JointSubsystem jointSubsystem, PuncherSubsystem puncherSubsystem, ElevatorSubsystem elevatorSubsystem) { this.driverXbox = driverXbox; - this.swerveSubsystem = drivebase; - this.intakeSubsystem = intake; - this.jointSubsystem = joint; - this.puncherSubsystem = puncher; + this.swerveSubsystem = swerveSubsystem; + this.intakeSubsystem = intakeSubsystem; + this.jointSubsystem = jointSubsystem; + this.puncherSubsystem = puncherSubsystem; this.elevatorSubsystem = elevatorSubsystem; - driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> driverXbox.getLeftY() * -1, - () -> driverXbox.getLeftX() * -1) + driveAngularVelocitySwerveInputStream = SwerveInputStream.of(this.swerveSubsystem.getSwerveDrive(), + () -> this.driverXbox.getLeftY() * -1, + () -> this.driverXbox.getLeftX() * -1) .withControllerRotationAxis(() -> { - double leftTrigger = driverXbox.getLeftTriggerAxis(); - double rightTrigger = driverXbox.getRightTriggerAxis(); + double leftTrigger = this.driverXbox.getLeftTriggerAxis(); + double rightTrigger = this.driverXbox.getRightTriggerAxis(); double rotation = rightTrigger - leftTrigger; return rotation; }) @@ -66,65 +88,92 @@ public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem dr .scaleTranslation(0.8) .allianceRelativeControl(true); - driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis - (driverXbox::getRightX, driverXbox::getRightY).headingWhile(true); + driveDirectAngleSwerveInputStream = driveAngularVelocitySwerveInputStream.copy().withControllerHeadingAxis + (this.driverXbox::getRightX, this.driverXbox::getRightY).headingWhile(true); - driveRobotOriented = driveAngularVelocity.copy().robotRelative(true) + driveRobotOrientedSwerveInputStream = driveAngularVelocitySwerveInputStream.copy().robotRelative(true) .allianceRelativeControl(false); - driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX()) - .withControllerRotationAxis(() -> driverXbox.getRawAxis( + driveAngularVelocityKeyboardSwerveInputStream = SwerveInputStream.of(this.swerveSubsystem.getSwerveDrive(), + () -> -this.driverXbox.getLeftY(), + () -> -this.driverXbox.getLeftX()) + .withControllerRotationAxis(() -> this.driverXbox.getRawAxis( 2)) .deadband(HIDConstants.DEADBAND) .scaleTranslation(0.8) .allianceRelativeControl(true); - driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() + driveDirectAngleKeyboardSwerveInputStream = driveAngularVelocityKeyboardSwerveInputStream.copy() .withControllerHeadingAxis(() -> Math.sin( - driverXbox.getRawAxis(2) * Math.PI) * (Math.PI * 2), - () -> Math.cos(driverXbox.getRawAxis(2) * Math.PI) * (Math.PI * 2)) + this.driverXbox.getRawAxis(2) * Math.PI) * (Math.PI * 2), + () -> Math.cos(this.driverXbox.getRawAxis(2) * Math.PI) * (Math.PI * 2)) .headingWhile(true); - driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle); - driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); - driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); - driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngle); - driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard); - driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard); - driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngleKeyboard); + driveFieldOrientedDirectAngleCommand = this.swerveSubsystem.driveFieldOriented(driveDirectAngleSwerveInputStream); + driveFieldOrientedAngularVelocityCommand = this.swerveSubsystem.driveFieldOriented(driveAngularVelocitySwerveInputStream); + driveRobotOrientedAngularVelocityCommand = this.swerveSubsystem.driveFieldOriented(driveRobotOrientedSwerveInputStream); + driveSetpointGenCommand = this.swerveSubsystem.driveWithSetpointGeneratorFieldRelative(driveDirectAngleSwerveInputStream); + driveFieldOrientedDirectAngleKeyboardCommand = this.swerveSubsystem.driveFieldOriented(driveDirectAngleKeyboardSwerveInputStream); + driveFieldOrientedAngularVelocityKeyboardCommand = this.swerveSubsystem.driveFieldOriented(driveAngularVelocityKeyboardSwerveInputStream); + driveSetpointGenKeyboardCommand = this.swerveSubsystem.driveWithSetpointGeneratorFieldRelative(driveDirectAngleKeyboardSwerveInputStream); + + swerveZeroGyroCommand = Commands.runOnce(this.swerveSubsystem::zeroGyro); + swerveLockPoseCommand = Commands.runOnce(this.swerveSubsystem::lock, this.swerveSubsystem).repeatedly(); + swerveAddFakeVisionReadingCommand = Commands.runOnce(this.swerveSubsystem::addFakeVisionReading); + swerveCenterModulesCommand = this.swerveSubsystem.centerModulesCommand(); + + intakeCommand = this.intakeSubsystem.percentOutCommand(() -> -0.3); + outtakeCommand = this.intakeSubsystem.percentOutCommand(() -> 0.6); + jointAscendCommand = this.jointSubsystem.percentOutCommand(() -> -0.85); + jointDescendCommand = this.jointSubsystem.percentOutCommand(() -> 0.7); + puncherBuildUpCommand = this.puncherSubsystem.buildUpCommand(() -> 45); + puncherReleaseCommand = this.puncherSubsystem.releaseCommand(() -> 0.3, () -> 0.31, () -> 1.1); + elevatorAscendCommand = this.elevatorSubsystem.percentOutCommand(() -> 0.5); + elevatorDescendCommand = this.elevatorSubsystem.percentOutCommand(() -> -0.5); + + testPuncherBuildUpCommand = this.puncherSubsystem.testBuildUpCommand(); + testPuncherReleaseCommand = this.puncherSubsystem.testReleaseCommand(); + testPuncherPrintCommand = this.puncherSubsystem.testPrintCommand(); + testIntakeCommand = this.intakeSubsystem.testIntakeCommand(); + testOuttakeCommand = this.intakeSubsystem.testOuttakeCommand(); + testElevatorAscendCommand = this.elevatorSubsystem.testAscendCommand(); + testElevatorDescendCommand = this.elevatorSubsystem.testDescendCommand(); + testJointCommand = this.jointSubsystem.testOpenLoopCommand(); } public void configureBindings() { - drivebaseDefaultButtonBindings(); + //drivebaseDefaultButtonBindings(); //drivebaseSimulationButtonBindings(); //drivebaseTestButtonBindings(); - intakeButtonBindings(); - jointButtonBindings(); - puncherButtonBindings(); - //elevatorButtonBindings(); + + if(frc.robot.RobotState.debugging) { + intakeTestButtonBindings(); + jointTestButtonBindings(); + puncherTestButtonBindings(); + elevatorTestButtonBindings(); + } else { + intakeButtonBindings(); + jointButtonBindings(); + puncherButtonBindings(); + elevatorButtonBindings(); + } } public void drivebaseDefaultButtonBindings() { if (!RobotBase.isSimulation()) { - swerveSubsystem.setDefaultCommand(driveFieldOrientedAnglularVelocity); + swerveSubsystem.setDefaultCommand(driveFieldOrientedDirectAngleCommand); + driverXbox.leftBumper().whileTrue(driveRobotOrientedAngularVelocityCommand); } if (!DriverStation.isTest()) { - driverXbox.a().onTrue((Commands.runOnce(swerveSubsystem::zeroGyro))); - driverXbox.x().onTrue(Commands.runOnce(swerveSubsystem::addFakeVisionReading)); - driverXbox.b().whileTrue(swerveSubsystem.driveToPose(new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))); - driverXbox.start().whileTrue(Commands.none()); - driverXbox.back().whileTrue(Commands.none()); - driverXbox.leftBumper().whileTrue(Commands.runOnce(swerveSubsystem::lock, swerveSubsystem).repeatedly()); - driverXbox.rightBumper().onTrue(Commands.none()); + driverXbox.start().onTrue(swerveZeroGyroCommand); + driverXbox.leftStick().whileTrue(swerveLockPoseCommand); } } public void drivebaseSimulationButtonBindings() { if (RobotBase.isSimulation()) { - swerveSubsystem.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard); + swerveSubsystem.setDefaultCommand(driveFieldOrientedDirectAngleKeyboardCommand); } if (Robot.isSimulation()) { @@ -135,35 +184,51 @@ public void drivebaseSimulationButtonBindings() { public void drivebaseTestButtonBindings() { if (DriverStation.isTest()) { - swerveSubsystem.setDefaultCommand(driveFieldOrientedAnglularVelocity); - driverXbox.x().whileTrue(Commands.runOnce(swerveSubsystem::lock, swerveSubsystem).repeatedly()); - driverXbox.y().whileTrue(swerveSubsystem.driveToDistanceCommand(1.0, 0.2)); - driverXbox.start().onTrue((Commands.runOnce(swerveSubsystem::zeroGyro))); - driverXbox.back().whileTrue(swerveSubsystem.centerModulesCommand()); - driverXbox.leftBumper().onTrue(Commands.none()); - driverXbox.rightBumper().onTrue(Commands.none()); + swerveSubsystem.setDefaultCommand(driveFieldOrientedAngularVelocityCommand); + driverXbox.x().whileTrue(swerveLockPoseCommand); + driverXbox.start().onTrue(swerveZeroGyroCommand); + driverXbox.back().whileTrue(swerveCenterModulesCommand); + driverXbox.x().onTrue(swerveAddFakeVisionReadingCommand); } } + public void intakeTestButtonBindings() { + driverXbox.y().whileTrue(testIntakeCommand); + driverXbox.x().whileTrue(testOuttakeCommand); + } + + public void jointTestButtonBindings() { + driverXbox.a().whileTrue(testJointCommand); + } + + public void puncherTestButtonBindings() { + driverXbox.rightTrigger().whileTrue(testPuncherBuildUpCommand); + driverXbox.rightBumper().onTrue(testPuncherReleaseCommand); + driverXbox.leftBumper().onTrue(testPuncherPrintCommand); + } + + public void elevatorTestButtonBindings() { + driverXbox.x().whileTrue(testElevatorAscendCommand); + driverXbox.b().whileTrue(testElevatorDescendCommand); + } + public void intakeButtonBindings() { - driverXbox.y().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(0.6), () -> intakeSubsystem.stop(), intakeSubsystem)); - driverXbox.x().whileTrue(Commands.startEnd(() -> intakeSubsystem.percentOut(-0.6), () -> intakeSubsystem.stop(), intakeSubsystem)); + driverXbox.y().whileTrue(intakeCommand); + driverXbox.x().whileTrue(outtakeCommand); } public void jointButtonBindings() { - driverXbox.povUp().whileTrue(Commands.startEnd(() -> jointSubsystem.percentOut(0.5), () -> jointSubsystem.stop(), jointSubsystem)); - driverXbox.povDown().whileTrue(Commands.startEnd(() -> jointSubsystem.percentOut(-0.5), () -> jointSubsystem.stop(), jointSubsystem)); + driverXbox.povUp().whileTrue(jointAscendCommand); + driverXbox.povDown().whileTrue(jointDescendCommand); } public void puncherButtonBindings() { - driverXbox.b().whileTrue(Commands.startEnd(() -> puncherSubsystem.goToPosition(100), () -> puncherSubsystem.goToPosition(0), puncherSubsystem)); - driverXbox.povLeft().onTrue(Commands.startEnd(() -> puncherSubsystem.setBaixo(0.15), () -> puncherSubsystem.setBaixo(0), puncherSubsystem).withTimeout(1.5)); - driverXbox.povRight().onTrue(Commands.startEnd(() -> puncherSubsystem.setBaixo(-0.15), () -> puncherSubsystem.setBaixo(0), puncherSubsystem).withTimeout(1.5) - .andThen(Commands.startEnd(() -> puncherSubsystem.setBaixo(0.15), () -> puncherSubsystem.setBaixo(0), puncherSubsystem).withTimeout(1.5))); + driverXbox.rightTrigger().whileTrue(puncherBuildUpCommand); + driverXbox.rightBumper().onTrue(puncherReleaseCommand); } public void elevatorButtonBindings() { - driverXbox.x().whileTrue(Commands.startEnd(() -> elevatorSubsystem.percentOut(0.5), elevatorSubsystem::stop, elevatorSubsystem)); - driverXbox.b().whileTrue(Commands.startEnd(() -> elevatorSubsystem.percentOut(-0.5), elevatorSubsystem::stop, elevatorSubsystem)); + driverXbox.x().whileTrue(elevatorAscendCommand); + driverXbox.b().whileTrue(elevatorDescendCommand); } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 8a20abc..c84b49d 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -1,41 +1,36 @@ package frc.robot.subsystems; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotState; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.can.VictorSPX; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix.motorcontrol.ControlMode; public class ElevatorSubsystem extends SubsystemBase { private final TalonSRX motor; private final DigitalInput minLimitSwitch; - private final DigitalInput maxLimitSwitch; - - private static final double SPEED_UP = 0.0; - private static final double SPEED_DOWN = 0.0; + + private GenericEntry ascendDutyCycleOutValue; + private GenericEntry descendDutyCycleOutValue; public ElevatorSubsystem() { motor = new TalonSRX(15); minLimitSwitch = new DigitalInput(0); - maxLimitSwitch = new DigitalInput(1); - } - - public void moveUp() { - if (!this.isAtMax()) { - motor.set(ControlMode.PercentOutput, SPEED_UP); - } else { - stop(); - } - } - - public void moveDown() { - if (!this.isAtMin()) { - motor.set(ControlMode.PercentOutput, SPEED_DOWN); - } else { - stop(); + + if(RobotState.debugging) { + ascendDutyCycleOutValue = Shuffleboard.getTab("Elevator Subsystem").add("Ascend: DutyCycleOut", 0) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + descendDutyCycleOutValue = Shuffleboard.getTab("Elevator Subsystem").add("Descend: DutyCycleOut", 0) + .withWidget(BuiltInWidgets.kTextView).getEntry(); } } @@ -44,14 +39,37 @@ public void stop() { } public boolean isAtMin() { - return !minLimitSwitch.get(); - } - - public boolean isAtMax() { - return !maxLimitSwitch.get(); + return minLimitSwitch.get(); } public void percentOut(double speed) { motor.set(ControlMode.PercentOutput, speed); } + + public Command percentOutCommand(DoubleSupplier speed) { + return Commands.startEnd(() -> this.percentOut(speed.getAsDouble()), this::stop, this); + } + + public Command descendCommand() { + return Commands.startEnd(() -> this.percentOut(-1), this::stop, this).until(this::isAtMin); + } + + public Command testDescendCommand() { + return Commands.startEnd(() -> this.percentOut(descendDutyCycleOutValue.getDouble(0)*(-1)), this::stop, this).until(this::isAtMin); + } + + public Command testAscendCommand() { + return Commands.startEnd(() -> this.percentOut(ascendDutyCycleOutValue.getDouble(0)), this::stop, this); + } + + public void debug() { + + } + + @Override + public void periodic() { + if(RobotState.debugging) { + this.debug(); + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 854f913..07bd2a0 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -1,13 +1,25 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotState; public class IntakeSubsystem extends SubsystemBase { private final TalonSRX intakeMotor; + + private GenericEntry intakeDutyCycleValue; + private GenericEntry outtakeDutyCycleValue; public IntakeSubsystem() { intakeMotor = new TalonSRX(12); @@ -17,6 +29,14 @@ public IntakeSubsystem() { intakeMotor.configAllSettings(configs); intakeMotor.setNeutralMode(NeutralMode.Coast); intakeMotor.setInverted(false); + + if(RobotState.debugging) { + intakeDutyCycleValue = Shuffleboard.getTab("Intake Subsystem").add("Intake: DutyCycleOut", 0.3) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + outtakeDutyCycleValue = Shuffleboard.getTab("Intake Subsystem").add("Outtake: DutyCycleOut", 0.6) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + } } public void percentOut(double speed) { @@ -26,4 +46,27 @@ public void percentOut(double speed) { public void stop() { intakeMotor.set(TalonSRXControlMode.PercentOutput, 0); } + + public Command percentOutCommand(DoubleSupplier speed) { + return Commands.startEnd(() -> this.percentOut(speed.getAsDouble()), this::stop, this); + } + + public Command testIntakeCommand() { + return Commands.startEnd(() -> this.percentOut(-intakeDutyCycleValue.getDouble(0.3)), this::stop, this); + } + + public Command testOuttakeCommand() { + return Commands.startEnd(() -> this.percentOut(outtakeDutyCycleValue.getDouble(0.6)), this::stop, this); + } + + public void debug() { + + } + + @Override + public void periodic() { + if(RobotState.debugging) { + this.debug(); + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/JointSubsystem.java b/src/main/java/frc/robot/subsystems/JointSubsystem.java index e973ba0..0a72901 100644 --- a/src/main/java/frc/robot/subsystems/JointSubsystem.java +++ b/src/main/java/frc/robot/subsystems/JointSubsystem.java @@ -1,44 +1,76 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; -import com.ctre.phoenix.motorcontrol.can.VictorSPX; import com.ctre.phoenix.motorcontrol.can.VictorSPXConfiguration; - +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotState; public class JointSubsystem extends SubsystemBase { - private final TalonSRX jointLeftMotor; - private final VictorSPX jointRightMotor; + private final WPI_TalonSRX jointLeftMotor; + private final WPI_VictorSPX jointRightMotor; private final DutyCycleEncoder absoluteEncoder; - //private final PIDController pidController; + + private final PIDController masterController; + private final PIDController slaveController; private boolean absoluteEncoderConnected; private int absoluteEncoderFrequency; - private double absoluteEncoderOutput; - private double absoluteEncoderPosition; + private double absoluteEncoderRaw; + private double absoluteEncoderAdjusted; + + private double analogEncoderRaw; + private double analogEncoderAdjusted; + + private GenericEntry absoluteEncoderMultiplierValue; + private GenericEntry absoluteEncoderOffsetValue; + + private GenericEntry analogEncoderMultiplierValue; + private GenericEntry analogEncoderOffsetValue; - private double relativeEncoderPosition; + private GenericEntry setpointValue; + + private GenericEntry masterControllerProportionalValue; + private GenericEntry masterControllerIntegralValue; + private GenericEntry masterControllerDerivativeValue; + + private GenericEntry slaveControllerProportionalValue; + private GenericEntry slaveControllerIntegralValue; + private GenericEntry slaveControllerDerivativeValue; + + private GenericEntry dutyCycleOutValue; public JointSubsystem() { - jointLeftMotor = new TalonSRX(13); - jointRightMotor = new VictorSPX(14); + jointLeftMotor = new WPI_TalonSRX(13); + jointRightMotor = new WPI_VictorSPX(14); - absoluteEncoder = new DutyCycleEncoder(3, 1, 0); + absoluteEncoder = new DutyCycleEncoder(3, 100, 0); absoluteEncoder.setAssumedFrequency(975.6); - jointLeftMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 10); - //jointRightMotor.configSelectedFeedbackSensor(FeedbackDevice.PulseWidthEncodedPosition, 0, 10); + //jointLeftMotor.getSensorCollection().setQuadraturePosition((int) Math.round(absoluteEncoder.get()), 0); - jointLeftMotor.getSensorCollection().setQuadraturePosition((int) Math.round(absoluteEncoder.get()), 0); + masterController = new PIDController(0, 0, 0); + masterController.setTolerance(2); - //pidController = new PIDController(relativeEncoderPosition, absoluteEncoderPosition, absoluteEncoderOutput, absoluteEncoderFrequency); + slaveController = new PIDController(0, 0, 0); + slaveController.setTolerance(0.2); TalonSRXConfiguration masterConfigs = new TalonSRXConfiguration(); VictorSPXConfiguration slaveConfigs = new VictorSPXConfiguration(); @@ -46,38 +78,128 @@ public JointSubsystem() { jointLeftMotor.configAllSettings(masterConfigs); jointRightMotor.configAllSettings(slaveConfigs); + jointLeftMotor.configSelectedFeedbackSensor(FeedbackDevice.Analog, 0, 0); + jointLeftMotor.configSelectedFeedbackSensor(FeedbackDevice.None, 1, 0); + jointLeftMotor.setNeutralMode(NeutralMode.Brake); jointRightMotor.setNeutralMode(NeutralMode.Brake); jointLeftMotor.setInverted(true); jointRightMotor.setInverted(false); - jointRightMotor.follow(jointLeftMotor); + //jointRightMotor.follow(jointLeftMotor); + + if(RobotState.debugging) { + absoluteEncoderMultiplierValue = Shuffleboard.getTab("Joint Subsystem").add("Absolute Encoder: Multipler", 1) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + absoluteEncoderOffsetValue = Shuffleboard.getTab("Joint Subsystem").add("Absolute Encoder: Offset", 0) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + analogEncoderMultiplierValue = Shuffleboard.getTab("Joint Subsystem").add("Analog Encoder: Multipler", 1) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + analogEncoderOffsetValue = Shuffleboard.getTab("Joint Subsystem").add("Analog Encoder: Offset", 0) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + setpointValue = Shuffleboard.getTab("Joint Subsystem").add("PID Controller: Setpoint", 0) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + masterControllerProportionalValue = Shuffleboard.getTab("Joint Subsystem").add("Master: P", 0) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + masterControllerIntegralValue = Shuffleboard.getTab("Joint Subsystem").add("Master: I", 0) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + masterControllerDerivativeValue = Shuffleboard.getTab("Joint Subsystem").add("Master: D", 0) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + slaveControllerProportionalValue = Shuffleboard.getTab("Joint Subsystem").add("Slave: P", 0) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + slaveControllerIntegralValue = Shuffleboard.getTab("Joint Subsystem").add("Slave: I", 0) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + slaveControllerDerivativeValue = Shuffleboard.getTab("Joint Subsystem").add("Slave: D", 0) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + dutyCycleOutValue = Shuffleboard.getTab("Joint Subsystem").add("DutyCycleOut", 0) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + Shuffleboard.getTab("Joint Subsystem").add("Closed Loop Command", testClosedLoopCommand()); + Shuffleboard.getTab("Joint Subsystem").add("Update Controllers Parameters Command", testUpdateClosedLoopControllersCommand()); + } } public void percentOut(double speed) { jointLeftMotor.set(ControlMode.PercentOutput, speed); + jointRightMotor.set(ControlMode.PercentOutput, speed); } public void stop() { jointLeftMotor.set(ControlMode.PercentOutput, 0); + jointRightMotor.set(ControlMode.PercentOutput, 0); } - @Override - public void periodic() { + public boolean atSetpoint() { + return masterController.atSetpoint() && slaveController.atSetpoint(); + } + + public void masterClosedLoop(double setpoint) { + jointLeftMotor.set(TalonSRXControlMode.Position, masterController.calculate(analogEncoderAdjusted, setpoint)); + } + + public void slaveClosedLoop() { + jointRightMotor.set(VictorSPXControlMode.PercentOutput, slaveController.calculate(absoluteEncoderAdjusted, analogEncoderAdjusted)); + } + + public void positionClosedLoop(double setpoint) { + masterClosedLoop(setpoint); + slaveClosedLoop(); + } + + public Command percentOutCommand(DoubleSupplier speed) { + return Commands.startEnd(() -> this.percentOut(speed.getAsDouble()), this::stop, this); + } + + public Command closedLoopCommand(DoubleSupplier speed) { + return Commands.startEnd(() -> this.positionClosedLoop(speed.getAsDouble()), this::stop, this); + } + + public Command testClosedLoopCommand() { + return Commands.startEnd(() -> this.positionClosedLoop(setpointValue.getDouble(0)), this::stop, this).until(this::atSetpoint); + } + + public void updateClosedLoopControllersParameters() { + masterController.setPID(masterControllerProportionalValue.getDouble(0), masterControllerIntegralValue.getDouble(0), masterControllerDerivativeValue.getDouble(0)); + slaveController.setPID(slaveControllerProportionalValue.getDouble(0), slaveControllerIntegralValue.getDouble(0), slaveControllerDerivativeValue.getDouble(0)); + } + + public Command testOpenLoopCommand() { + return this.runEnd(() -> this.percentOut(dutyCycleOutValue.getDouble(0)), this::stop); + } + + public Command testUpdateClosedLoopControllersCommand() { + return new InstantCommand(this::updateClosedLoopControllersParameters, this); + } + + public void debug() { absoluteEncoderConnected = absoluteEncoder.isConnected(); absoluteEncoderFrequency = absoluteEncoder.getFrequency(); - absoluteEncoderOutput = (((absoluteEncoder.get()*100)-74)*36.7) - 308; - //absoluteEncoderOutput = absoluteEncoder.get(); - absoluteEncoderPosition = jointRightMotor.getSelectedSensorPosition(0); - - relativeEncoderPosition = jointLeftMotor.getSelectedSensorPosition(0)*(-1); + absoluteEncoderAdjusted = ((absoluteEncoder.get()) + absoluteEncoderOffsetValue.getDouble(0))*(absoluteEncoderMultiplierValue.getDouble(1)); + absoluteEncoderRaw = (absoluteEncoder.get()); + analogEncoderAdjusted = (jointLeftMotor.getSelectedSensorPosition(0) + analogEncoderOffsetValue.getDouble(0))*(analogEncoderMultiplierValue.getDouble(1)); + analogEncoderRaw = jointLeftMotor.getSelectedSensorPosition(0); SmartDashboard.putBoolean("Joint Subsystem/Absolute Encoder/Connected", absoluteEncoderConnected); SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Frequency", absoluteEncoderFrequency); - SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Output", absoluteEncoderOutput); - SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Position", absoluteEncoderPosition); + SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Adjusted", absoluteEncoderAdjusted); + SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Raw", absoluteEncoderRaw); + SmartDashboard.putNumber("Joint Subsystem/Analog Encoder/Adjusted", analogEncoderAdjusted); + SmartDashboard.putNumber("Joint Subsystem/Analog Encoder/Raw", analogEncoderRaw); + SmartDashboard.putBoolean("Joint Subsystem/Encoders/Is Matching", analogEncoderAdjusted == absoluteEncoderAdjusted); + SmartDashboard.putNumber("Joint Subsystem/Encoders/Get Difference", analogEncoderAdjusted - absoluteEncoderAdjusted); + } - SmartDashboard.putNumber("Joint Subsystem/Relative Encoder/Position", relativeEncoderPosition); + @Override + public void periodic() { + if(RobotState.debugging) { + this.debug(); + } } } diff --git a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java index d0056dd..2193765 100644 --- a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java @@ -1,7 +1,10 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; @@ -13,24 +16,35 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.MathUtil; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotState; public class PuncherSubsystem extends SubsystemBase { private final TalonFX puncherLeftMotor; private final TalonFX puncherRightMotor; private final Follower follower; - private final TalonSRX baixo; + private final TalonSRX puncherReleaseMotor; private final DutyCycleOut percentOutCycle = new DutyCycleOut(0); private final PositionDutyCycle positionCycle = new PositionDutyCycle(0); + private GenericEntry setpointValue; + private GenericEntry releaseTimeValue; + private GenericEntry releaseDutyCycleValue; + private GenericEntry tightenDutyCycleValue; + public PuncherSubsystem() { puncherLeftMotor = new TalonFX(10); puncherRightMotor = new TalonFX(11); follower = new Follower(10, false); - baixo = new TalonSRX(16); + puncherReleaseMotor = new TalonSRX(16); puncherLeftMotor.setPosition(0); puncherRightMotor.setPosition(0); @@ -64,6 +78,29 @@ public PuncherSubsystem() { limitSwitchConfigs.ReverseSoftLimitThreshold = 0; masterConfig.apply(limitSwitchConfigs); slaveConfig.apply(limitSwitchConfigs); + + CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs(); + currentLimitsConfigs.StatorCurrentLimitEnable = false; + currentLimitsConfigs.SupplyCurrentLimitEnable = true; + currentLimitsConfigs.SupplyCurrentLimit = 35; + currentLimitsConfigs.SupplyCurrentLowerLimit = 30; + currentLimitsConfigs.SupplyCurrentLowerTime = 1; + masterConfig.apply(currentLimitsConfigs); + slaveConfig.apply(currentLimitsConfigs); + + if(RobotState.debugging) { + setpointValue = Shuffleboard.getTab("Puncher Subsystem").add("Setpoint", 45) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + releaseTimeValue = Shuffleboard.getTab("Puncher Subsystem").add("Release and Tighten: Timer", 1.1) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + releaseDutyCycleValue = Shuffleboard.getTab("Puncher Subsystem").add("Release: DutyCycleOut", 0.3) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + tightenDutyCycleValue = Shuffleboard.getTab("Puncher Subsystem").add("Tighten: DutyCycleOut", 0.31) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + } } public double getPosition() { @@ -71,12 +108,11 @@ public double getPosition() { } public void resetPosition(){ - puncherLeftMotor.setPosition(null); + puncherLeftMotor.setPosition(0); } public void goToPosition(double position) { - double clampedPosition = MathUtil.clamp(position, 0, 100); - puncherLeftMotor.setControl(positionCycle.withPosition(clampedPosition)); + puncherLeftMotor.setControl(positionCycle.withPosition(position)); puncherRightMotor.setControl(follower); } @@ -90,12 +126,53 @@ public void stop() { puncherRightMotor.stopMotor(); } - public void setBaixo(double speed) { - baixo.set(TalonSRXControlMode.PercentOutput,speed); + public void setRelease(double speed) { + puncherReleaseMotor.set(TalonSRXControlMode.PercentOutput, speed); + } + + public void stopRelease() { + puncherReleaseMotor.set(TalonSRXControlMode.PercentOutput, 0); } public void resetEncoders() { puncherLeftMotor.setPosition(0); puncherRightMotor.setPosition(0); } + + public Command buildUpCommand(DoubleSupplier position) { + return Commands.startEnd(() -> this.goToPosition(position.getAsDouble()), () -> this.goToPosition(0), this); + } + + public Command releaseCommand(DoubleSupplier speed1, DoubleSupplier speed2, DoubleSupplier time) { + return Commands.startEnd(() -> this.setRelease(-speed1.getAsDouble()), this::stopRelease, this) + .withTimeout(time.getAsDouble()).andThen + (Commands.startEnd(() -> this.setRelease(speed2.getAsDouble()), this::stopRelease, this) + .withTimeout(time.getAsDouble())); + } + + public Command testPrintCommand() { + return new InstantCommand(() -> System.out.println(setpointValue.getDouble(40)), this).ignoringDisable(true); + } + + public Command testBuildUpCommand() { + return Commands.startEnd(() -> this.goToPosition(setpointValue.getDouble(45)), () -> this.goToPosition(0), this); + } + + public Command testReleaseCommand() { + return Commands.startEnd(() -> this.setRelease(-releaseDutyCycleValue.getDouble(0.3)), this::stopRelease, this) + .withTimeout(releaseTimeValue.getDouble(1.1)).andThen + (Commands.startEnd(() -> this.setRelease(tightenDutyCycleValue.getDouble(0.31)), this::stopRelease, this) + .withTimeout(releaseTimeValue.getDouble(1.1))); + } + + public void debug() { + + } + + @Override + public void periodic() { + if(RobotState.debugging) { + this.debug(); + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 851a06d..a881239 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -21,7 +21,6 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -29,7 +28,6 @@ import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -104,7 +102,7 @@ public SwerveSubsystem(File directory) 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1. swerveDrive.setModuleEncoderAutoSynchronize(false, 1); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. -// swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible + swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible if (visionDriveTest) { setupPhotonVision(); From 45964b3f6ed11f2317061475d31dbd33ae84f796 Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Sun, 9 Mar 2025 16:12:47 -0300 Subject: [PATCH 14/21] commit --- src/main/deploy/swerve/modules/backleft.json | 4 +- src/main/deploy/swerve/modules/backright.json | 2 +- src/main/java/frc/robot/Constants.java | 46 +++++++++++++++---- src/main/java/frc/robot/Robot.java | 5 +- src/main/java/frc/robot/RobotState.java | 9 ---- .../buttonBindings/DriverButtonBindings.java | 30 ++++++------ .../robot/subsystems/ElevatorSubsystem.java | 6 +-- .../frc/robot/subsystems/IntakeSubsystem.java | 6 +-- .../frc/robot/subsystems/JointSubsystem.java | 6 +-- .../robot/subsystems/PuncherSubsystem.java | 18 +++++--- .../swervedrive/SwerveSubsystem.java | 2 +- 11 files changed, 78 insertions(+), 56 deletions(-) delete mode 100644 src/main/java/frc/robot/RobotState.java diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index 0850304..bb163ab 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -3,14 +3,14 @@ "front": -10.531, "left": 10.531 }, - "absoluteEncoderOffset": 259.7, + "absoluteEncoderOffset": 269, "drive": { "type": "sparkmax_neo", "id": 7, "canbus": null }, "inverted": { - "drive": true, + "drive": false, "angle": false }, "angle": { diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index 84f5270..cc2d8d3 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -3,7 +3,7 @@ "front": -10.531, "left": -10.531 }, - "absoluteEncoderOffset": 275.8, + "absoluteEncoderOffset": 274, "drive": { "type": "sparkmax_neo", "id": 5, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0183fa5..27789c6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,15 +9,11 @@ import swervelib.math.Matter; public final class Constants { - public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound + public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); - public static final double LOOP_TIME = 0.13; //s, 20ms + 110ms sprk max velocity lag + public static final double LOOP_TIME = 0.13; public static final double MAX_SPEED = Units.feetToMeters(14.5); - // Maximum speed of the robot in meters per second, used to limit acceleration. - - public static final class DrivebaseConstants { - public static final double WHEEL_LOCK_TIME = 10; // seconds - } + public static final boolean DEV_MODE = true; public static class HIDConstants { public static final double DEADBAND = 0.1; @@ -25,4 +21,38 @@ public static class HIDConstants { public static final double RIGHT_X_DEADBAND = 0.1; public static final double TURN_CONSTANT = 6; } -} + + public static final class SwerveConstants { + public static final double WHEEL_LOCK_TIME = 10; // seconds + } + + public static class IntakeConstants { + public static final int MOTOR_CAN_ID = 10; + public static final double INTAKE = 0.3; + public static final double OUTTAKE = -0.6; + } + + public static class JointConstants { + public static final int LEFT_MOTOR_CAN_ID = 13; + public static final int RIGHT_MOTOR_CAN_ID = 14; + public static final int RIGHT_ENCODER_DIO_PORT = 3; + public static final double ASCEND = -0.85; + public static final double DESCEND = 0.7; + } + + public static class PuncherConstants { + public static final int LEFT_MOTOR_CAN_ID = 13; + public static final int RIGHT_MOTOR_CAN_ID = 14; + public static final double POSITION = 45; + public static final double RELEASE = 0.3; + public static final double TIGHTEN = 0.31; + public static final double TIMER = 1.1; + } + + public static class ElevatorConstants { + public static final int MOTOR_CAN_ID = 16; + public static final int LIMIT_SWITCH_DIO_PORT = 0; + public static final double ASCEND = -0.3; + public static final double DESCEND = 0.3; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0afaa06..cb768b7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -52,8 +52,7 @@ public void disabledInit() { @Override public void disabledPeriodic() { - RobotState.alliance = DriverStation.getAlliance(); - if (disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) { + if (disabledTimer.hasElapsed(Constants.SwerveConstants.WHEEL_LOCK_TIME)) { m_robotContainer.setMotorBrake(false); disabledTimer.stop(); disabledTimer.reset(); @@ -62,7 +61,6 @@ public void disabledPeriodic() { @Override public void autonomousInit() { - RobotState.alliance = DriverStation.getAlliance(); m_robotContainer.setMotorBrake(true); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); @@ -79,7 +77,6 @@ public void autonomousPeriodic() { public void teleopInit() { m_robotContainer.resetPuncherSubsystemEncoders(); m_robotContainer.zeroGyro(); - RobotState.alliance = DriverStation.getAlliance(); if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } else { diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java deleted file mode 100644 index d8c2f29..0000000 --- a/src/main/java/frc/robot/RobotState.java +++ /dev/null @@ -1,9 +0,0 @@ -package frc.robot; - -import java.util.Optional; -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -public class RobotState { - public static Optional alliance = Optional.empty(); - public static boolean debugging = true; -} diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java index bbc77cb..e757624 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants; import frc.robot.Constants.HIDConstants; import frc.robot.Robot; @@ -122,14 +123,14 @@ public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem sw swerveAddFakeVisionReadingCommand = Commands.runOnce(this.swerveSubsystem::addFakeVisionReading); swerveCenterModulesCommand = this.swerveSubsystem.centerModulesCommand(); - intakeCommand = this.intakeSubsystem.percentOutCommand(() -> -0.3); - outtakeCommand = this.intakeSubsystem.percentOutCommand(() -> 0.6); - jointAscendCommand = this.jointSubsystem.percentOutCommand(() -> -0.85); - jointDescendCommand = this.jointSubsystem.percentOutCommand(() -> 0.7); - puncherBuildUpCommand = this.puncherSubsystem.buildUpCommand(() -> 45); - puncherReleaseCommand = this.puncherSubsystem.releaseCommand(() -> 0.3, () -> 0.31, () -> 1.1); - elevatorAscendCommand = this.elevatorSubsystem.percentOutCommand(() -> 0.5); - elevatorDescendCommand = this.elevatorSubsystem.percentOutCommand(() -> -0.5); + intakeCommand = this.intakeSubsystem.percentOutCommand(() -> Constants.IntakeConstants.INTAKE); + outtakeCommand = this.intakeSubsystem.percentOutCommand(() -> Constants.IntakeConstants.OUTTAKE); + jointAscendCommand = this.jointSubsystem.percentOutCommand(() -> Constants.JointConstants.ASCEND); + jointDescendCommand = this.jointSubsystem.percentOutCommand(() -> Constants.JointConstants.DESCEND); + puncherBuildUpCommand = this.puncherSubsystem.buildUpCommand(() -> Constants.PuncherConstants.POSITION); + puncherReleaseCommand = this.puncherSubsystem.releaseCommand(() -> Constants.PuncherConstants.RELEASE, () -> Constants.PuncherConstants.TIGHTEN, () -> Constants.PuncherConstants.TIMER); + elevatorAscendCommand = this.elevatorSubsystem.percentOutCommand(() -> Constants.ElevatorConstants.ASCEND); + elevatorDescendCommand = this.elevatorSubsystem.percentOutCommand(() -> Constants.ElevatorConstants.DESCEND); testPuncherBuildUpCommand = this.puncherSubsystem.testBuildUpCommand(); testPuncherReleaseCommand = this.puncherSubsystem.testReleaseCommand(); @@ -142,11 +143,11 @@ public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem sw } public void configureBindings() { - //drivebaseDefaultButtonBindings(); - //drivebaseSimulationButtonBindings(); - //drivebaseTestButtonBindings(); + drivebaseDefaultButtonBindings(); + drivebaseSimulationButtonBindings(); + drivebaseTestButtonBindings(); - if(frc.robot.RobotState.debugging) { + if(Constants.DEV_MODE) { intakeTestButtonBindings(); jointTestButtonBindings(); puncherTestButtonBindings(); @@ -161,7 +162,7 @@ public void configureBindings() { public void drivebaseDefaultButtonBindings() { if (!RobotBase.isSimulation()) { - swerveSubsystem.setDefaultCommand(driveFieldOrientedDirectAngleCommand); + swerveSubsystem.setDefaultCommand(driveRobotOrientedAngularVelocityCommand); driverXbox.leftBumper().whileTrue(driveRobotOrientedAngularVelocityCommand); } @@ -202,9 +203,8 @@ public void jointTestButtonBindings() { } public void puncherTestButtonBindings() { - driverXbox.rightTrigger().whileTrue(testPuncherBuildUpCommand); + driverXbox.leftBumper().whileTrue(testPuncherBuildUpCommand); driverXbox.rightBumper().onTrue(testPuncherReleaseCommand); - driverXbox.leftBumper().onTrue(testPuncherPrintCommand); } public void elevatorTestButtonBindings() { diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index c84b49d..78bee43 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotState; +import frc.robot.Constants; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import java.util.function.DoubleSupplier; @@ -25,7 +25,7 @@ public ElevatorSubsystem() { motor = new TalonSRX(15); minLimitSwitch = new DigitalInput(0); - if(RobotState.debugging) { + if(Constants.DEV_MODE) { ascendDutyCycleOutValue = Shuffleboard.getTab("Elevator Subsystem").add("Ascend: DutyCycleOut", 0) .withWidget(BuiltInWidgets.kTextView).getEntry(); @@ -68,7 +68,7 @@ public void debug() { @Override public void periodic() { - if(RobotState.debugging) { + if(Constants.DEV_MODE) { this.debug(); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 07bd2a0..d8de1bb 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotState; +import frc.robot.Constants; public class IntakeSubsystem extends SubsystemBase { private final TalonSRX intakeMotor; @@ -30,7 +30,7 @@ public IntakeSubsystem() { intakeMotor.setNeutralMode(NeutralMode.Coast); intakeMotor.setInverted(false); - if(RobotState.debugging) { + if(Constants.DEV_MODE) { intakeDutyCycleValue = Shuffleboard.getTab("Intake Subsystem").add("Intake: DutyCycleOut", 0.3) .withWidget(BuiltInWidgets.kTextView).getEntry(); @@ -65,7 +65,7 @@ public void debug() { @Override public void periodic() { - if(RobotState.debugging) { + if(Constants.DEV_MODE) { this.debug(); } } diff --git a/src/main/java/frc/robot/subsystems/JointSubsystem.java b/src/main/java/frc/robot/subsystems/JointSubsystem.java index 0a72901..64d45c7 100644 --- a/src/main/java/frc/robot/subsystems/JointSubsystem.java +++ b/src/main/java/frc/robot/subsystems/JointSubsystem.java @@ -21,7 +21,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotState; +import frc.robot.Constants; public class JointSubsystem extends SubsystemBase { private final WPI_TalonSRX jointLeftMotor; @@ -89,7 +89,7 @@ public JointSubsystem() { //jointRightMotor.follow(jointLeftMotor); - if(RobotState.debugging) { + if(Constants.DEV_MODE) { absoluteEncoderMultiplierValue = Shuffleboard.getTab("Joint Subsystem").add("Absolute Encoder: Multipler", 1) .withWidget(BuiltInWidgets.kTextView).getEntry(); @@ -198,7 +198,7 @@ public void debug() { @Override public void periodic() { - if(RobotState.debugging) { + if(Constants.DEV_MODE) { this.debug(); } } diff --git a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java index 2193765..6f4fc4e 100644 --- a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java @@ -23,7 +23,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotState; +import frc.robot.Constants; public class PuncherSubsystem extends SubsystemBase { private final TalonFX puncherLeftMotor; @@ -36,7 +36,8 @@ public class PuncherSubsystem extends SubsystemBase { private final PositionDutyCycle positionCycle = new PositionDutyCycle(0); private GenericEntry setpointValue; - private GenericEntry releaseTimeValue; + private GenericEntry releaseTimeInValue; + private GenericEntry releaseTimeOutValue; private GenericEntry releaseDutyCycleValue; private GenericEntry tightenDutyCycleValue; @@ -88,11 +89,14 @@ public PuncherSubsystem() { masterConfig.apply(currentLimitsConfigs); slaveConfig.apply(currentLimitsConfigs); - if(RobotState.debugging) { + if(Constants.DEV_MODE) { setpointValue = Shuffleboard.getTab("Puncher Subsystem").add("Setpoint", 45) .withWidget(BuiltInWidgets.kTextView).getEntry(); - releaseTimeValue = Shuffleboard.getTab("Puncher Subsystem").add("Release and Tighten: Timer", 1.1) + releaseTimeInValue = Shuffleboard.getTab("Puncher Subsystem").add("Release: Timer", 0.3) + .withWidget(BuiltInWidgets.kTextView).getEntry(); + + releaseTimeOutValue = Shuffleboard.getTab("Puncher Subsystem").add("Tighten: Timer", 0.3) .withWidget(BuiltInWidgets.kTextView).getEntry(); releaseDutyCycleValue = Shuffleboard.getTab("Puncher Subsystem").add("Release: DutyCycleOut", 0.3) @@ -160,9 +164,9 @@ public Command testBuildUpCommand() { public Command testReleaseCommand() { return Commands.startEnd(() -> this.setRelease(-releaseDutyCycleValue.getDouble(0.3)), this::stopRelease, this) - .withTimeout(releaseTimeValue.getDouble(1.1)).andThen + .withTimeout(releaseTimeInValue.getDouble(0.3)).andThen (Commands.startEnd(() -> this.setRelease(tightenDutyCycleValue.getDouble(0.31)), this::stopRelease, this) - .withTimeout(releaseTimeValue.getDouble(1.1))); + .withTimeout(releaseTimeOutValue.getDouble(0.3))); } public void debug() { @@ -171,7 +175,7 @@ public void debug() { @Override public void periodic() { - if(RobotState.debugging) { + if(Constants.DEV_MODE) { this.debug(); } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index a881239..02148dc 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -102,7 +102,7 @@ public SwerveSubsystem(File directory) 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1. swerveDrive.setModuleEncoderAutoSynchronize(false, 1); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. - swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible + //swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible if (visionDriveTest) { setupPhotonVision(); From b002e8debc33275698251bb8afd213227bef83af Mon Sep 17 00:00:00 2001 From: Pedro Henrique Date: Sun, 9 Mar 2025 18:44:07 -0300 Subject: [PATCH 15/21] commit --- src/main/deploy/swerve/modules/backleft.json | 2 +- src/main/deploy/swerve/modules/backright.json | 2 +- src/main/deploy/swerve/modules/frontleft.json | 2 +- src/main/deploy/swerve/modules/frontright.json | 2 +- src/main/java/frc/robot/subsystems/PuncherSubsystem.java | 4 ++-- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index bb163ab..3609880 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -3,7 +3,7 @@ "front": -10.531, "left": 10.531 }, - "absoluteEncoderOffset": 269, + "absoluteEncoderOffset": 269.4, "drive": { "type": "sparkmax_neo", "id": 7, diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index cc2d8d3..b2f6410 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -3,7 +3,7 @@ "front": -10.531, "left": -10.531 }, - "absoluteEncoderOffset": 274, + "absoluteEncoderOffset": 272.8, "drive": { "type": "sparkmax_neo", "id": 5, diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index 4f98b16..f9934f7 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -3,7 +3,7 @@ "front": 10.531, "left": 10.531 }, - "absoluteEncoderOffset": 255.1, + "absoluteEncoderOffset": 252.9, "drive": { "type": "sparkmax_neo", "id": 1, diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index 5e5ade0..ae3d6f0 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -3,7 +3,7 @@ "front": 10.531, "left": -10.531 }, - "absoluteEncoderOffset": 320.1, + "absoluteEncoderOffset": 327.5, "drive": { "type": "sparkmax_neo", "id": 3, diff --git a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java index 6f4fc4e..161c2d2 100644 --- a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java @@ -164,9 +164,9 @@ public Command testBuildUpCommand() { public Command testReleaseCommand() { return Commands.startEnd(() -> this.setRelease(-releaseDutyCycleValue.getDouble(0.3)), this::stopRelease, this) - .withTimeout(releaseTimeInValue.getDouble(0.3)).andThen + .withTimeout(0.2).andThen (Commands.startEnd(() -> this.setRelease(tightenDutyCycleValue.getDouble(0.31)), this::stopRelease, this) - .withTimeout(releaseTimeOutValue.getDouble(0.3))); + .withTimeout(0.2)); } public void debug() { From 108bbf99c99210fcdf1b641af098743206b1859b Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Mon, 10 Mar 2025 02:20:48 -0300 Subject: [PATCH 16/21] commit --- .../java/frc/robot/buttonBindings/DriverButtonBindings.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java index e757624..e948f2e 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -208,8 +208,8 @@ public void puncherTestButtonBindings() { } public void elevatorTestButtonBindings() { - driverXbox.x().whileTrue(testElevatorAscendCommand); - driverXbox.b().whileTrue(testElevatorDescendCommand); + driverXbox.povUp().whileTrue(testElevatorAscendCommand); + driverXbox.povDown().whileTrue(testElevatorDescendCommand); } public void intakeButtonBindings() { From 5d763fecefe3269f2111d8095e445534f09025d1 Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Thu, 13 Mar 2025 02:39:38 -0300 Subject: [PATCH 17/21] commit --- src/main/java/frc/robot/Constants.java | 11 ++++---- .../buttonBindings/DriverButtonBindings.java | 2 +- .../robot/subsystems/ElevatorSubsystem.java | 7 ++--- .../frc/robot/subsystems/IntakeSubsystem.java | 2 +- .../frc/robot/subsystems/JointSubsystem.java | 6 ++--- .../robot/subsystems/PuncherSubsystem.java | 26 ++++++++++++------- 6 files changed, 31 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 27789c6..bbd8900 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,7 @@ public static final class SwerveConstants { } public static class IntakeConstants { - public static final int MOTOR_CAN_ID = 10; + public static final int MOTOR_CAN_ID = 12; public static final double INTAKE = 0.3; public static final double OUTTAKE = -0.6; } @@ -41,16 +41,17 @@ public static class JointConstants { } public static class PuncherConstants { - public static final int LEFT_MOTOR_CAN_ID = 13; - public static final int RIGHT_MOTOR_CAN_ID = 14; + public static final int LEFT_MOTOR_CAN_ID = 10; + public static final int RIGHT_MOTOR_CAN_ID = 11; + public static final int RELEASE_MOTOR_CAN_ID = 16; + public static final int SENSOR_DIO_PORT = 4; public static final double POSITION = 45; public static final double RELEASE = 0.3; public static final double TIGHTEN = 0.31; - public static final double TIMER = 1.1; } public static class ElevatorConstants { - public static final int MOTOR_CAN_ID = 16; + public static final int MOTOR_CAN_ID = 15; public static final int LIMIT_SWITCH_DIO_PORT = 0; public static final double ASCEND = -0.3; public static final double DESCEND = 0.3; diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java index e948f2e..d34afa2 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -128,7 +128,7 @@ public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem sw jointAscendCommand = this.jointSubsystem.percentOutCommand(() -> Constants.JointConstants.ASCEND); jointDescendCommand = this.jointSubsystem.percentOutCommand(() -> Constants.JointConstants.DESCEND); puncherBuildUpCommand = this.puncherSubsystem.buildUpCommand(() -> Constants.PuncherConstants.POSITION); - puncherReleaseCommand = this.puncherSubsystem.releaseCommand(() -> Constants.PuncherConstants.RELEASE, () -> Constants.PuncherConstants.TIGHTEN, () -> Constants.PuncherConstants.TIMER); + puncherReleaseCommand = this.puncherSubsystem.releaseCommand(() -> Constants.PuncherConstants.RELEASE, () -> Constants.PuncherConstants.TIGHTEN); elevatorAscendCommand = this.elevatorSubsystem.percentOutCommand(() -> Constants.ElevatorConstants.ASCEND); elevatorDescendCommand = this.elevatorSubsystem.percentOutCommand(() -> Constants.ElevatorConstants.DESCEND); diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 78bee43..2d42f53 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -22,8 +23,8 @@ public class ElevatorSubsystem extends SubsystemBase { private GenericEntry descendDutyCycleOutValue; public ElevatorSubsystem() { - motor = new TalonSRX(15); - minLimitSwitch = new DigitalInput(0); + motor = new TalonSRX(Constants.ElevatorConstants.MOTOR_CAN_ID); + minLimitSwitch = new DigitalInput(Constants.ElevatorConstants.LIMIT_SWITCH_DIO_PORT); if(Constants.DEV_MODE) { ascendDutyCycleOutValue = Shuffleboard.getTab("Elevator Subsystem").add("Ascend: DutyCycleOut", 0) @@ -63,7 +64,7 @@ public Command testAscendCommand() { } public void debug() { - + SmartDashboard.putBoolean("Elevator Subsystem/Limit Switch", minLimitSwitch.get()); } @Override diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index d8de1bb..60d5a40 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -22,7 +22,7 @@ public class IntakeSubsystem extends SubsystemBase { private GenericEntry outtakeDutyCycleValue; public IntakeSubsystem() { - intakeMotor = new TalonSRX(12); + intakeMotor = new TalonSRX(Constants.IntakeConstants.MOTOR_CAN_ID); TalonSRXConfiguration configs = new TalonSRXConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/JointSubsystem.java b/src/main/java/frc/robot/subsystems/JointSubsystem.java index 64d45c7..87d25c0 100644 --- a/src/main/java/frc/robot/subsystems/JointSubsystem.java +++ b/src/main/java/frc/robot/subsystems/JointSubsystem.java @@ -58,10 +58,10 @@ public class JointSubsystem extends SubsystemBase { private GenericEntry dutyCycleOutValue; public JointSubsystem() { - jointLeftMotor = new WPI_TalonSRX(13); - jointRightMotor = new WPI_VictorSPX(14); + jointLeftMotor = new WPI_TalonSRX(Constants.JointConstants.LEFT_MOTOR_CAN_ID); + jointRightMotor = new WPI_VictorSPX(Constants.JointConstants.RIGHT_MOTOR_CAN_ID); - absoluteEncoder = new DutyCycleEncoder(3, 100, 0); + absoluteEncoder = new DutyCycleEncoder(Constants.JointConstants.RIGHT_ENCODER_DIO_PORT, 100, 0); absoluteEncoder.setAssumedFrequency(975.6); //jointLeftMotor.getSensorCollection().setQuadraturePosition((int) Math.round(absoluteEncoder.get()), 0); diff --git a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java index 161c2d2..36c8f66 100644 --- a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java @@ -17,8 +17,10 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -31,6 +33,7 @@ public class PuncherSubsystem extends SubsystemBase { private final Follower follower; private final TalonSRX puncherReleaseMotor; + private final DigitalInput puncherReleaseFeedback; private final DutyCycleOut percentOutCycle = new DutyCycleOut(0); private final PositionDutyCycle positionCycle = new PositionDutyCycle(0); @@ -42,10 +45,11 @@ public class PuncherSubsystem extends SubsystemBase { private GenericEntry tightenDutyCycleValue; public PuncherSubsystem() { - puncherLeftMotor = new TalonFX(10); - puncherRightMotor = new TalonFX(11); - follower = new Follower(10, false); - puncherReleaseMotor = new TalonSRX(16); + puncherLeftMotor = new TalonFX(Constants.PuncherConstants.LEFT_MOTOR_CAN_ID); + puncherRightMotor = new TalonFX(Constants.PuncherConstants.RIGHT_MOTOR_CAN_ID); + follower = new Follower(Constants.PuncherConstants.LEFT_MOTOR_CAN_ID, false); + puncherReleaseMotor = new TalonSRX(Constants.PuncherConstants.RELEASE_MOTOR_CAN_ID); + puncherReleaseFeedback = new DigitalInput(Constants.PuncherConstants.SENSOR_DIO_PORT); puncherLeftMotor.setPosition(0); puncherRightMotor.setPosition(0); @@ -147,11 +151,11 @@ public Command buildUpCommand(DoubleSupplier position) { return Commands.startEnd(() -> this.goToPosition(position.getAsDouble()), () -> this.goToPosition(0), this); } - public Command releaseCommand(DoubleSupplier speed1, DoubleSupplier speed2, DoubleSupplier time) { + public Command releaseCommand(DoubleSupplier speed1, DoubleSupplier speed2) { return Commands.startEnd(() -> this.setRelease(-speed1.getAsDouble()), this::stopRelease, this) - .withTimeout(time.getAsDouble()).andThen + .until(puncherReleaseFeedback::get).andThen (Commands.startEnd(() -> this.setRelease(speed2.getAsDouble()), this::stopRelease, this) - .withTimeout(time.getAsDouble())); + .until(puncherReleaseFeedback::get)); } public Command testPrintCommand() { @@ -164,13 +168,15 @@ public Command testBuildUpCommand() { public Command testReleaseCommand() { return Commands.startEnd(() -> this.setRelease(-releaseDutyCycleValue.getDouble(0.3)), this::stopRelease, this) - .withTimeout(0.2).andThen + .until(puncherReleaseFeedback::get).andThen (Commands.startEnd(() -> this.setRelease(tightenDutyCycleValue.getDouble(0.31)), this::stopRelease, this) - .withTimeout(0.2)); + .until(() -> !puncherReleaseFeedback.get())).andThen + (Commands.startEnd(() -> this.setRelease(tightenDutyCycleValue.getDouble(0.31)), this::stopRelease, this)) + .until(puncherReleaseFeedback::get); } public void debug() { - + SmartDashboard.putBoolean("Puncher Subsystem/Release/Feedback", puncherReleaseFeedback.get()); } @Override From 5dc3988a8e811215375648f141ca51f854496950 Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Thu, 13 Mar 2025 14:18:11 -0300 Subject: [PATCH 18/21] commit --- simgui-ds.json | 5 -- simgui.json | 42 +++++++++++------ src/main/java/frc/robot/Constants.java | 3 +- src/main/java/frc/robot/RobotContainer.java | 1 + .../buttonBindings/DriverButtonBindings.java | 16 +++++-- .../frc/robot/subsystems/IntakeSubsystem.java | 2 +- .../frc/robot/subsystems/JointSubsystem.java | 12 +++++ .../robot/subsystems/PuncherSubsystem.java | 46 +++++++++++++------ 8 files changed, 90 insertions(+), 37 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 8f80438..e695a15 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -4,11 +4,6 @@ "visible": false } }, - "System Joysticks": { - "window": { - "enabled": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/simgui.json b/simgui.json index ff49fb0..2257382 100644 --- a/simgui.json +++ b/simgui.json @@ -1,5 +1,10 @@ { "HALProvider": { + "DIO": { + "window": { + "visible": true + } + }, "Other Devices": { "SPARK MAX [1] RELATIVE ENCODER": { "header": { @@ -80,10 +85,28 @@ "types": { "/FMSInfo": "FMSInfo", "/LiveWindow/SwerveSubsystem": "Subsystem", + "/LiveWindow/Ungrouped/DigitalInput[0]": "Digital Input", + "/LiveWindow/Ungrouped/DigitalInput[3]": "Digital Input", + "/LiveWindow/Ungrouped/PIDController[12]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[13]": "PIDController", "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[4]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[5]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[6]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[7]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[8]": "PIDController", "/LiveWindow/Ungrouped/Pigeon 2 [13]": "Gyro", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", + "/LiveWindow/Ungrouped/Talon FX (v6) [10]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX (v6) [11]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon SRX [13]": "Motor Controller", + "/LiveWindow/Ungrouped/Victor SPX [14]": "Motor Controller", + "/Shuffleboard/Autonomous/Choose Autonomous Routine": "String Chooser", "/Shuffleboard/Joint Subsystem/Closed Loop Command": "Command", + "/Shuffleboard/Joint Subsystem/Update Controllers Parameters Command": "Command", + "/SmartDashboard/ADXRS450_Gyro[0]": "Gyro", "/SmartDashboard/Alerts": "Alerts", "/SmartDashboard/Encoders": "Alerts", "/SmartDashboard/Field": "Field2d", @@ -143,15 +166,6 @@ } }, "NetworkTables": { - "Persistent Values": { - "open": false - }, - "Retained Values": { - "open": false - }, - "Transitory Values": { - "open": false - }, "retained": { "Shuffleboard": { "Main": { @@ -165,13 +179,13 @@ "open": true }, "SmartDashboard": { - "Field": { + "Joint Subsystem": { + "Absolute Encoder": { + "open": true + }, "open": true }, - "open": true, - "swerve": { - "open": true - } + "open": true } } }, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bbd8900..76eca5d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -44,7 +44,8 @@ public static class PuncherConstants { public static final int LEFT_MOTOR_CAN_ID = 10; public static final int RIGHT_MOTOR_CAN_ID = 11; public static final int RELEASE_MOTOR_CAN_ID = 16; - public static final int SENSOR_DIO_PORT = 4; + public static final int ENCODER_DIO_PORT_A = 4; + public static final int ENCODER_DIO_PORT_B = 5; public static final double POSITION = 45; public static final double RELEASE = 0.3; public static final double TIGHTEN = 0.31; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b4da29b..655078c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -84,6 +84,7 @@ public void setMotorBrake(boolean brake) { public void resetPuncherSubsystemEncoders() { puncherSubsystem.resetEncoders(); + puncherSubsystem.resetState(); } public void zeroGyro() { diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java index d34afa2..cfd98d6 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -5,8 +5,10 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; import frc.robot.Constants.HIDConstants; @@ -49,12 +51,16 @@ public class DriverButtonBindings { private final Command intakeCommand; private final Command outtakeCommand; + private final Command intakeStopCommand; private final Command jointAscendCommand; private final Command jointDescendCommand; private final Command puncherBuildUpCommand; private final Command puncherReleaseCommand; private final Command elevatorAscendCommand; private final Command elevatorDescendCommand; + private final Command driverFeedbackCommand; + + private final ConditionalCommand puncherConditionalCommand; private final Command testPuncherBuildUpCommand; private final Command testPuncherReleaseCommand; @@ -125,6 +131,7 @@ public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem sw intakeCommand = this.intakeSubsystem.percentOutCommand(() -> Constants.IntakeConstants.INTAKE); outtakeCommand = this.intakeSubsystem.percentOutCommand(() -> Constants.IntakeConstants.OUTTAKE); + intakeStopCommand = this.intakeSubsystem.percentOutCommand(() -> 0); jointAscendCommand = this.jointSubsystem.percentOutCommand(() -> Constants.JointConstants.ASCEND); jointDescendCommand = this.jointSubsystem.percentOutCommand(() -> Constants.JointConstants.DESCEND); puncherBuildUpCommand = this.puncherSubsystem.buildUpCommand(() -> Constants.PuncherConstants.POSITION); @@ -132,6 +139,9 @@ public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem sw elevatorAscendCommand = this.elevatorSubsystem.percentOutCommand(() -> Constants.ElevatorConstants.ASCEND); elevatorDescendCommand = this.elevatorSubsystem.percentOutCommand(() -> Constants.ElevatorConstants.DESCEND); + driverFeedbackCommand = Commands.startEnd(() -> this.driverXbox.setRumble(RumbleType.kBothRumble, 1), () -> this.driverXbox.setRumble(RumbleType.kBothRumble, 0), puncherSubsystem).withTimeout(2); + puncherConditionalCommand = new ConditionalCommand(puncherBuildUpCommand, driverFeedbackCommand, () -> jointSubsystem.isSafeBuildUpAngle() && !puncherSubsystem.isPuncherReady()); + testPuncherBuildUpCommand = this.puncherSubsystem.testBuildUpCommand(); testPuncherReleaseCommand = this.puncherSubsystem.testReleaseCommand(); testPuncherPrintCommand = this.puncherSubsystem.testPrintCommand(); @@ -163,7 +173,6 @@ public void configureBindings() { public void drivebaseDefaultButtonBindings() { if (!RobotBase.isSimulation()) { swerveSubsystem.setDefaultCommand(driveRobotOrientedAngularVelocityCommand); - driverXbox.leftBumper().whileTrue(driveRobotOrientedAngularVelocityCommand); } if (!DriverStation.isTest()) { @@ -203,7 +212,7 @@ public void jointTestButtonBindings() { } public void puncherTestButtonBindings() { - driverXbox.leftBumper().whileTrue(testPuncherBuildUpCommand); + driverXbox.leftBumper().onTrue(puncherConditionalCommand); driverXbox.rightBumper().onTrue(testPuncherReleaseCommand); } @@ -214,7 +223,8 @@ public void elevatorTestButtonBindings() { public void intakeButtonBindings() { driverXbox.y().whileTrue(intakeCommand); - driverXbox.x().whileTrue(outtakeCommand); + driverXbox.x().whileTrue(outtakeCommand); + driverXbox.b().whileTrue(intakeStopCommand); } public void jointButtonBindings() { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 60d5a40..8d099ae 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -48,7 +48,7 @@ public void stop() { } public Command percentOutCommand(DoubleSupplier speed) { - return Commands.startEnd(() -> this.percentOut(speed.getAsDouble()), this::stop, this); + return Commands.startEnd(this::stop, () -> this.percentOut(speed.getAsDouble()), this); } public Command testIntakeCommand() { diff --git a/src/main/java/frc/robot/subsystems/JointSubsystem.java b/src/main/java/frc/robot/subsystems/JointSubsystem.java index 87d25c0..f2ba3f7 100644 --- a/src/main/java/frc/robot/subsystems/JointSubsystem.java +++ b/src/main/java/frc/robot/subsystems/JointSubsystem.java @@ -170,6 +170,18 @@ public void updateClosedLoopControllersParameters() { slaveController.setPID(slaveControllerProportionalValue.getDouble(0), slaveControllerIntegralValue.getDouble(0), slaveControllerDerivativeValue.getDouble(0)); } + public boolean isSafeBuildUpAngle() { + return absoluteEncoderAdjusted < 60; + } + + public boolean isAngleLower() { + return absoluteEncoderAdjusted < 70; + } + + public boolean isAngleHigher() { + return absoluteEncoderAdjusted > 60; + } + public Command testOpenLoopCommand() { return this.runEnd(() -> this.percentOut(dutyCycleOutValue.getDouble(0)), this::stop); } diff --git a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java index 36c8f66..0e25ea5 100644 --- a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java @@ -18,6 +18,7 @@ import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -33,10 +34,11 @@ public class PuncherSubsystem extends SubsystemBase { private final Follower follower; private final TalonSRX puncherReleaseMotor; - private final DigitalInput puncherReleaseFeedback; + private final Encoder puncherReleaseFeedback; private final DutyCycleOut percentOutCycle = new DutyCycleOut(0); private final PositionDutyCycle positionCycle = new PositionDutyCycle(0); + private boolean isPuncherReady = false; private GenericEntry setpointValue; private GenericEntry releaseTimeInValue; @@ -49,7 +51,7 @@ public PuncherSubsystem() { puncherRightMotor = new TalonFX(Constants.PuncherConstants.RIGHT_MOTOR_CAN_ID); follower = new Follower(Constants.PuncherConstants.LEFT_MOTOR_CAN_ID, false); puncherReleaseMotor = new TalonSRX(Constants.PuncherConstants.RELEASE_MOTOR_CAN_ID); - puncherReleaseFeedback = new DigitalInput(Constants.PuncherConstants.SENSOR_DIO_PORT); + puncherReleaseFeedback = new Encoder(Constants.PuncherConstants.ENCODER_DIO_PORT_A, Constants.PuncherConstants.ENCODER_DIO_PORT_B); puncherLeftMotor.setPosition(0); puncherRightMotor.setPosition(0); @@ -93,6 +95,8 @@ public PuncherSubsystem() { masterConfig.apply(currentLimitsConfigs); slaveConfig.apply(currentLimitsConfigs); + puncherReleaseFeedback.reset(); + if(Constants.DEV_MODE) { setpointValue = Shuffleboard.getTab("Puncher Subsystem").add("Setpoint", 45) .withWidget(BuiltInWidgets.kTextView).getEntry(); @@ -111,6 +115,14 @@ public PuncherSubsystem() { } } + public boolean isPuncherReady() { + return isPuncherReady; + } + + public void setIsPuncherReady(boolean isPuncherReady) { + this.isPuncherReady = isPuncherReady; + } + public double getPosition() { return puncherLeftMotor.getPosition().getValueAsDouble(); } @@ -148,14 +160,16 @@ public void resetEncoders() { } public Command buildUpCommand(DoubleSupplier position) { - return Commands.startEnd(() -> this.goToPosition(position.getAsDouble()), () -> this.goToPosition(0), this); + return Commands.startEnd(() -> this.goToPosition(position.getAsDouble()), () -> {this.goToPosition(0); + this.setIsPuncherReady(true);}, this).until(() -> puncherLeftMotor.getPosition().getValueAsDouble() > 20); } public Command releaseCommand(DoubleSupplier speed1, DoubleSupplier speed2) { return Commands.startEnd(() -> this.setRelease(-speed1.getAsDouble()), this::stopRelease, this) - .until(puncherReleaseFeedback::get).andThen - (Commands.startEnd(() -> this.setRelease(speed2.getAsDouble()), this::stopRelease, this) - .until(puncherReleaseFeedback::get)); + .until(() -> puncherReleaseFeedback.get() == 500).andThen + (Commands.startEnd(() -> this.setRelease(speed2.getAsDouble()), () -> {this.stopRelease(); + this.setIsPuncherReady(false);}, this) + .until(() -> puncherReleaseFeedback.get() == 0)); } public Command testPrintCommand() { @@ -163,20 +177,26 @@ public Command testPrintCommand() { } public Command testBuildUpCommand() { - return Commands.startEnd(() -> this.goToPosition(setpointValue.getDouble(45)), () -> this.goToPosition(0), this); + return Commands.startEnd(() -> this.goToPosition(setpointValue.getDouble(45)), () -> {this.goToPosition(0); + this.setIsPuncherReady(true);}, this).until(() -> puncherLeftMotor.getPosition().getValueAsDouble() > 20); } public Command testReleaseCommand() { return Commands.startEnd(() -> this.setRelease(-releaseDutyCycleValue.getDouble(0.3)), this::stopRelease, this) - .until(puncherReleaseFeedback::get).andThen - (Commands.startEnd(() -> this.setRelease(tightenDutyCycleValue.getDouble(0.31)), this::stopRelease, this) - .until(() -> !puncherReleaseFeedback.get())).andThen - (Commands.startEnd(() -> this.setRelease(tightenDutyCycleValue.getDouble(0.31)), this::stopRelease, this)) - .until(puncherReleaseFeedback::get); + .until(() -> puncherReleaseFeedback.get() == 500).andThen + (Commands.startEnd(() -> this.setRelease(tightenDutyCycleValue.getDouble(0.31)), () -> {this.stopRelease(); + this.setIsPuncherReady(false);}, this) + .until(() -> puncherReleaseFeedback.get() == 0)); + } + + public void resetState() { + this.setIsPuncherReady(false); } public void debug() { - SmartDashboard.putBoolean("Puncher Subsystem/Release/Feedback", puncherReleaseFeedback.get()); + SmartDashboard.putNumber("Puncher Subsystem/Release/Feedback", puncherReleaseFeedback.get()); + SmartDashboard.putBoolean("Puncher Subsystem/State", isPuncherReady()); + SmartDashboard.putNumber("Puncher Subsystem/Motor", puncherLeftMotor.getPosition().getValueAsDouble()); } @Override From fcc7aed857e6712c536f9fa7f929b0de60aa88cf Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Fri, 14 Mar 2025 14:20:56 -0300 Subject: [PATCH 19/21] commit --- src/main/deploy/swerve/modules/backleft.json | 14 +- src/main/deploy/swerve/modules/backright.json | 14 +- src/main/deploy/swerve/modules/frontleft.json | 14 +- .../deploy/swerve/modules/frontright.json | 14 +- src/main/java/frc/robot/Constants.java | 12 +- src/main/java/frc/robot/RobotContainer.java | 15 +- .../buttonBindings/DriverButtonBindings.java | 41 +- .../DriverTankButtonBindings.java | 160 +++++++ .../robot/subsystems/ElevatorSubsystem.java | 6 +- .../frc/robot/subsystems/IntakeSubsystem.java | 4 +- .../frc/robot/subsystems/JointSubsystem.java | 9 - .../robot/subsystems/PuncherSubsystem.java | 20 +- .../subsystems/swervetank/Drivetrain.java | 138 ++++++ vendordeps/PathplannerLib-2025.2.6.json | 38 ++ vendordeps/PathplannerLib.json | 38 -- vendordeps/Phoenix6-25.2.1.json | 419 ---------------- vendordeps/Phoenix6-25.3.1.json | 449 ++++++++++++++++++ vendordeps/REVLib.json | 138 +++--- vendordeps/ReduxLib-2025.0.0.json | 72 --- vendordeps/ReduxLib-2025.0.1.json | 72 +++ vendordeps/ThriftyLib.json | 36 +- vendordeps/maple-sim.json | 4 +- vendordeps/photonlib.json | 138 +++--- vendordeps/yagsl-2025.2.2.json | 64 --- vendordeps/yagsl-2025.7.2.json | 64 +++ 25 files changed, 1169 insertions(+), 824 deletions(-) create mode 100644 src/main/java/frc/robot/buttonBindings/DriverTankButtonBindings.java create mode 100644 src/main/java/frc/robot/subsystems/swervetank/Drivetrain.java create mode 100644 vendordeps/PathplannerLib-2025.2.6.json delete mode 100644 vendordeps/PathplannerLib.json delete mode 100644 vendordeps/Phoenix6-25.2.1.json create mode 100644 vendordeps/Phoenix6-25.3.1.json delete mode 100644 vendordeps/ReduxLib-2025.0.0.json create mode 100644 vendordeps/ReduxLib-2025.0.1.json delete mode 100644 vendordeps/yagsl-2025.2.2.json create mode 100644 vendordeps/yagsl-2025.7.2.json diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index 3609880..f916d49 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -1,26 +1,26 @@ { "location": { - "front": -10.531, - "left": 10.531 + "front": 10.531, + "left": -10.531 }, - "absoluteEncoderOffset": 269.4, + "absoluteEncoderOffset": -214, "drive": { "type": "sparkmax_neo", - "id": 7, + "id": 3, "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": false }, "angle": { "type": "sparkmax_neo", - "id": 9, + "id": 2, "canbus": null }, "encoder": { "type": "ma3", - "id": 3, + "id": 0, "canbus": null }, "absoluteEncoderInverted": true diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index b2f6410..1f296ca 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -1,26 +1,26 @@ { "location": { - "front": -10.531, - "left": -10.531 + "front": 10.531, + "left": 10.531 }, - "absoluteEncoderOffset": 272.8, + "absoluteEncoderOffset": -289, "drive": { "type": "sparkmax_neo", - "id": 5, + "id": 1, "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": false }, "angle": { "type": "sparkmax_neo", - "id": 6, + "id": 4, "canbus": null }, "encoder": { "type": "ma3", - "id": 2, + "id": 1, "canbus": null }, "absoluteEncoderInverted": true diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index f9934f7..2ae33c5 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -1,26 +1,26 @@ { "location": { - "front": 10.531, - "left": 10.531 + "front": -10.531, + "left": -10.531 }, - "absoluteEncoderOffset": 252.9, + "absoluteEncoderOffset": -271, "drive": { "type": "sparkmax_neo", - "id": 1, + "id": 5, "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": false }, "angle": { "type": "sparkmax_neo", - "id": 4, + "id": 6, "canbus": null }, "encoder": { "type": "ma3", - "id": 1, + "id": 2, "canbus": null }, "absoluteEncoderInverted": true diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index ae3d6f0..38aa2f0 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -1,26 +1,26 @@ { "location": { - "front": 10.531, - "left": -10.531 + "front": -10.531, + "left": 10.531 }, - "absoluteEncoderOffset": 327.5, + "absoluteEncoderOffset": -270, "drive": { "type": "sparkmax_neo", - "id": 3, + "id": 7, "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": false }, "angle": { "type": "sparkmax_neo", - "id": 2, + "id": 9, "canbus": null }, "encoder": { "type": "ma3", - "id": 0, + "id": 3, "canbus": null }, "absoluteEncoderInverted": true diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 76eca5d..fe185c4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,7 +13,7 @@ public final class Constants { public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; public static final double MAX_SPEED = Units.feetToMeters(14.5); - public static final boolean DEV_MODE = true; + public static final boolean DEV_MODE = false; public static class HIDConstants { public static final double DEADBAND = 0.1; @@ -28,8 +28,8 @@ public static final class SwerveConstants { public static class IntakeConstants { public static final int MOTOR_CAN_ID = 12; - public static final double INTAKE = 0.3; - public static final double OUTTAKE = -0.6; + public static final double INTAKE = -0.4; + public static final double OUTTAKE = 0.7; } public static class JointConstants { @@ -46,7 +46,7 @@ public static class PuncherConstants { public static final int RELEASE_MOTOR_CAN_ID = 16; public static final int ENCODER_DIO_PORT_A = 4; public static final int ENCODER_DIO_PORT_B = 5; - public static final double POSITION = 45; + public static final double POSITION = 100; public static final double RELEASE = 0.3; public static final double TIGHTEN = 0.31; } @@ -54,7 +54,7 @@ public static class PuncherConstants { public static class ElevatorConstants { public static final int MOTOR_CAN_ID = 15; public static final int LIMIT_SWITCH_DIO_PORT = 0; - public static final double ASCEND = -0.3; - public static final double DESCEND = 0.3; + public static final double ASCEND = 1; + public static final double DESCEND = -1; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 655078c..9449ff7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,14 +13,17 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.buttonBindings.DriverButtonBindings; +import frc.robot.buttonBindings.DriverTankButtonBindings; import frc.robot.buttonBindings.OperatorButtonBindings; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.JointSubsystem; import frc.robot.subsystems.PuncherSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervetank.Drivetrain; import java.io.File; @@ -29,12 +32,16 @@ public class RobotContainer { private final CommandXboxController operatorXbox; private final SwerveSubsystem swerveSubsystem; + //private final Drivetrain swerveSubsystem; + private final IntakeSubsystem intakeSubsystem; private final PuncherSubsystem puncherSubsystem; private final JointSubsystem jointSubsystem; private final ElevatorSubsystem elevatorSubsystem; private final DriverButtonBindings driverButtonBindings; + //private final DriverTankButtonBindings driverButtonBindings; + private final OperatorButtonBindings operatorButtonBindings; SendableChooser autonomousChooser; @@ -43,14 +50,15 @@ public RobotContainer() { operatorXbox = new CommandXboxController(1); swerveSubsystem = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),"swerve")); + //swerveSubsystem = new Drivetrain(); intakeSubsystem = new IntakeSubsystem(); puncherSubsystem = new PuncherSubsystem(); jointSubsystem = new JointSubsystem(); elevatorSubsystem = new ElevatorSubsystem(); + driverButtonBindings = new DriverButtonBindings(driverXbox, swerveSubsystem, intakeSubsystem, jointSubsystem, puncherSubsystem, elevatorSubsystem); + //driverButtonBindings = new DriverTankButtonBindings(driverXbox, swerveSubsystem, intakeSubsystem, jointSubsystem, puncherSubsystem, elevatorSubsystem); - driverButtonBindings = new DriverButtonBindings(driverXbox, swerveSubsystem, intakeSubsystem, - jointSubsystem, puncherSubsystem, elevatorSubsystem); operatorButtonBindings = new OperatorButtonBindings(operatorXbox); configureBindings(); @@ -66,7 +74,8 @@ private void configureBindings() { } public Command getAutonomousCommand() { - return autonomousChooser.getSelected(); + //return swerveSubsystem.autonomousCommand(); + return null; //autonomousChooser.getSelected(); } public void autonomousChooserSetup() { diff --git a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java index cfd98d6..6c43e1c 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverButtonBindings.java @@ -60,7 +60,7 @@ public class DriverButtonBindings { private final Command elevatorDescendCommand; private final Command driverFeedbackCommand; - private final ConditionalCommand puncherConditionalCommand; + //private final ConditionalCommand puncherConditionalCommand; private final Command testPuncherBuildUpCommand; private final Command testPuncherReleaseCommand; @@ -83,8 +83,8 @@ public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem sw this.elevatorSubsystem = elevatorSubsystem; driveAngularVelocitySwerveInputStream = SwerveInputStream.of(this.swerveSubsystem.getSwerveDrive(), - () -> this.driverXbox.getLeftY() * -1, - () -> this.driverXbox.getLeftX() * -1) + () -> this.driverXbox.getLeftY() * 1, + () -> this.driverXbox.getLeftX() * 1) .withControllerRotationAxis(() -> { double leftTrigger = this.driverXbox.getLeftTriggerAxis(); double rightTrigger = this.driverXbox.getRightTriggerAxis(); @@ -137,10 +137,10 @@ public DriverButtonBindings(CommandXboxController driverXbox, SwerveSubsystem sw puncherBuildUpCommand = this.puncherSubsystem.buildUpCommand(() -> Constants.PuncherConstants.POSITION); puncherReleaseCommand = this.puncherSubsystem.releaseCommand(() -> Constants.PuncherConstants.RELEASE, () -> Constants.PuncherConstants.TIGHTEN); elevatorAscendCommand = this.elevatorSubsystem.percentOutCommand(() -> Constants.ElevatorConstants.ASCEND); - elevatorDescendCommand = this.elevatorSubsystem.percentOutCommand(() -> Constants.ElevatorConstants.DESCEND); + elevatorDescendCommand = this.elevatorSubsystem.descendCommand(); driverFeedbackCommand = Commands.startEnd(() -> this.driverXbox.setRumble(RumbleType.kBothRumble, 1), () -> this.driverXbox.setRumble(RumbleType.kBothRumble, 0), puncherSubsystem).withTimeout(2); - puncherConditionalCommand = new ConditionalCommand(puncherBuildUpCommand, driverFeedbackCommand, () -> jointSubsystem.isSafeBuildUpAngle() && !puncherSubsystem.isPuncherReady()); + //puncherConditionalCommand = new ConditionalCommand(puncherBuildUpCommand, driverFeedbackCommand, () -> jointSubsystem.isSafeBuildUpAngle() && !puncherSubsystem.isPuncherReady()); testPuncherBuildUpCommand = this.puncherSubsystem.testBuildUpCommand(); testPuncherReleaseCommand = this.puncherSubsystem.testReleaseCommand(); @@ -203,28 +203,33 @@ public void drivebaseTestButtonBindings() { } public void intakeTestButtonBindings() { - driverXbox.y().whileTrue(testIntakeCommand); - driverXbox.x().whileTrue(testOuttakeCommand); + driverXbox.y().whileTrue(testIntakeCommand); //TESTAR + driverXbox.x().whileTrue(testOuttakeCommand); //TESTAR + driverXbox.b().whileTrue(intakeStopCommand); //TESTAR } public void jointTestButtonBindings() { - driverXbox.a().whileTrue(testJointCommand); + //driverXbox.a().whileTrue(testJointCommand); + driverXbox.povUp().whileTrue(jointAscendCommand); + driverXbox.povDown().whileTrue(jointDescendCommand); } public void puncherTestButtonBindings() { - driverXbox.leftBumper().onTrue(puncherConditionalCommand); + //driverXbox.leftBumper().onTrue(puncherConditionalCommand); //TESTAR + driverXbox.leftBumper().whileTrue(puncherBuildUpCommand); driverXbox.rightBumper().onTrue(testPuncherReleaseCommand); + } public void elevatorTestButtonBindings() { - driverXbox.povUp().whileTrue(testElevatorAscendCommand); - driverXbox.povDown().whileTrue(testElevatorDescendCommand); + driverXbox.povLeft().whileTrue(testElevatorAscendCommand); + driverXbox.povRight().whileTrue(testElevatorDescendCommand); } public void intakeButtonBindings() { - driverXbox.y().whileTrue(intakeCommand); - driverXbox.x().whileTrue(outtakeCommand); - driverXbox.b().whileTrue(intakeStopCommand); + driverXbox.y().whileTrue(intakeCommand); //TESTAR + driverXbox.x().whileTrue(outtakeCommand); //TESTAR + driverXbox.b().whileTrue(intakeStopCommand); //TESTAR } public void jointButtonBindings() { @@ -233,12 +238,12 @@ public void jointButtonBindings() { } public void puncherButtonBindings() { - driverXbox.rightTrigger().whileTrue(puncherBuildUpCommand); - driverXbox.rightBumper().onTrue(puncherReleaseCommand); + driverXbox.leftBumper().whileTrue(puncherBuildUpCommand); + driverXbox.rightBumper().onTrue(testPuncherReleaseCommand); } public void elevatorButtonBindings() { - driverXbox.x().whileTrue(elevatorAscendCommand); - driverXbox.b().whileTrue(elevatorDescendCommand); + driverXbox.povLeft().whileTrue(elevatorAscendCommand); + driverXbox.povRight().whileTrue(elevatorDescendCommand); } } diff --git a/src/main/java/frc/robot/buttonBindings/DriverTankButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverTankButtonBindings.java new file mode 100644 index 0000000..6956a8e --- /dev/null +++ b/src/main/java/frc/robot/buttonBindings/DriverTankButtonBindings.java @@ -0,0 +1,160 @@ +package frc.robot.buttonBindings; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants; +import frc.robot.Constants.HIDConstants; +import frc.robot.Robot; + +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.JointSubsystem; +import frc.robot.subsystems.PuncherSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervetank.Drivetrain; +import swervelib.SwerveInputStream; + +public class DriverTankButtonBindings { + private final CommandXboxController driverXbox; + + private final Drivetrain drivetrain; + private final IntakeSubsystem intakeSubsystem; + private final JointSubsystem jointSubsystem; + private final PuncherSubsystem puncherSubsystem; + private final ElevatorSubsystem elevatorSubsystem; + + private final Command driveCommand; + + private final Command intakeCommand; + private final Command outtakeCommand; + private final Command intakeStopCommand; + private final Command jointAscendCommand; + private final Command jointDescendCommand; + private final Command puncherBuildUpCommand; + private final Command puncherReleaseCommand; + private final Command elevatorAscendCommand; + private final Command elevatorDescendCommand; + private final Command driverFeedbackCommand; + + //private final ConditionalCommand puncherConditionalCommand; + + private final Command testPuncherBuildUpCommand; + private final Command testPuncherReleaseCommand; + private final Command testPuncherPrintCommand; + private final Command testIntakeCommand; + private final Command testOuttakeCommand; + private final Command testElevatorAscendCommand; + private final Command testElevatorDescendCommand; + private final Command testJointCommand; + + + public DriverTankButtonBindings(CommandXboxController driverXbox, Drivetrain drivetrain, + IntakeSubsystem intakeSubsystem, JointSubsystem jointSubsystem, PuncherSubsystem puncherSubsystem, ElevatorSubsystem elevatorSubsystem) { + this.driverXbox = driverXbox; + + this.drivetrain = drivetrain; + this.intakeSubsystem = intakeSubsystem; + this.jointSubsystem = jointSubsystem; + this.puncherSubsystem = puncherSubsystem; + this.elevatorSubsystem = elevatorSubsystem; + + driveCommand = this.drivetrain.drive(this.driverXbox::getLeftY, () -> this.driverXbox.getRightX()); + + intakeCommand = this.intakeSubsystem.percentOutCommand(() -> Constants.IntakeConstants.INTAKE); + outtakeCommand = this.intakeSubsystem.percentOutCommand(() -> Constants.IntakeConstants.OUTTAKE); + intakeStopCommand = this.intakeSubsystem.percentOutCommand(() -> 0); + jointAscendCommand = this.jointSubsystem.percentOutCommand(() -> Constants.JointConstants.ASCEND); + jointDescendCommand = this.jointSubsystem.percentOutCommand(() -> Constants.JointConstants.DESCEND); + puncherBuildUpCommand = this.puncherSubsystem.buildUpCommand(() -> Constants.PuncherConstants.POSITION); + puncherReleaseCommand = this.puncherSubsystem.releaseCommand(() -> Constants.PuncherConstants.RELEASE, () -> Constants.PuncherConstants.TIGHTEN); + elevatorAscendCommand = this.elevatorSubsystem.percentOutCommand(() -> Constants.ElevatorConstants.ASCEND); + elevatorDescendCommand = this.elevatorSubsystem.descendCommand(); + + + + driverFeedbackCommand = Commands.startEnd(() -> this.driverXbox.setRumble(RumbleType.kBothRumble, 1), () -> this.driverXbox.setRumble(RumbleType.kBothRumble, 0), puncherSubsystem).withTimeout(2); + //puncherConditionalCommand = new ConditionalCommand(puncherBuildUpCommand, driverFeedbackCommand, () -> jointSubsystem.isSafeBuildUpAngle() && !puncherSubsystem.isPuncherReady()); + + testPuncherBuildUpCommand = this.puncherSubsystem.testBuildUpCommand(); + testPuncherReleaseCommand = this.puncherSubsystem.testReleaseCommand(); + testPuncherPrintCommand = this.puncherSubsystem.testPrintCommand(); + testIntakeCommand = this.intakeSubsystem.testIntakeCommand(); + testOuttakeCommand = this.intakeSubsystem.testOuttakeCommand(); + testElevatorAscendCommand = this.elevatorSubsystem.testAscendCommand(); + testElevatorDescendCommand = this.elevatorSubsystem.testDescendCommand(); + testJointCommand = this.jointSubsystem.testOpenLoopCommand(); + } + + public void configureBindings() { + drivebaseButtonBindings(); + + if(Constants.DEV_MODE) { + intakeTestButtonBindings(); + jointTestButtonBindings(); + puncherTestButtonBindings(); + elevatorTestButtonBindings(); + } else { + intakeButtonBindings(); + jointButtonBindings(); + puncherButtonBindings(); + elevatorButtonBindings(); + } + } + + public void drivebaseButtonBindings() { + drivetrain.setDefaultCommand(driveCommand); + } + + public void intakeTestButtonBindings() { + driverXbox.y().whileTrue(testIntakeCommand); //TESTAR + driverXbox.x().whileTrue(testOuttakeCommand); //TESTAR + driverXbox.b().whileTrue(intakeStopCommand); //TESTAR + } + + public void jointTestButtonBindings() { + //driverXbox.a().whileTrue(testJointCommand); + driverXbox.povUp().whileTrue(jointAscendCommand); + driverXbox.povDown().whileTrue(jointDescendCommand); + } + + public void puncherTestButtonBindings() { + //driverXbox.leftBumper().onTrue(puncherConditionalCommand); //TESTAR + driverXbox.leftBumper().whileTrue(puncherBuildUpCommand); + driverXbox.rightBumper().onTrue(testPuncherReleaseCommand); + + } + + public void elevatorTestButtonBindings() { + driverXbox.povLeft().whileTrue(testElevatorAscendCommand); + driverXbox.povRight().whileTrue(testElevatorDescendCommand); + } + + public void intakeButtonBindings() { + driverXbox.y().whileTrue(intakeCommand); //TESTAR + driverXbox.x().whileTrue(outtakeCommand); //TESTAR + driverXbox.b().whileTrue(intakeStopCommand); //TESTAR + } + + public void jointButtonBindings() { + driverXbox.povUp().whileTrue(jointAscendCommand); + driverXbox.povDown().whileTrue(jointDescendCommand); + } + + public void puncherButtonBindings() { + driverXbox.leftBumper().whileTrue(puncherBuildUpCommand); + driverXbox.rightBumper().onTrue(testPuncherReleaseCommand); + } + + public void elevatorButtonBindings() { + driverXbox.povLeft().whileTrue(elevatorAscendCommand); + driverXbox.povRight().whileTrue(elevatorDescendCommand); + } +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 2d42f53..2a83a89 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -14,6 +15,7 @@ import java.util.function.DoubleSupplier; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; public class ElevatorSubsystem extends SubsystemBase { private final TalonSRX motor; @@ -24,6 +26,8 @@ public class ElevatorSubsystem extends SubsystemBase { public ElevatorSubsystem() { motor = new TalonSRX(Constants.ElevatorConstants.MOTOR_CAN_ID); + motor.setNeutralMode(NeutralMode.Brake); + minLimitSwitch = new DigitalInput(Constants.ElevatorConstants.LIMIT_SWITCH_DIO_PORT); if(Constants.DEV_MODE) { @@ -52,7 +56,7 @@ public Command percentOutCommand(DoubleSupplier speed) { } public Command descendCommand() { - return Commands.startEnd(() -> this.percentOut(-1), this::stop, this).until(this::isAtMin); + return new ConditionalCommand(percentOutCommand(() -> -1), percentOutCommand(() -> 0), () -> !this.isAtMin()); } public Command testDescendCommand() { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 8d099ae..545743b 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -52,11 +52,11 @@ public Command percentOutCommand(DoubleSupplier speed) { } public Command testIntakeCommand() { - return Commands.startEnd(() -> this.percentOut(-intakeDutyCycleValue.getDouble(0.3)), this::stop, this); + return Commands.startEnd(this::stop,() -> this.percentOut(-intakeDutyCycleValue.getDouble(0.3)), this); } public Command testOuttakeCommand() { - return Commands.startEnd(() -> this.percentOut(outtakeDutyCycleValue.getDouble(0.6)), this::stop, this); + return Commands.startEnd(this::stop, () -> this.percentOut(outtakeDutyCycleValue.getDouble(0.6)), this); } public void debug() { diff --git a/src/main/java/frc/robot/subsystems/JointSubsystem.java b/src/main/java/frc/robot/subsystems/JointSubsystem.java index f2ba3f7..2b8db09 100644 --- a/src/main/java/frc/robot/subsystems/JointSubsystem.java +++ b/src/main/java/frc/robot/subsystems/JointSubsystem.java @@ -78,9 +78,6 @@ public JointSubsystem() { jointLeftMotor.configAllSettings(masterConfigs); jointRightMotor.configAllSettings(slaveConfigs); - jointLeftMotor.configSelectedFeedbackSensor(FeedbackDevice.Analog, 0, 0); - jointLeftMotor.configSelectedFeedbackSensor(FeedbackDevice.None, 1, 0); - jointLeftMotor.setNeutralMode(NeutralMode.Brake); jointRightMotor.setNeutralMode(NeutralMode.Brake); @@ -195,17 +192,11 @@ public void debug() { absoluteEncoderFrequency = absoluteEncoder.getFrequency(); absoluteEncoderAdjusted = ((absoluteEncoder.get()) + absoluteEncoderOffsetValue.getDouble(0))*(absoluteEncoderMultiplierValue.getDouble(1)); absoluteEncoderRaw = (absoluteEncoder.get()); - analogEncoderAdjusted = (jointLeftMotor.getSelectedSensorPosition(0) + analogEncoderOffsetValue.getDouble(0))*(analogEncoderMultiplierValue.getDouble(1)); - analogEncoderRaw = jointLeftMotor.getSelectedSensorPosition(0); SmartDashboard.putBoolean("Joint Subsystem/Absolute Encoder/Connected", absoluteEncoderConnected); SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Frequency", absoluteEncoderFrequency); SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Adjusted", absoluteEncoderAdjusted); SmartDashboard.putNumber("Joint Subsystem/Absolute Encoder/Raw", absoluteEncoderRaw); - SmartDashboard.putNumber("Joint Subsystem/Analog Encoder/Adjusted", analogEncoderAdjusted); - SmartDashboard.putNumber("Joint Subsystem/Analog Encoder/Raw", analogEncoderRaw); - SmartDashboard.putBoolean("Joint Subsystem/Encoders/Is Matching", analogEncoderAdjusted == absoluteEncoderAdjusted); - SmartDashboard.putNumber("Joint Subsystem/Encoders/Get Difference", analogEncoderAdjusted - absoluteEncoderAdjusted); } @Override diff --git a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java index 0e25ea5..8c046d5 100644 --- a/src/main/java/frc/robot/subsystems/PuncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PuncherSubsystem.java @@ -2,6 +2,7 @@ import java.util.function.DoubleSupplier; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -45,13 +46,16 @@ public class PuncherSubsystem extends SubsystemBase { private GenericEntry releaseTimeOutValue; private GenericEntry releaseDutyCycleValue; private GenericEntry tightenDutyCycleValue; + private GenericEntry encoderSetpointValue; public PuncherSubsystem() { puncherLeftMotor = new TalonFX(Constants.PuncherConstants.LEFT_MOTOR_CAN_ID); puncherRightMotor = new TalonFX(Constants.PuncherConstants.RIGHT_MOTOR_CAN_ID); follower = new Follower(Constants.PuncherConstants.LEFT_MOTOR_CAN_ID, false); puncherReleaseMotor = new TalonSRX(Constants.PuncherConstants.RELEASE_MOTOR_CAN_ID); - puncherReleaseFeedback = new Encoder(Constants.PuncherConstants.ENCODER_DIO_PORT_A, Constants.PuncherConstants.ENCODER_DIO_PORT_B); + puncherReleaseFeedback = new Encoder(Constants.PuncherConstants.ENCODER_DIO_PORT_B, Constants.PuncherConstants.ENCODER_DIO_PORT_A); + + puncherReleaseMotor.setNeutralMode(NeutralMode.Brake); puncherLeftMotor.setPosition(0); puncherRightMotor.setPosition(0); @@ -112,6 +116,9 @@ public PuncherSubsystem() { tightenDutyCycleValue = Shuffleboard.getTab("Puncher Subsystem").add("Tighten: DutyCycleOut", 0.31) .withWidget(BuiltInWidgets.kTextView).getEntry(); + + encoderSetpointValue = Shuffleboard.getTab("Puncher Subsystem").add("Release: Setpoint", 500) + .withWidget(BuiltInWidgets.kTextView).getEntry(); } } @@ -157,11 +164,12 @@ public void stopRelease() { public void resetEncoders() { puncherLeftMotor.setPosition(0); puncherRightMotor.setPosition(0); + puncherReleaseFeedback.reset(); } public Command buildUpCommand(DoubleSupplier position) { return Commands.startEnd(() -> this.goToPosition(position.getAsDouble()), () -> {this.goToPosition(0); - this.setIsPuncherReady(true);}, this).until(() -> puncherLeftMotor.getPosition().getValueAsDouble() > 20); + this.setIsPuncherReady(true);}, this); //ADICIONAR UNTIL PARA POSICAO ESPECIFICA E TESTAR } public Command releaseCommand(DoubleSupplier speed1, DoubleSupplier speed2) { @@ -182,11 +190,11 @@ public Command testBuildUpCommand() { } public Command testReleaseCommand() { - return Commands.startEnd(() -> this.setRelease(-releaseDutyCycleValue.getDouble(0.3)), this::stopRelease, this) - .until(() -> puncherReleaseFeedback.get() == 500).andThen - (Commands.startEnd(() -> this.setRelease(tightenDutyCycleValue.getDouble(0.31)), () -> {this.stopRelease(); + return Commands.startEnd(() -> this.setRelease(-0.3), this::stopRelease, this) + .until(() -> puncherReleaseFeedback.get() > 440).andThen + (Commands.startEnd(() -> this.setRelease(0.3), () -> {this.stopRelease(); this.setIsPuncherReady(false);}, this) - .until(() -> puncherReleaseFeedback.get() == 0)); + .until(() -> puncherReleaseFeedback.get() < 10)); } public void resetState() { diff --git a/src/main/java/frc/robot/subsystems/swervetank/Drivetrain.java b/src/main/java/frc/robot/subsystems/swervetank/Drivetrain.java new file mode 100644 index 0000000..c46a9a0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervetank/Drivetrain.java @@ -0,0 +1,138 @@ +package frc.robot.subsystems.swervetank; + +import java.nio.file.ClosedFileSystemException; +import java.util.function.DoubleSupplier; + +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +public class Drivetrain extends SubsystemBase { + private final DifferentialDrive differentialDrive; + + private final SparkMax leftFrontDrive; + private final SparkMax leftBackDrive; + private final SparkMax rightFrontDrive; + private final SparkMax rightBackDrive; + + private final SparkMax leftFrontAngle; + private final SparkMax leftBackAngle; + private final SparkMax rightFrontAngle; + private final SparkMax rightBackAngle; + + private final SparkClosedLoopController leftFrontAngleClosedLoopController; + private final SparkClosedLoopController leftBackAngleClosedLoopController; + private final SparkClosedLoopController rightFrontAngleClosedLoopController; + private final SparkClosedLoopController rightBackAngleClosedLoopController; + + private final SparkMaxConfig globalDriveConfig; + private final SparkMaxConfig globalAngleConfig; + + private final SparkMaxConfig leftFollowerConfig; + private final SparkMaxConfig rightFollowerConfig; + + + public Drivetrain() { + leftFrontDrive = new SparkMax(1, MotorType.kBrushless); + leftBackDrive = new SparkMax(7, MotorType.kBrushless); + rightFrontDrive = new SparkMax(3, MotorType.kBrushless); + rightBackDrive = new SparkMax(5, MotorType.kBrushless); + + leftFrontAngle = new SparkMax(4, MotorType.kBrushless); + leftBackAngle = new SparkMax(9, MotorType.kBrushless); + rightFrontAngle = new SparkMax(2, MotorType.kBrushless); + rightBackAngle = new SparkMax(6, MotorType.kBrushless); + + leftFrontAngleClosedLoopController = leftFrontAngle.getClosedLoopController(); + leftBackAngleClosedLoopController = leftBackAngle.getClosedLoopController(); + rightFrontAngleClosedLoopController = rightFrontAngle.getClosedLoopController(); + rightBackAngleClosedLoopController = rightBackAngle.getClosedLoopController(); + + globalDriveConfig = new SparkMaxConfig(); + globalAngleConfig = new SparkMaxConfig(); + + leftFollowerConfig = new SparkMaxConfig(); + rightFollowerConfig = new SparkMaxConfig(); + + globalDriveConfig + .smartCurrentLimit(50) + .idleMode(IdleMode.kCoast); + + globalAngleConfig + .smartCurrentLimit(50) + .idleMode(IdleMode.kBrake); + + globalAngleConfig.encoder + .positionConversionFactor(1) + .velocityConversionFactor(1); + + globalAngleConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .p(0.1) + .i(0) + .d(0) + .outputRange(-1, 1); + + leftFollowerConfig + .apply(globalDriveConfig) + .follow(leftFrontDrive); + + rightFollowerConfig + .apply(globalDriveConfig) + .follow(rightFrontDrive); + + leftFrontDrive.configure(globalDriveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + leftBackDrive.configure(leftFollowerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + rightFrontDrive.configure(globalDriveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + rightBackDrive.configure(rightFollowerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + leftFrontAngle.configure(globalAngleConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + leftBackAngle.configure(globalAngleConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + rightFrontAngle.configure(globalAngleConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + rightBackAngle.configure(globalAngleConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + differentialDrive = new DifferentialDrive(leftFrontDrive::set, rightFrontDrive::set); + } + + public Command drive(DoubleSupplier leftSpeed, DoubleSupplier rightSpeed) { + return this.run(() -> differentialDrive.arcadeDrive(leftSpeed.getAsDouble(), rightSpeed.getAsDouble())); + } + + public void setMotorBrake(boolean isBrake) { + + } + + public void zeroGyro() { + + } + + public Command autonomousCommand() { + return this.runEnd(() -> differentialDrive.tankDrive(1, 1), differentialDrive::stopMotor).withTimeout(5); + } + + public void lockMotors() { + leftFrontAngleClosedLoopController.setReference(0, ControlType.kPosition, ClosedLoopSlot.kSlot0); + leftBackAngleClosedLoopController.setReference(0, ControlType.kPosition, ClosedLoopSlot.kSlot0); + rightFrontAngleClosedLoopController.setReference(0, ControlType.kPosition, ClosedLoopSlot.kSlot0); + rightBackAngleClosedLoopController.setReference(0, ControlType.kPosition, ClosedLoopSlot.kSlot0); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Tank/Position", leftFrontAngle.getEncoder().getPosition()); + lockMotors(); + } +} diff --git a/vendordeps/PathplannerLib-2025.2.6.json b/vendordeps/PathplannerLib-2025.2.6.json new file mode 100644 index 0000000..95ba203 --- /dev/null +++ b/vendordeps/PathplannerLib-2025.2.6.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2025.2.6.json", + "name": "PathplannerLib", + "version": "2025.2.6", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2025", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2025.2.6" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2025.2.6", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json deleted file mode 100644 index eef8f6e..0000000 --- a/vendordeps/PathplannerLib.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib.json", - "name": "PathplannerLib", - "version": "2025.2.1", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2025", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2025.2.1" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2025.2.1", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/Phoenix6-25.2.1.json b/vendordeps/Phoenix6-25.2.1.json deleted file mode 100644 index 04fa95a..0000000 --- a/vendordeps/Phoenix6-25.2.1.json +++ /dev/null @@ -1,419 +0,0 @@ -{ - "fileName": "Phoenix6-25.2.1.json", - "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", - "frcYear": "2025", - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "25.2.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "api-cpp", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "api-cpp-sim", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "25.2.1", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.2.1", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.2.1", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.2.1", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.2.1", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.2.1", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.2.1", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.2.1", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.2.1", - "libName": "CTRE_SimProTalonFXS", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.2.1", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.2.1", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.2.1", - "libName": "CTRE_SimProCANrange", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file diff --git a/vendordeps/Phoenix6-25.3.1.json b/vendordeps/Phoenix6-25.3.1.json new file mode 100644 index 0000000..a216d97 --- /dev/null +++ b/vendordeps/Phoenix6-25.3.1.json @@ -0,0 +1,449 @@ +{ + "fileName": "Phoenix6-25.3.1.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.3.1", + "frcYear": "2025", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.3.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.3.1", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.3.1", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.3.1", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.3.1", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.3.1", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.3.1", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.3.1", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.3.1", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.3.1", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.3.1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.3.1", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.3.1", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.3.1", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index ac85012..ac62be8 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,71 +1,71 @@ { - "fileName": "REVLib.json", - "name": "REVLib", - "version": "2025.0.1", - "frcYear": "2025", - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "mavenUrls": [ - "https://maven.revrobotics.com/" - ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-java", - "version": "2025.0.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2025.0.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-cpp", - "version": "2025.0.1", - "libName": "REVLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2025.0.1", - "libName": "REVLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2025.0.3", + "frcYear": "2025", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2025.0.3" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2025.0.3", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.3", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] } \ No newline at end of file diff --git a/vendordeps/ReduxLib-2025.0.0.json b/vendordeps/ReduxLib-2025.0.0.json deleted file mode 100644 index 41303ed..0000000 --- a/vendordeps/ReduxLib-2025.0.0.json +++ /dev/null @@ -1,72 +0,0 @@ -{ - "fileName": "ReduxLib-2025.0.0.json", - "name": "ReduxLib", - "version": "2025.0.0", - "frcYear": "2025", - "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", - "mavenUrls": [ - "https://maven.reduxrobotics.com/" - ], - "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", - "javaDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-java", - "version": "2025.0.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-driver", - "version": "2025.0.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-cpp", - "version": "2025.0.0", - "libName": "ReduxLib", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - }, - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-driver", - "version": "2025.0.0", - "libName": "ReduxCore", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/ReduxLib-2025.0.1.json b/vendordeps/ReduxLib-2025.0.1.json new file mode 100644 index 0000000..6cc750e --- /dev/null +++ b/vendordeps/ReduxLib-2025.0.1.json @@ -0,0 +1,72 @@ +{ + "fileName": "ReduxLib-2025.0.1.json", + "name": "ReduxLib", + "version": "2025.0.1", + "frcYear": "2025", + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "mavenUrls": [ + "https://maven.reduxrobotics.com/" + ], + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", + "javaDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-cpp", + "version": "2025.0.1", + "libName": "ReduxLib", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + }, + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.1", + "libName": "ReduxCore", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json index 7b29626..93fde22 100644 --- a/vendordeps/ThriftyLib.json +++ b/vendordeps/ThriftyLib.json @@ -1,20 +1,20 @@ { - "fileName": "ThriftyLib.json", - "name": "ThriftyLib", - "version": "2025.0.2", - "frcYear": "2025", - "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", - "mavenUrls": [ - "https://docs.home.thethriftybot.com" - ], - "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json", - "javaDependencies": [ - { - "groupId": "com.thethriftybot.frc", - "artifactId": "ThriftyLib-java", - "version": "2025.0.2" - } - ], - "jniDependencies": [], - "cppDependencies": [] + "fileName": "ThriftyLib.json", + "name": "ThriftyLib", + "version": "2025.1.0", + "frcYear": "2025", + "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", + "mavenUrls": [ + "https://docs.home.thethriftybot.com" + ], + "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json", + "javaDependencies": [ + { + "groupId": "com.thethriftybot.frc", + "artifactId": "ThriftyLib-java", + "version": "2025.1.0" + } + ], + "jniDependencies": [], + "cppDependencies": [] } \ No newline at end of file diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index a524490..a6ee6d7 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.3.2", + "version": "0.3.10", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.3.2" + "version": "0.3.10" }, { "groupId": "org.dyn4j", diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 28ce4f4..6bc95c7 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,71 +1,71 @@ { - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2025.1.1", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2025", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.1.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2025.1.1", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.1.1", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2025.1.1" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2025.1.1" - } - ] + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.3.1-rc1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1-rc1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.3.1-rc1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1-rc1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.3.1-rc1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.3.1-rc1" + } + ] } \ No newline at end of file diff --git a/vendordeps/yagsl-2025.2.2.json b/vendordeps/yagsl-2025.2.2.json deleted file mode 100644 index 23386f5..0000000 --- a/vendordeps/yagsl-2025.2.2.json +++ /dev/null @@ -1,64 +0,0 @@ -{ - "fileName": "yagsl-2025.2.2.json", - "name": "YAGSL", - "version": "2025.2.2", - "frcYear": "2025", - "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", - "mavenUrls": [ - "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/repos" - ], - "jsonUrl": "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/yagsl.json", - "javaDependencies": [ - { - "groupId": "swervelib", - "artifactId": "YAGSL-java", - "version": "2025.2.2" - } - ], - "requires": [ - { - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "errorMessage": "REVLib is required!", - "offlineFileName": "REVLib-2025.0.0.json", - "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json" - }, - { - "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", - "errorMessage": "ReduxLib is required!", - "offlineFileName": "ReduxLib-2025.0.0.json", - "onlineUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json" - }, - { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Phoenix6 is required!", - "offlineFileName": "Phoenix6-25.1.0.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" - }, - { - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "errorMessage": "Phoenix5 is required!", - "offlineFileName": "Phoenix5-5.35.0.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json" - }, - { - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "errorMessage": "Studica is required!", - "offlineFileName": "Studica-2025.0.0.json", - "onlineUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json" - }, - { - "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", - "errorMessage": "ThriftyLib is required!", - "offlineFileName": "ThriftyLib.json", - "onlineUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json" - }, - { - "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", - "errorMessage": "maple-sim is required for simulation", - "offlineFileName": "maple-sim.json", - "onlineUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json" - } - ], - "jniDependencies": [], - "cppDependencies": [] -} \ No newline at end of file diff --git a/vendordeps/yagsl-2025.7.2.json b/vendordeps/yagsl-2025.7.2.json new file mode 100644 index 0000000..9fb6a48 --- /dev/null +++ b/vendordeps/yagsl-2025.7.2.json @@ -0,0 +1,64 @@ +{ + "fileName": "yagsl-2025.7.2.json", + "name": "YAGSL", + "version": "2025.7.2", + "frcYear": "2025", + "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", + "mavenUrls": [ + "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/repos" + ], + "jsonUrl": "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/yagsl.json", + "javaDependencies": [ + { + "groupId": "swervelib", + "artifactId": "YAGSL-java", + "version": "2025.7.2" + } + ], + "requires": [ + { + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "errorMessage": "REVLib is required!", + "offlineFileName": "REVLib-2025.0.0.json", + "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json" + }, + { + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "errorMessage": "ReduxLib is required!", + "offlineFileName": "ReduxLib-2025.0.0.json", + "onlineUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json" + }, + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix6 is required!", + "offlineFileName": "Phoenix6-25.1.0.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" + }, + { + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "errorMessage": "Phoenix5 is required!", + "offlineFileName": "Phoenix5-5.35.0.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json" + }, + { + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "errorMessage": "Studica is required!", + "offlineFileName": "Studica-2025.0.0.json", + "onlineUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json" + }, + { + "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", + "errorMessage": "ThriftyLib is required!", + "offlineFileName": "ThriftyLib.json", + "onlineUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json" + }, + { + "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", + "errorMessage": "maple-sim is required for simulation", + "offlineFileName": "maple-sim.json", + "onlineUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} \ No newline at end of file From 8b9863751b0f942ef9a40c5897bc48ed036b3dcb Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Thu, 20 Mar 2025 20:27:52 -0300 Subject: [PATCH 20/21] commit --- src/main/deploy/swerve/modules/frontleft.json | 2 +- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 36 +- .../DriverTankButtonBindings.java | 69 +- .../robot/subsystems/LimelightHelpers.java | 1700 +++++++++++++++++ .../subsystems/swervetank/Drivetrain.java | 6 +- 6 files changed, 1763 insertions(+), 52 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/LimelightHelpers.java diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index 2ae33c5..179a57c 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -3,7 +3,7 @@ "front": -10.531, "left": -10.531 }, - "absoluteEncoderOffset": -271, + "absoluteEncoderOffset": -91, "drive": { "type": "sparkmax_neo", "id": 5, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fe185c4..2d6db25 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -29,7 +29,7 @@ public static final class SwerveConstants { public static class IntakeConstants { public static final int MOTOR_CAN_ID = 12; public static final double INTAKE = -0.4; - public static final double OUTTAKE = 0.7; + public static final double OUTTAKE = 1.0; } public static class JointConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9449ff7..b977820 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,16 +31,16 @@ public class RobotContainer { private final CommandXboxController driverXbox; private final CommandXboxController operatorXbox; - private final SwerveSubsystem swerveSubsystem; - //private final Drivetrain swerveSubsystem; + //private final SwerveSubsystem swerveSubsystem; + private final Drivetrain swerveSubsystem; - private final IntakeSubsystem intakeSubsystem; - private final PuncherSubsystem puncherSubsystem; - private final JointSubsystem jointSubsystem; + //private final IntakeSubsystem intakeSubsystem; + //private final PuncherSubsystem puncherSubsystem; + //private final JointSubsystem jointSubsystem; private final ElevatorSubsystem elevatorSubsystem; - private final DriverButtonBindings driverButtonBindings; - //private final DriverTankButtonBindings driverButtonBindings; + //private final DriverButtonBindings driverButtonBindings; + private final DriverTankButtonBindings driverButtonBindings; private final OperatorButtonBindings operatorButtonBindings; SendableChooser autonomousChooser; @@ -49,15 +49,15 @@ public RobotContainer() { driverXbox = new CommandXboxController(0); operatorXbox = new CommandXboxController(1); - swerveSubsystem = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),"swerve")); - //swerveSubsystem = new Drivetrain(); - intakeSubsystem = new IntakeSubsystem(); - puncherSubsystem = new PuncherSubsystem(); - jointSubsystem = new JointSubsystem(); + //swerveSubsystem = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),"swerve")); + swerveSubsystem = new Drivetrain(); + //intakeSubsystem = new IntakeSubsystem(); + //puncherSubsystem = new PuncherSubsystem(); + //jointSubsystem = new JointSubsystem(); elevatorSubsystem = new ElevatorSubsystem(); - driverButtonBindings = new DriverButtonBindings(driverXbox, swerveSubsystem, intakeSubsystem, jointSubsystem, puncherSubsystem, elevatorSubsystem); - //driverButtonBindings = new DriverTankButtonBindings(driverXbox, swerveSubsystem, intakeSubsystem, jointSubsystem, puncherSubsystem, elevatorSubsystem); + //driverButtonBindings = new DriverButtonBindings(driverXbox, swerveSubsystem, intakeSubsystem, jointSubsystem, puncherSubsystem, elevatorSubsystem); + driverButtonBindings = new DriverTankButtonBindings(driverXbox, swerveSubsystem, elevatorSubsystem); operatorButtonBindings = new OperatorButtonBindings(operatorXbox); configureBindings(); @@ -74,8 +74,8 @@ private void configureBindings() { } public Command getAutonomousCommand() { - //return swerveSubsystem.autonomousCommand(); - return null; //autonomousChooser.getSelected(); + return swerveSubsystem.autonomousCommand(); + //return null; //autonomousChooser.getSelected(); } public void autonomousChooserSetup() { @@ -92,8 +92,8 @@ public void setMotorBrake(boolean brake) { } public void resetPuncherSubsystemEncoders() { - puncherSubsystem.resetEncoders(); - puncherSubsystem.resetState(); + //puncherSubsystem.resetEncoders(); + //puncherSubsystem.resetState(); } public void zeroGyro() { diff --git a/src/main/java/frc/robot/buttonBindings/DriverTankButtonBindings.java b/src/main/java/frc/robot/buttonBindings/DriverTankButtonBindings.java index 6956a8e..a9d2ecc 100644 --- a/src/main/java/frc/robot/buttonBindings/DriverTankButtonBindings.java +++ b/src/main/java/frc/robot/buttonBindings/DriverTankButtonBindings.java @@ -26,85 +26,96 @@ public class DriverTankButtonBindings { private final CommandXboxController driverXbox; private final Drivetrain drivetrain; - private final IntakeSubsystem intakeSubsystem; - private final JointSubsystem jointSubsystem; - private final PuncherSubsystem puncherSubsystem; + //private final PuncherSubsystem puncherSubsystem; private final ElevatorSubsystem elevatorSubsystem; private final Command driveCommand; + /* + private final IntakeSubsystem intakeSubsystem; + private final JointSubsystem jointSubsystem; + private final Command intakeCommand; private final Command outtakeCommand; private final Command intakeStopCommand; private final Command jointAscendCommand; private final Command jointDescendCommand; - private final Command puncherBuildUpCommand; - private final Command puncherReleaseCommand; - private final Command elevatorAscendCommand; - private final Command elevatorDescendCommand; + //private final Command puncherBuildUpCommand; + //private final Command puncherReleaseCommand; private final Command driverFeedbackCommand; //private final ConditionalCommand puncherConditionalCommand; - private final Command testPuncherBuildUpCommand; - private final Command testPuncherReleaseCommand; - private final Command testPuncherPrintCommand; + //private final Command testPuncherBuildUpCommand; + //private final Command testPuncherReleaseCommand; + //private final Command testPuncherPrintCommand; private final Command testIntakeCommand; private final Command testOuttakeCommand; private final Command testElevatorAscendCommand; private final Command testElevatorDescendCommand; private final Command testJointCommand; + */ + + private final Command elevatorAscendCommand; + private final Command elevatorDescendCommand; - public DriverTankButtonBindings(CommandXboxController driverXbox, Drivetrain drivetrain, - IntakeSubsystem intakeSubsystem, JointSubsystem jointSubsystem, PuncherSubsystem puncherSubsystem, ElevatorSubsystem elevatorSubsystem) { + //public DriverTankButtonBindings(CommandXboxController driverXbox, Drivetrain drivetrain, IntakeSubsystem intakeSubsystem, JointSubsystem jointSubsystem, PuncherSubsystem puncherSubsystem, ElevatorSubsystem elevatorSubsystem) { + public DriverTankButtonBindings(CommandXboxController driverXbox, Drivetrain drivetrain, ElevatorSubsystem elevatorSubsystem) { this.driverXbox = driverXbox; this.drivetrain = drivetrain; + /* this.intakeSubsystem = intakeSubsystem; this.jointSubsystem = jointSubsystem; this.puncherSubsystem = puncherSubsystem; + */ this.elevatorSubsystem = elevatorSubsystem; driveCommand = this.drivetrain.drive(this.driverXbox::getLeftY, () -> this.driverXbox.getRightX()); + /* intakeCommand = this.intakeSubsystem.percentOutCommand(() -> Constants.IntakeConstants.INTAKE); outtakeCommand = this.intakeSubsystem.percentOutCommand(() -> Constants.IntakeConstants.OUTTAKE); intakeStopCommand = this.intakeSubsystem.percentOutCommand(() -> 0); jointAscendCommand = this.jointSubsystem.percentOutCommand(() -> Constants.JointConstants.ASCEND); jointDescendCommand = this.jointSubsystem.percentOutCommand(() -> Constants.JointConstants.DESCEND); - puncherBuildUpCommand = this.puncherSubsystem.buildUpCommand(() -> Constants.PuncherConstants.POSITION); - puncherReleaseCommand = this.puncherSubsystem.releaseCommand(() -> Constants.PuncherConstants.RELEASE, () -> Constants.PuncherConstants.TIGHTEN); + //puncherBuildUpCommand = this.puncherSubsystem.buildUpCommand(() -> Constants.PuncherConstants.POSITION); + //puncherReleaseCommand = this.puncherSubsystem.releaseCommand(() -> Constants.PuncherConstants.RELEASE, () -> Constants.PuncherConstants.TIGHTEN); + */ + elevatorAscendCommand = this.elevatorSubsystem.percentOutCommand(() -> Constants.ElevatorConstants.ASCEND); elevatorDescendCommand = this.elevatorSubsystem.descendCommand(); - driverFeedbackCommand = Commands.startEnd(() -> this.driverXbox.setRumble(RumbleType.kBothRumble, 1), () -> this.driverXbox.setRumble(RumbleType.kBothRumble, 0), puncherSubsystem).withTimeout(2); + //driverFeedbackCommand = Commands.startEnd(() -> this.driverXbox.setRumble(RumbleType.kBothRumble, 1), () -> this.driverXbox.setRumble(RumbleType.kBothRumble, 0), puncherSubsystem).withTimeout(2); //puncherConditionalCommand = new ConditionalCommand(puncherBuildUpCommand, driverFeedbackCommand, () -> jointSubsystem.isSafeBuildUpAngle() && !puncherSubsystem.isPuncherReady()); - testPuncherBuildUpCommand = this.puncherSubsystem.testBuildUpCommand(); - testPuncherReleaseCommand = this.puncherSubsystem.testReleaseCommand(); - testPuncherPrintCommand = this.puncherSubsystem.testPrintCommand(); + //testPuncherBuildUpCommand = this.puncherSubsystem.testBuildUpCommand(); + //testPuncherReleaseCommand = this.puncherSubsystem.testReleaseCommand(); + //testPuncherPrintCommand = this.puncherSubsystem.testPrintCommand(); + /* testIntakeCommand = this.intakeSubsystem.testIntakeCommand(); testOuttakeCommand = this.intakeSubsystem.testOuttakeCommand(); testElevatorAscendCommand = this.elevatorSubsystem.testAscendCommand(); testElevatorDescendCommand = this.elevatorSubsystem.testDescendCommand(); testJointCommand = this.jointSubsystem.testOpenLoopCommand(); + */ } public void configureBindings() { drivebaseButtonBindings(); if(Constants.DEV_MODE) { - intakeTestButtonBindings(); - jointTestButtonBindings(); - puncherTestButtonBindings(); - elevatorTestButtonBindings(); + //intakeTestButtonBindings(); + //jointTestButtonBindings(); + //puncherTestButtonBindings(); + //elevatorTestButtonBindings(); } else { - intakeButtonBindings(); - jointButtonBindings(); - puncherButtonBindings(); + //intakeButtonBindings(); + //jointButtonBindings(); + //puncherButtonBindings(); elevatorButtonBindings(); } } @@ -113,6 +124,7 @@ public void drivebaseButtonBindings() { drivetrain.setDefaultCommand(driveCommand); } + /* public void intakeTestButtonBindings() { driverXbox.y().whileTrue(testIntakeCommand); //TESTAR driverXbox.x().whileTrue(testOuttakeCommand); //TESTAR @@ -126,9 +138,8 @@ public void jointTestButtonBindings() { } public void puncherTestButtonBindings() { - //driverXbox.leftBumper().onTrue(puncherConditionalCommand); //TESTAR - driverXbox.leftBumper().whileTrue(puncherBuildUpCommand); - driverXbox.rightBumper().onTrue(testPuncherReleaseCommand); + //driverXbox.leftBumper().whileTrue(puncherBuildUpCommand); + //driverXbox.rightBumper().onTrue(testPuncherReleaseCommand); } @@ -152,7 +163,7 @@ public void puncherButtonBindings() { driverXbox.leftBumper().whileTrue(puncherBuildUpCommand); driverXbox.rightBumper().onTrue(testPuncherReleaseCommand); } - + */ public void elevatorButtonBindings() { driverXbox.povLeft().whileTrue(elevatorAscendCommand); driverXbox.povRight().whileTrue(elevatorDescendCommand); diff --git a/src/main/java/frc/robot/subsystems/LimelightHelpers.java b/src/main/java/frc/robot/subsystems/LimelightHelpers.java new file mode 100644 index 0000000..294415b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LimelightHelpers.java @@ -0,0 +1,1700 @@ +//LimelightHelpers v1.12 (REQUIRES LLOS 2025.0 OR LATER) + +package frc.robot.subsystems; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import frc.robot.subsystems.LimelightHelpers.LimelightResults; +import frc.robot.subsystems.LimelightHelpers.PoseEstimate; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Arrays; +import java.util.Map; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; + +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. + * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. + */ +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output + */ + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output + */ + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** + * Represents a Barcode Target Result extracted from JSON Output + */ + public static class LimelightTarget_Barcode { + + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; + + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() { + } + + public String getFamily() { + return family; + } + } + + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() { + } + } + + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + */ + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + + + } + + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + */ + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + RawFiducial other = (RawFiducial) obj; + return id == other.id && + Double.compare(txnc, other.txnc) == 0 && + Double.compare(tync, other.tync) == 0 && + Double.compare(ta, other.ta) == 0 && + Double.compare(distToCamera, other.distToCamera) == 0 && + Double.compare(distToRobot, other.distToRobot) == 0 && + Double.compare(ambiguity, other.ambiguity) == 0; + } + + } + + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** + * Represents a 3D Pose Estimate. + */ + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** + * Instantiates a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + this.isMegaTag2 = false; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + PoseEstimate that = (PoseEstimate) obj; + // We don't compare the timestampSeconds as it isn't relevant for equality and makes + // unit testing harder + return Double.compare(that.latency, latency) == 0 + && tagCount == that.tagCount + && Double.compare(that.tagSpan, tagSpan) == 0 + && Double.compare(that.avgTagDist, avgTagDist) == 0 + && Double.compare(that.avgTagArea, avgTagArea) == 0 + && pose.equals(that.pose) + && Arrays.equals(rawFiducials, that.rawFiducials); + } + + } + + /** + * Encapsulates the state of an internal Limelight IMU. + */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if ("".equals(name) || name == null) { + return "limelight"; + } + return name; + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); + } + + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + /** + * Does the Limelight have a valid target? + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, + * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector pipeline. + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + /** + * Gets the capture latency. + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + /** + * Gets the active pipeline index. + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + /** + * Gets the current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } + + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Make sure you are calling setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired", false); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + /** + * Gets the current IMU data from NetworkTables. + * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. + * Returns all zeros if data is invalid or unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * Sets LED mode to be controlled by the current pipeline. + * @param limelightName Name of the Limelight camera + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + /** + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * Configures the complementary filter alpha value for IMU Assist Modes (Modes 3 and 4) + * + * @param limelightName Name/identifier of the Limelight + * @param alpha Defaults to .001. Higher values will cause the internal IMU to converge onto the assist source more rapidly. + */ + public static void SetIMUAssistAlpha(String limelightName, double alpha) { + setLimelightNTDouble(limelightName, "imuassistalpha_set", alpha); + } + + + /** + * Configures the throttle value. Set to 100-200 while disabled to reduce thermal output/temperature. + * + * @param limelightName Name/identifier of the Limelight + * @param throttle Defaults to 0. Your Limelgiht will process one frame after skipping frames. + */ + public static void SetThrottle(String limelightName, int throttle) { + setLimelightNTDouble(limelightName, "throttle_set", throttle); + } + + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. + * Tags not in this list will be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + /** + * Sets the downscaling factor for AprilTag detection. + * Increasing downscale can improve performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + /** + * Sets the camera pose relative to the robot. + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && !"".equals(snapshotName)) { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swervetank/Drivetrain.java b/src/main/java/frc/robot/subsystems/swervetank/Drivetrain.java index c46a9a0..c408677 100644 --- a/src/main/java/frc/robot/subsystems/swervetank/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervetank/Drivetrain.java @@ -69,7 +69,7 @@ public Drivetrain() { globalDriveConfig .smartCurrentLimit(50) - .idleMode(IdleMode.kCoast); + .idleMode(IdleMode.kBrake); globalAngleConfig .smartCurrentLimit(50) @@ -108,7 +108,7 @@ public Drivetrain() { } public Command drive(DoubleSupplier leftSpeed, DoubleSupplier rightSpeed) { - return this.run(() -> differentialDrive.arcadeDrive(leftSpeed.getAsDouble(), rightSpeed.getAsDouble())); + return this.run(() -> differentialDrive.arcadeDrive(-leftSpeed.getAsDouble(), -rightSpeed.getAsDouble())); } public void setMotorBrake(boolean isBrake) { @@ -120,7 +120,7 @@ public void zeroGyro() { } public Command autonomousCommand() { - return this.runEnd(() -> differentialDrive.tankDrive(1, 1), differentialDrive::stopMotor).withTimeout(5); + return this.runEnd(() -> differentialDrive.tankDrive(-0.8, -0.8), differentialDrive::stopMotor).withTimeout(5); } public void lockMotors() { From 17c5de1adb4a173fd31db927f4c751488d7309f7 Mon Sep 17 00:00:00 2001 From: temaosmetais Date: Thu, 20 Mar 2025 20:59:47 -0300 Subject: [PATCH 21/21] commit --- src/main/deploy/swerve/swervedrive.json | 5 ++-- src/main/java/frc/robot/RobotContainer.java | 31 +++++++++++---------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json index 1aacdbe..9ea97af 100644 --- a/src/main/deploy/swerve/swervedrive.json +++ b/src/main/deploy/swerve/swervedrive.json @@ -1,9 +1,8 @@ { "imu": { - "type": "adxrs450", - "id": 0, + "type": "pigeon", + "id": 19, "canbus": null - }, "invertedIMU": false, "modules": [ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b977820..845dec9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,16 +31,16 @@ public class RobotContainer { private final CommandXboxController driverXbox; private final CommandXboxController operatorXbox; - //private final SwerveSubsystem swerveSubsystem; - private final Drivetrain swerveSubsystem; + private final SwerveSubsystem swerveSubsystem; + //private final Drivetrain swerveSubsystem; - //private final IntakeSubsystem intakeSubsystem; - //private final PuncherSubsystem puncherSubsystem; - //private final JointSubsystem jointSubsystem; + private final IntakeSubsystem intakeSubsystem; + private final PuncherSubsystem puncherSubsystem; + private final JointSubsystem jointSubsystem; private final ElevatorSubsystem elevatorSubsystem; - //private final DriverButtonBindings driverButtonBindings; - private final DriverTankButtonBindings driverButtonBindings; + private final DriverButtonBindings driverButtonBindings; + //private final DriverTankButtonBindings driverButtonBindings; private final OperatorButtonBindings operatorButtonBindings; SendableChooser autonomousChooser; @@ -49,15 +49,15 @@ public RobotContainer() { driverXbox = new CommandXboxController(0); operatorXbox = new CommandXboxController(1); - //swerveSubsystem = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),"swerve")); - swerveSubsystem = new Drivetrain(); - //intakeSubsystem = new IntakeSubsystem(); - //puncherSubsystem = new PuncherSubsystem(); - //jointSubsystem = new JointSubsystem(); + swerveSubsystem = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),"swerve")); + //swerveSubsystem = new Drivetrain(); + intakeSubsystem = new IntakeSubsystem(); + puncherSubsystem = new PuncherSubsystem(); + jointSubsystem = new JointSubsystem(); elevatorSubsystem = new ElevatorSubsystem(); - //driverButtonBindings = new DriverButtonBindings(driverXbox, swerveSubsystem, intakeSubsystem, jointSubsystem, puncherSubsystem, elevatorSubsystem); - driverButtonBindings = new DriverTankButtonBindings(driverXbox, swerveSubsystem, elevatorSubsystem); + driverButtonBindings = new DriverButtonBindings(driverXbox, swerveSubsystem, intakeSubsystem, jointSubsystem, puncherSubsystem, elevatorSubsystem); + //driverButtonBindings = new DriverTankButtonBindings(driverXbox, swerveSubsystem, elevatorSubsystem); operatorButtonBindings = new OperatorButtonBindings(operatorXbox); configureBindings(); @@ -74,7 +74,8 @@ private void configureBindings() { } public Command getAutonomousCommand() { - return swerveSubsystem.autonomousCommand(); + return null; + //return swerveSubsystem.autonomousCommand(); //return null; //autonomousChooser.getSelected(); }