From b633bb02de46dea0f97ffdb34e08c24170e27d80 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 18 Aug 2024 16:50:41 +0300 Subject: [PATCH 1/9] Hehehe messy code --- .../trigon/hardware/phoenix6/talonfx/TalonFXMotor.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java index 4a59cc4c..86dc2fb7 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java +++ b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java @@ -4,6 +4,10 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.hardware.TalonFX; +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.WaitCommand; import org.littletonrobotics.junction.Logger; import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.Phoenix6Inputs; @@ -26,7 +30,11 @@ public TalonFXMotor(int id, String motorName, String canbus) { this.motorIO = generateIO(id, canbus); this.motorInputs = new Phoenix6Inputs(motorName); this.id = id; - motorIO.optimizeBusUsage(); + Commands.run(this::dl); + } + + private Command dl() { + return new WaitCommand(5).andThen(new InstantCommand(motorIO::optimizeBusUsage)); } public void update() { From 46bbe4ac5f1c805cf1f1ea204cddd0858f040339 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 18 Aug 2024 16:56:42 +0300 Subject: [PATCH 2/9] Reverted changes --- .../trigon/hardware/phoenix6/talonfx/TalonFXMotor.java | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java index 86dc2fb7..4a59cc4c 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java +++ b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java @@ -4,10 +4,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.hardware.TalonFX; -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.WaitCommand; import org.littletonrobotics.junction.Logger; import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.Phoenix6Inputs; @@ -30,11 +26,7 @@ public TalonFXMotor(int id, String motorName, String canbus) { this.motorIO = generateIO(id, canbus); this.motorInputs = new Phoenix6Inputs(motorName); this.id = id; - Commands.run(this::dl); - } - - private Command dl() { - return new WaitCommand(5).andThen(new InstantCommand(motorIO::optimizeBusUsage)); + motorIO.optimizeBusUsage(); } public void update() { From 6aadac61f057e8082e4f18011039da0adef50bcb Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 18 Aug 2024 17:22:07 +0300 Subject: [PATCH 3/9] Test thingy --- .../java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java b/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java index d2bdf698..9051985b 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java +++ b/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java @@ -23,10 +23,8 @@ public Phoenix6Inputs(String name) { public void toLog(LogTable table) { if (signals.length == 0) return; - BaseStatusSignal.refreshAll(signals); for (BaseStatusSignal signal : signals) { - if (signal.getName().equals("ClosedLoopReference")) - ((StatusSignal) signal).refresh(); + ((StatusSignal) signal).refresh(); table.put(signal.getName(), signal.getValueAsDouble()); } for (Map.Entry> entry : signalToThreadedQueue.entrySet()) { From d08b8db81efa301835891398bb5c534b9b9b936e Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 19 Aug 2024 17:32:46 +0300 Subject: [PATCH 4/9] remove optimize bus util --- .../java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java index 4a59cc4c..0f70fbfe 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java +++ b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java @@ -26,7 +26,7 @@ public TalonFXMotor(int id, String motorName, String canbus) { this.motorIO = generateIO(id, canbus); this.motorInputs = new Phoenix6Inputs(motorName); this.id = id; - motorIO.optimizeBusUsage(); +// motorIO.optimizeBusUsage(); } public void update() { From a1ae5139bbfedda27d7bb8b06f4036f05451860e Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Mon, 2 Sep 2024 21:10:20 +0300 Subject: [PATCH 5/9] Test --- .../phoenix6/talonfx/TalonFXSignal.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXSignal.java b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXSignal.java index c766068b..0dd69bb4 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXSignal.java +++ b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXSignal.java @@ -7,15 +7,15 @@ import java.util.function.Function; public enum TalonFXSignal { - POSITION(TalonFX::getPosition), - VELOCITY(TalonFX::getVelocity), - TORQUE_CURRENT(TalonFX::getTorqueCurrent), - STATOR_CURRENT(TalonFX::getStatorCurrent), - SUPPLY_CURRENT(TalonFX::getSupplyCurrent), - CLOSED_LOOP_REFERENCE(TalonFX::getClosedLoopReference), - MOTOR_VOLTAGE(TalonFX::getMotorVoltage), - FORWARD_LIMIT(TalonFX::getForwardLimit), - REVERSE_LIMIT(TalonFX::getReverseLimit); + POSITION((motor) -> motor.getPosition().clone()), + VELOCITY((motor) -> motor.getVelocity().clone()), + TORQUE_CURRENT((motor) -> motor.getTorqueCurrent().clone()), + STATOR_CURRENT((motor) -> motor.getStatorCurrent().clone()), + SUPPLY_CURRENT((motor) -> motor.getSupplyCurrent().clone()), + CLOSED_LOOP_REFERENCE((motor) -> motor.getClosedLoopReference().clone()), + MOTOR_VOLTAGE((motor) -> motor.getMotorVoltage().clone()), + FORWARD_LIMIT((motor) -> motor.getForwardLimit().clone()), + REVERSE_LIMIT((motor) -> motor.getReverseLimit().clone()); final String name; final Function signalFunction; From d468193385cdbd1a14824475e131a3d863137a04 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Mon, 2 Sep 2024 22:17:23 +0300 Subject: [PATCH 6/9] Fixed annoying problem by updating very fast? --- .../hardware/phoenix6/Phoenix6Inputs.java | 2 ++ .../phoenix6/talonfx/TalonFXSignal.java | 18 +++++++++--------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java b/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java index 5d358f13..368d3c5c 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java +++ b/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java @@ -41,6 +41,8 @@ public void registerThreadedSignal(BaseStatusSignal statusSignal, double updateF public void registerSignal(BaseStatusSignal statusSignal, double updateFrequencyHertz) { if (statusSignal == null || RobotHardwareStats.isReplay()) return; + if (RobotHardwareStats.isSimulation()) + updateFrequencyHertz = 1000; // For some reason, simulation sometimes malfunctions if a status signal isn't updated frequently enough. statusSignal.setUpdateFrequency(updateFrequencyHertz); addSignalToSignalsArray(statusSignal); diff --git a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXSignal.java b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXSignal.java index 0dd69bb4..c766068b 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXSignal.java +++ b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXSignal.java @@ -7,15 +7,15 @@ import java.util.function.Function; public enum TalonFXSignal { - POSITION((motor) -> motor.getPosition().clone()), - VELOCITY((motor) -> motor.getVelocity().clone()), - TORQUE_CURRENT((motor) -> motor.getTorqueCurrent().clone()), - STATOR_CURRENT((motor) -> motor.getStatorCurrent().clone()), - SUPPLY_CURRENT((motor) -> motor.getSupplyCurrent().clone()), - CLOSED_LOOP_REFERENCE((motor) -> motor.getClosedLoopReference().clone()), - MOTOR_VOLTAGE((motor) -> motor.getMotorVoltage().clone()), - FORWARD_LIMIT((motor) -> motor.getForwardLimit().clone()), - REVERSE_LIMIT((motor) -> motor.getReverseLimit().clone()); + POSITION(TalonFX::getPosition), + VELOCITY(TalonFX::getVelocity), + TORQUE_CURRENT(TalonFX::getTorqueCurrent), + STATOR_CURRENT(TalonFX::getStatorCurrent), + SUPPLY_CURRENT(TalonFX::getSupplyCurrent), + CLOSED_LOOP_REFERENCE(TalonFX::getClosedLoopReference), + MOTOR_VOLTAGE(TalonFX::getMotorVoltage), + FORWARD_LIMIT(TalonFX::getForwardLimit), + REVERSE_LIMIT(TalonFX::getReverseLimit); final String name; final Function signalFunction; From 2415d46bf61db7c8e2e77c08ab3aa959aebb5028 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Mon, 2 Sep 2024 22:23:47 +0300 Subject: [PATCH 7/9] Changed update freq to 100 --- src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java b/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java index 368d3c5c..941eea53 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java +++ b/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java @@ -42,7 +42,7 @@ public void registerSignal(BaseStatusSignal statusSignal, double updateFrequency if (statusSignal == null || RobotHardwareStats.isReplay()) return; if (RobotHardwareStats.isSimulation()) - updateFrequencyHertz = 1000; // For some reason, simulation sometimes malfunctions if a status signal isn't updated frequently enough. + updateFrequencyHertz = 100; // For some reason, simulation sometimes malfunctions if a status signal isn't updated frequently enough. statusSignal.setUpdateFrequency(updateFrequencyHertz); addSignalToSignalsArray(statusSignal); From 1e3ed563d64cb0e6f51f0f7e61bb05cd4caa7aac Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Mon, 2 Sep 2024 22:31:37 +0300 Subject: [PATCH 8/9] update freq = 50 for threaded signals --- src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java b/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java index 941eea53..4db0b965 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java +++ b/src/main/java/org/trigon/hardware/phoenix6/Phoenix6Inputs.java @@ -35,6 +35,7 @@ public void registerThreadedSignal(BaseStatusSignal statusSignal, double updateF return; registerSignal(statusSignal, updateFrequencyHertz); + statusSignal.setUpdateFrequency(50); signalToThreadedQueue.put(statusSignal.getName() + "_Threaded", signalThread.registerSignal(statusSignal)); } From 5e44d253b30e548d1e7a75abe2708595ac34fffa Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Mon, 2 Sep 2024 22:38:19 +0300 Subject: [PATCH 9/9] Returned `optimizeBusUtilization` --- .../java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java index 1dd938eb..bf0b786a 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java +++ b/src/main/java/org/trigon/hardware/phoenix6/talonfx/TalonFXMotor.java @@ -26,7 +26,7 @@ public TalonFXMotor(int id, String motorName, String canbus) { this.motorIO = generateIO(id, canbus); this.motorInputs = new Phoenix6Inputs(motorName); this.id = id; -// motorIO.optimizeBusUsage(); + motorIO.optimizeBusUsage(); } public void update() {