From a65a2577aeec4c62f8b54c5d7579efa1c316184e Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Tue, 2 Apr 2024 18:08:24 -0400 Subject: [PATCH] fix: Change gear ratios. --- simgui-ds.json | 2 +- .../frc/robot/arm/ShoulderMotorIOTalonFX.java | 30 ++++++++++++++----- .../robot/intake/RollerMotorIOTalonFX.java | 8 ++++- .../robot/shooter/FlywheelMotorIOTalonFX.java | 5 ++++ .../shooter/SerializerMotorIOTalonFX.java | 5 ++++ 5 files changed, 41 insertions(+), 9 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 2e2b48e..af8e13e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -98,10 +98,10 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", "useGamepad": true }, { + "guid": "78696e70757401000000000000000000", "useGamepad": true } ] diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOTalonFX.java b/src/main/java/frc/robot/arm/ShoulderMotorIOTalonFX.java index a87429b..fec80e7 100644 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOTalonFX.java +++ b/src/main/java/frc/robot/arm/ShoulderMotorIOTalonFX.java @@ -1,9 +1,13 @@ package frc.robot.arm; +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; + import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; @@ -14,6 +18,8 @@ public class ShoulderMotorIOTalonFX implements ShoulderMotorIO { private final TalonFX eastTalonFX, westTalonFX; + private final CANcoder cancoder; + /** Feedback controller for shoulder position. */ private final PIDController feedback; @@ -25,6 +31,8 @@ public ShoulderMotorIOTalonFX() { eastTalonFX = new TalonFX(0); // TODO westTalonFX = new TalonFX(0); // TODO + cancoder = new CANcoder(0); // TODO + feedback = new PIDController(0, 0, 0); feedforward = new ArmFeedforward(0, 0, 0); @@ -32,17 +40,25 @@ public ShoulderMotorIOTalonFX() { @Override public void configure() { - final TalonFXConfiguration config = new TalonFXConfiguration(); + final TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); + + // talonFXConfig.Feedback.SensorToMechanismRatio = 39.771428571; + + talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + talonFXConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + Configurator.configureTalonFX(eastTalonFX.getConfigurator(), talonFXConfig); + + talonFXConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + Configurator.configureTalonFX(westTalonFX.getConfigurator(), talonFXConfig); - config.Feedback.SensorToMechanismRatio = 1.0; // TODO + final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - Configurator.configureTalonFX(eastTalonFX.getConfigurator(), config); + cancoderConfig.MagnetSensor.MagnetOffset = -0.0; // TODO - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - Configurator.configureTalonFX(westTalonFX.getConfigurator(), config); + Configurator.configureCANcoder(cancoder.getConfigurator(), cancoderConfig); } @Override diff --git a/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java b/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java index 6644005..e27214f 100644 --- a/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java +++ b/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import frc.lib.Configurator; @@ -24,11 +25,16 @@ public RollerMotorIOTalonFX() { public void configure() { final TalonFXConfiguration config = new TalonFXConfiguration(); - config.Feedback.SensorToMechanismRatio = 1.0; // TODO + config.Feedback.SensorToMechanismRatio = 24.0 / 16.0; config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + Configurator.configureTalonFX(topTalonFX.getConfigurator(), config); + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + Configurator.configureTalonFX(bottomTalonFX.getConfigurator(), config); } diff --git a/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java b/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java index 493993e..12a96bd 100644 --- a/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java +++ b/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import frc.lib.Configurator; @@ -29,8 +30,12 @@ public FlywheelMotorIOTalonFX() { public void configure() { final TalonFXConfiguration config = new TalonFXConfiguration(); + config.Feedback.SensorToMechanismRatio = 36.0 / 16.0; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + Configurator.configureTalonFX(talonFX.getConfigurator(), config); } diff --git a/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java b/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java index a2de8e3..23960f7 100644 --- a/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java +++ b/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import frc.lib.Configurator; @@ -29,8 +30,12 @@ public SerializerMotorIOTalonFX() { public void configure() { final TalonFXConfiguration config = new TalonFXConfiguration(); + config.Feedback.SensorToMechanismRatio = 36.0 / 16.0; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + Configurator.configureTalonFX(talonFX.getConfigurator(), config); }