diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 0c1400b2..c2b6a39c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -5,9 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import com.stuypulse.stuylib.network.SmartNumber; - import com.pathplanner.lib.config.PIDConstants; +import com.stuypulse.stuylib.network.SmartNumber; public class Gains { @@ -40,7 +39,7 @@ public interface slot0 { double kD = 0.0; double kS = 0.4775; - double kV = 0.0; + double kV = 0.12; double kA = 0.0; } @@ -50,7 +49,7 @@ public interface slot1 { SmartNumber kD = new SmartNumber("Superstructure/Turret/Gains/kD", 10.0); SmartNumber kS = new SmartNumber("Superstructure/Turret/Gains/kS", 0.4775); - SmartNumber kV = new SmartNumber("Superstructure/Turret/Gains/kV", 0.0); + SmartNumber kV = new SmartNumber("Superstructure/Turret/Gains/kV", 0.12); SmartNumber kA = new SmartNumber("Superstructure/Turret/Gains/kA", 0.0); } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 9d355df2..bf054fb8 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,9 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; - import com.ctre.phoenix6.CANBus; import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.stuylib.network.SmartBoolean; @@ -21,6 +18,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.util.Color; @@ -224,6 +223,10 @@ public interface Angles { } public interface Turret { + //Set these arbitarily high such that the motion magic acts like a pure PID control + final double MAX_VELOCITY_DEG_PER_SEC = 600.0; + final double MAX_ACCEL_DEG_PER_SEC_SQ = 1000.0; + public final Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0)); public final Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); public final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index 412d59ea..164deacc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -5,17 +5,26 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.turret; +import java.util.Optional; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; +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 com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.stuypulse.robot.Robot; -import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.Robot.RobotMode; +import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.DriverConstants; import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.EnergyUtil; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.SysId; import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; @@ -28,17 +37,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.PositionVoltage; -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 com.ctre.phoenix6.signals.StaticFeedforwardSignValue; - -import java.util.Optional; - public class TurretImpl extends Turret { private final Motors.TalonFXConfig turretConfig; private final Motors.CANCoderConfig encoder17tConfig; @@ -53,6 +51,7 @@ public class TurretImpl extends Turret { private Optional voltageOverride; private final PositionVoltage controller; + private final MotionMagicVoltage motionMagicVoltageController; private double prevActualTargetAngle; private boolean isWrapping; @@ -89,10 +88,11 @@ public TurretImpl() { Gains.Superstructure.Turret.slot1.kI.get(), Gains.Superstructure.Turret.slot1.kD.get(), 1) .withFFConstants(Gains.Superstructure.Turret.slot1.kS.get(), Gains.Superstructure.Turret.slot1.kV.get(), Gains.Superstructure.Turret.slot1.kA.get(), 1) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 1) - .withSensorToMechanismRatio(Settings.Superstructure.Turret.GEAR_RATIO_MOTOR_TO_MECH) + .withMotionProfile(Settings.Superstructure.Turret.MAX_VELOCITY_DEG_PER_SEC, + Settings.Superstructure.Turret.MAX_ACCEL_DEG_PER_SEC_SQ) + .withSoftLimits( false, false, Settings.Superstructure.Turret.SoftwareLimit.FORWARD_MAX_ROTATIONS, @@ -122,6 +122,7 @@ public TurretImpl() { prevActualTargetAngle = getTargetAngle().getDegrees(); controller = new PositionVoltage(getTargetAngle().getRotations()).withEnableFOC(true); + motionMagicVoltageController = new MotionMagicVoltage(getTargetAngle().getRotations()).withEnableFOC(true); turretMotor.getClosedLoopError().setUpdateFrequency(1000.0); @@ -260,16 +261,18 @@ public void periodicAfterScheduler() { turretMotor.setVoltage(voltageOverride.get()); } else { - double omega = CommandSwerveDrivetrain.getInstance().getChassisSpeeds().omegaRadiansPerSecond; - double omegaFF = Gains.Superstructure.Turret.kOmega.get() * omega; - // double setpointVelocityRPS = delta / (360 * Settings.DT); - // double translationFF = Gains.Superstructure.Turret.slot0.kV * setpointVelocityRPS; - - turretMotor.setControl(controller - .withPosition(prevActualTargetAngle / 360.0) - .withSlot(slot) - .withFeedForward(omegaFF /* + translationFF */) - ); + // double omega = CommandSwerveDrivetrain.getInstance().getChassisSpeeds().omegaRadiansPerSecond; + // double omegaFF = Gains.Superstructure.Turret.kOmega.get() * omega; + // // double setpointVelocityRPS = delta / (360 * Settings.DT); + // // double translationFF = Gains.Superstructure.Turret.slot0.kV * setpointVelocityRPS; + + // turretMotor.setControl(controller + // .withPosition(prevActualTargetAngle / 360.0) + // .withSlot(slot) + // .withFeedForward(omegaFF /* + translationFF */) + // ); + motionMagicVoltageController.withPosition(prevActualTargetAngle / 360.0) + .withSlot(slot); } } else { turretMotor.stopMotor();