From 87b0a3c8af0c9392d23e106e25ce3ddc89965e26 Mon Sep 17 00:00:00 2001 From: mark-teskey Date: Mon, 30 Mar 2026 20:06:10 -0500 Subject: [PATCH 1/3] Changed to MotionMagic for the intake --- .../team1816/season/subsystems/Intake.java | 46 ++----------------- src/main/resources/yaml/ztldr.yml | 12 +++-- 2 files changed, 12 insertions(+), 46 deletions(-) diff --git a/src/main/java/com/team1816/season/subsystems/Intake.java b/src/main/java/com/team1816/season/subsystems/Intake.java index 84ba7d43..cbfda85e 100644 --- a/src/main/java/com/team1816/season/subsystems/Intake.java +++ b/src/main/java/com/team1816/season/subsystems/Intake.java @@ -1,7 +1,7 @@ package com.team1816.season.subsystems; import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; import com.team1816.lib.hardware.components.motor.IMotor; import com.team1816.lib.subsystems.ITestableSubsystem; import com.team1816.lib.util.GreenLogger; @@ -28,18 +28,10 @@ public class Intake extends SubsystemBase implements ITestableSubsystem { private final IMotor flipperMotor = (IMotor) factory.getDevice(NAME, "flipperMotor"); private final DutyCycleOut dutyCycleOut = new DutyCycleOut(0); - private final TorqueCurrentFOC flipperMotorTorqueCurrentRequest = new TorqueCurrentFOC(0); + private final MotionMagicExpoVoltage flipperMotorPositionRequest = new MotionMagicExpoVoltage(0); - /** - * The minimum position of the flipper motor to count it as far enough out to switch to our - * lower current to just hold it out instead of pushing it out. - */ private final double FLIPPER_MOTOR_OUT_POSITION; private final double FLIPPER_MOTOR_IN_POSITION; - private final double FLIPPER_MOTOR_RETRACT_CURRENT_AMPERES; - private final double FLIPPER_MOTOR_EXTEND_CURRENT_AMPERES; - private final double FLIPPER_MOTOR_HOLD_OUT_CURRENT_AMPERES; - private final double FLIPPER_MOTOR_HOLD_IN_CURRENT_AMPERES; //MECHANISMS private double currentPosition = 0; @@ -54,10 +46,6 @@ public Intake () { SmartDashboard.putData("Intake", intakeMech); FLIPPER_MOTOR_OUT_POSITION = factory.getConstant(NAME, "flipperMotorOutPosition", 0); FLIPPER_MOTOR_IN_POSITION = factory.getConstant(NAME, "flipperMotorInPosition", 0); - FLIPPER_MOTOR_RETRACT_CURRENT_AMPERES = factory.getConstant(NAME, "flipperMotorRetractCurrentAmperes", 0); - FLIPPER_MOTOR_EXTEND_CURRENT_AMPERES = factory.getConstant(NAME, "flipperMotorExtendCurrentAmperes", 0); - FLIPPER_MOTOR_HOLD_OUT_CURRENT_AMPERES = factory.getConstant(NAME, "flipperMotorHoldOutCurrentAmperes", 0); - FLIPPER_MOTOR_HOLD_IN_CURRENT_AMPERES = factory.getConstant(NAME, "flipperMotorHoldInCurrentAmperes", 0); GreenLogger.periodicLog(NAME + "/Wanted State", () -> wantedState); GreenLogger.periodicLog(NAME + "/Wanted Flipper Position", () -> wantedFlipperPosition); @@ -91,30 +79,8 @@ private void setIntakeSpeed(double velocity) { private void setFlipperPosition(FlipperPosition position) { switch (position) { - case IN -> { - if (flipperMotor.getMotorPosition() > FLIPPER_MOTOR_IN_POSITION) { - flipperMotor.setControl( - flipperMotorTorqueCurrentRequest.withOutput(FLIPPER_MOTOR_RETRACT_CURRENT_AMPERES) - ); - } - else { - flipperMotor.setControl( - flipperMotorTorqueCurrentRequest.withOutput(FLIPPER_MOTOR_HOLD_IN_CURRENT_AMPERES) - ); - } - } - case OUT -> { - if (flipperMotor.getMotorPosition() < FLIPPER_MOTOR_OUT_POSITION) { - flipperMotor.setControl( - flipperMotorTorqueCurrentRequest.withOutput(FLIPPER_MOTOR_EXTEND_CURRENT_AMPERES) - ); - } - else { - flipperMotor.setControl( - flipperMotorTorqueCurrentRequest.withOutput(FLIPPER_MOTOR_HOLD_OUT_CURRENT_AMPERES) - ); - } - } + case IN -> flipperMotor.setControl(flipperMotorPositionRequest.withPosition(FLIPPER_MOTOR_IN_POSITION)); + case OUT -> flipperMotor.setControl(flipperMotorPositionRequest.withPosition(FLIPPER_MOTOR_OUT_POSITION)); } } @@ -122,10 +88,6 @@ public void setWantedState(IntakeState state) { this.wantedState = state; } - /** - * The position of the flipper. This is not just a number, since we are doing a custom current - * based control, so the behavior is completely different for going in and out. - */ public enum FlipperPosition { IN, OUT diff --git a/src/main/resources/yaml/ztldr.yml b/src/main/resources/yaml/ztldr.yml index 6e488b13..67711392 100644 --- a/src/main/resources/yaml/ztldr.yml +++ b/src/main/resources/yaml/ztldr.yml @@ -191,6 +191,14 @@ subsystems: remoteSensorId: 10 remoteOffest: 0 rotorToSensor: 0.1 + pidConfig: + slot0: + kS: 0.3 # static friction - increase if stalling near setpoint + kP: 50 # position error gain - increase if not reaching target, decrease if oscillating + staticFeedforwardSign: UseClosedLoopSign + motionMagic: + expoKV: 2 # cruise speed - lower = faster, higher = slower + expoKA: 0.5 # acceleration smoothness - higher = gentler ramp, lower = snappier flipCoder: id: 10 deviceType: CANcoder @@ -202,10 +210,6 @@ subsystems: flipperMotorPullInOnePosition: 0.1 flipperMotorPullInTwoPosition: 0.05 flipperMotorInPosition: 0.025 - flipperMotorRetractCurrentAmperes: -25 - flipperMotorExtendCurrentAmperes: 25 - flipperMotorHoldOutCurrentAmperes: 8 - flipperMotorHoldInCurrentAmperes: -8 gatekeeper: canBusName: shootercanivore devices: From 9ba90fbea74ab05ab348d3620bc92e2d8b180b5f Mon Sep 17 00:00:00 2001 From: mark-teskey Date: Mon, 30 Mar 2026 21:57:13 -0500 Subject: [PATCH 2/3] Motion magic on the turret --- .../java/com/team1816/season/subsystems/Shooter.java | 2 +- src/main/resources/yaml/ztldr.yml | 9 ++++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index 6e88318b..3ce41f21 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -97,7 +97,7 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem { private final VelocityVoltage bottomLaunchMotorVelocityRequest = new VelocityVoltage(0); private final NeutralOut neutralModeRequest = new NeutralOut(); private final MotionMagicExpoVoltage inclineMotorPositionRequest = new MotionMagicExpoVoltage(0); - private final PositionVoltage turretMotorPositionRequest = new PositionVoltage(0); + private final MotionMagicExpoVoltage turretMotorPositionRequest = new MotionMagicExpoVoltage(0); //DEVICES private final DigitalInput leftTurretSensor = new DigitalInput((int) factory.getConstant(NAME, "leftTurretSensorChannel", 0)); diff --git a/src/main/resources/yaml/ztldr.yml b/src/main/resources/yaml/ztldr.yml index 67711392..572e7145 100644 --- a/src/main/resources/yaml/ztldr.yml +++ b/src/main/resources/yaml/ztldr.yml @@ -271,10 +271,13 @@ subsystems: motorRotation: Clockwise_Positive pidConfig: slot0: - kS: 0.29 - kP: 1 - kD: 0.05 + kS: 0.29 # static friction - increase if turret won't move at small errors + kP: 3 # position error gain - increase if still lagging, decrease if oscillating at setpoint + kD: 0.15 # damping - increase if oscillating, keep roughly kP/20 staticFeedforwardSign: UseClosedLoopSign + motionMagic: + expoKV: 0.12 # cruise speed - lower = faster moves, higher = slower. Start here, decrease if turret feels sluggish + expoKA: 0.3 # acceleration smoothness - increase if dead zone reversals are too violent, decrease if turret feels mushy inclineCoder: id: 29 deviceType: CANcoder From b55b583e7e86938ac45e2b6bdab05897bb053da3 Mon Sep 17 00:00:00 2001 From: mark-teskey Date: Tue, 31 Mar 2026 18:05:33 -0500 Subject: [PATCH 3/3] Intake and Turret Tuning --- .../hardware/components/motor/TalonFXImpl.java | 8 ++++---- src/main/resources/yaml/ztldr.yml | 16 ++++++++-------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/com/team1816/lib/hardware/components/motor/TalonFXImpl.java b/src/main/java/com/team1816/lib/hardware/components/motor/TalonFXImpl.java index 1e2a213f..8b9026dd 100644 --- a/src/main/java/com/team1816/lib/hardware/components/motor/TalonFXImpl.java +++ b/src/main/java/com/team1816/lib/hardware/components/motor/TalonFXImpl.java @@ -49,19 +49,19 @@ public double getDeviceError() { @Override public double getDeviceReference() { - return getClosedLoopReference().getValueAsDouble(); + return getClosedLoopReference(false).getValueAsDouble(); } @Override public double getMotorVelocity() { - return getVelocity().getValueAsDouble(); + return getVelocity(false).getValueAsDouble(); } @Override - public double getMotorPosition() { return getPosition().getValueAsDouble(); } + public double getMotorPosition() { return getPosition(false).getValueAsDouble(); } @Override - public boolean hasDeviceCrashed() { return getStickyFault_BootDuringEnable().getValue(); } + public boolean hasDeviceCrashed() { return getStickyFault_BootDuringEnable(false).getValue(); } @Override public void zeroMotorPosition() { diff --git a/src/main/resources/yaml/ztldr.yml b/src/main/resources/yaml/ztldr.yml index 572e7145..30b4357c 100644 --- a/src/main/resources/yaml/ztldr.yml +++ b/src/main/resources/yaml/ztldr.yml @@ -197,8 +197,8 @@ subsystems: kP: 50 # position error gain - increase if not reaching target, decrease if oscillating staticFeedforwardSign: UseClosedLoopSign motionMagic: - expoKV: 2 # cruise speed - lower = faster, higher = slower - expoKA: 0.5 # acceleration smoothness - higher = gentler ramp, lower = snappier + expoKV: 10 # cruise speed - lower = faster, higher = slower + expoKA: 5.0 # acceleration smoothness - higher = gentler ramp, lower = snappier flipCoder: id: 10 deviceType: CANcoder @@ -206,7 +206,7 @@ subsystems: intakeOnSpeed: .4 intakeSlowSpeed: .1 outtakeSpeed: -0.6 - flipperMotorOutPosition: 0.166 + flipperMotorOutPosition: 0.135 #0.166 flipperMotorPullInOnePosition: 0.1 flipperMotorPullInTwoPosition: 0.05 flipperMotorInPosition: 0.025 @@ -271,13 +271,13 @@ subsystems: motorRotation: Clockwise_Positive pidConfig: slot0: - kS: 0.29 # static friction - increase if turret won't move at small errors - kP: 3 # position error gain - increase if still lagging, decrease if oscillating at setpoint - kD: 0.15 # damping - increase if oscillating, keep roughly kP/20 + kS: 0.1 # static friction - increase if turret won't move at small errors + kP: 4 # position error gain - increase if still lagging, decrease if oscillating at setpoint + kD: 0.28 # damping - increase if oscillating, keep roughly kP/20 staticFeedforwardSign: UseClosedLoopSign motionMagic: - expoKV: 0.12 # cruise speed - lower = faster moves, higher = slower. Start here, decrease if turret feels sluggish - expoKA: 0.3 # acceleration smoothness - increase if dead zone reversals are too violent, decrease if turret feels mushy + expoKV: 0.04 #0.10 # cruise speed - lower = faster moves, higher = slower. Start here, decrease if turret feels sluggish + expoKA: 0.1 #0.2 # acceleration smoothness - increase if dead zone reversals are too violent, decrease if turret feels mushy inclineCoder: id: 29 deviceType: CANcoder