diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 38c9e73..0c793ca 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -6,14 +6,13 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -27,7 +26,7 @@ public class Arm extends SubsystemBase { private final CANcoder encoder = new CANcoder(32, canivore); // Position output control for the arm - private final PositionVoltage positionOut = new PositionVoltage(0); + private final MotionMagicVoltage positionOut = new MotionMagicVoltage(0); public Arm() { // Create and apply the configuration for the leader motor @@ -41,7 +40,8 @@ public Arm() { config.Slot0.kS = 0.0; // Static gain config.Slot0.kP = 0.0; // Proportional gain config.Slot0.kD = 0.0; // Derivative gain - config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; + config.MotionMagic.MotionMagicCruiseVelocity = 0.0; // Max velocity + config.MotionMagic.MotionMagicAcceleration = 0.0; // Max acceleration allowed // Configure the leader motor to use the CANcoder for position feedback config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; config.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID(); diff --git a/src/main/java/frc/robot/subsystems/Flywheel.java b/src/main/java/frc/robot/subsystems/Flywheel.java index ea7229e..c0ef686 100644 --- a/src/main/java/frc/robot/subsystems/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/Flywheel.java @@ -7,7 +7,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -24,7 +24,7 @@ public class Flywheel extends SubsystemBase { private final TalonFX follower = new TalonFX(22, canivore); // Velocity output control for the flywheel - private final VelocityVoltage velocityOut = new VelocityVoltage(0); + private final MotionMagicVelocityVoltage velocityOut = new MotionMagicVelocityVoltage(0); public Flywheel() { // Set the follower to follow the leader motor @@ -38,6 +38,8 @@ public Flywheel() { config.Slot0.kS = 0.0; // Static gain config.Slot0.kV = 0.0; // Velocity gain config.Slot0.kP = 0.0; // Proportional gain + config.MotionMagic.MotionMagicCruiseVelocity = 0.0; // Max velocity + config.MotionMagic.MotionMagicAcceleration = 0.0; // Max acceleration allowed // Try to apply config multiple time. Break after successfully applying for (int i = 0; i < 2; ++i) { var status = leader.getConfigurator().apply(config);