From f865594f7efefe7b645aba822bb72c30ad29eba6 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 30 Jan 2026 13:05:59 -0800 Subject: [PATCH] separate viscous friction from back-emf in motor config --- .../frc2025/CalgamesArm/CalgamesMech.java | 10 +- .../org/team100/frc2025/Climber/Climber.java | 3 +- .../frc2025/Climber/ClimberIntake.java | 3 +- .../org/team100/frc2025/grip/Manipulator.java | 8 +- .../team100/lib/config/Feedforward100.java | 200 ++++-------------- .../java/org/team100/lib/config/Friction.java | 61 ++++++ .../examples/motion/OpenLoopSubsystem.java | 2 +- .../OutboardRotaryPositionSubsystem.java | 3 +- .../motion/RotaryPositionSubsystem1d.java | 2 +- .../team100/lib/motor/ctre/Falcon6Motor.java | 36 ++++ .../team100/lib/motor/ctre/Kraken6Motor.java | 28 ++- .../lib/motor/rev/Neo550CANSparkMotor.java | 8 + .../lib/motor/rev/NeoCANSparkMotor.java | 8 + .../lib/motor/rev/NeoVortexCANSparkMotor.java | 8 + .../mecanum/MecanumDriveFactory.java | 2 +- .../shooter/DrumShooterFactory.java | 2 +- .../subsystems/shooter/PivotSubsystem.java | 3 +- .../swerve/module/WCPSwerveModule100.java | 6 +- .../lib/subsystems/tank/TankDriveFactory.java | 2 +- .../team100/lib/config/FeedforwardTest.java | 12 +- .../lib/mechanism/LinearMechanismTest.java | 4 +- .../lib/mechanism/RotaryMechanismTest.java | 6 +- .../CombinedRotaryPositionSensorTest.java | 2 +- .../servo/AnglePositionServoProfileTest.java | 2 +- .../lib/servo/AngularPositionProfileTest.java | 2 +- .../team100/lib/servo/GravityServoTest.java | 2 +- .../lib/servo/LinearVelocityServoTest.java | 2 +- .../OnboardAngularPositionServoTest.java | 2 +- .../OutboardAngularPositionServoTest.java | 2 +- 29 files changed, 229 insertions(+), 202 deletions(-) create mode 100644 lib/src/main/java/org/team100/lib/config/Friction.java diff --git a/lib/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java b/lib/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java index 1ac75c2..6cfd8a9 100644 --- a/lib/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java +++ b/lib/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java @@ -9,7 +9,6 @@ import org.team100.lib.commands.MoveAndHold; import org.team100.lib.config.ElevatorUtil.ScoringLevel; -import org.team100.lib.config.Feedforward100; import org.team100.lib.config.Identity; import org.team100.lib.config.PIDConstants; import org.team100.lib.geometry.AccelerationSE2; @@ -29,6 +28,7 @@ import org.team100.lib.mechanism.RotaryMechanism; import org.team100.lib.motor.MotorPhase; import org.team100.lib.motor.NeutralMode; +import org.team100.lib.motor.ctre.Falcon6Motor; import org.team100.lib.motor.ctre.Kraken6Motor; import org.team100.lib.motor.sim.SimulatedBareMotor; import org.team100.lib.music.Music; @@ -170,7 +170,7 @@ public CalgamesMech( 100, 100, PIDConstants.makePositionPID(elevatorfrontLog, 5), - Feedforward100.makeWCPSwerveTurningFalcon6(elevatorfrontLog)); + Falcon6Motor.swerveSteerFF(elevatorfrontLog)); IncrementalBareEncoder elevatorFrontEncoder = elevatorFrontMotor.encoder(); m_elevatorFront = new LinearMechanism( @@ -185,7 +185,7 @@ public CalgamesMech( 100, // orginally 60 100, // originally 90 PIDConstants.makePositionPID(elevatorbackLog, 5), - Feedforward100.makeWCPSwerveTurningFalcon6(elevatorbackLog)); + Falcon6Motor.swerveSteerFF(elevatorbackLog)); Talon6Encoder elevatorBackEncoder = elevatorBackMotor.encoder(); m_elevatorBack = new LinearMechanism( elevatorbackLog, elevatorBackMotor, elevatorBackEncoder, @@ -200,7 +200,7 @@ public CalgamesMech( 100, // og 60 100, // og 90 PIDConstants.makePositionPID(shoulderLog, 5), - Feedforward100.makeWCPSwerveTurningFalcon6(shoulderLog)); + Falcon6Motor.swerveSteerFF(shoulderLog)); Talon6Encoder shoulderEncoder = shoulderMotor.encoder(); // The shoulder has a 5048 on the intermediate shaft AS5048RotaryPositionSensor shoulderSensor = new AS5048RotaryPositionSensor( @@ -230,7 +230,7 @@ public CalgamesMech( 40, // og 60 60, // og 90 PIDConstants.makePositionPID(wristLog, 8), // og 10 - Feedforward100.makeWCPSwerveTurningFalcon6(wristLog)); + Falcon6Motor.swerveSteerFF(wristLog)); // the wrist has no angle sensor, so it needs to start in the "zero" position. Talon6Encoder wristEncoder = wristMotor.encoder(); final double wristGearRatio = 55.710; diff --git a/lib/src/main/java/org/team100/frc2025/Climber/Climber.java b/lib/src/main/java/org/team100/frc2025/Climber/Climber.java index 29b2427..c383ded 100644 --- a/lib/src/main/java/org/team100/frc2025/Climber/Climber.java +++ b/lib/src/main/java/org/team100/frc2025/Climber/Climber.java @@ -2,7 +2,6 @@ import java.util.function.DoubleSupplier; -import org.team100.lib.config.Feedforward100; import org.team100.lib.config.Identity; import org.team100.lib.config.PIDConstants; import org.team100.lib.controller.r1.PIDFeedback; @@ -45,7 +44,7 @@ public Climber(LoggerFactory parent, CanId canID) { Falcon6Motor motor = new Falcon6Motor(log, canID, NeutralMode.BRAKE, MotorPhase.REVERSE, 20, 20, PIDConstants.makePositionPID(log, 1), - Feedforward100.makeArmPivot(log)); + Falcon6Motor.ff2(log)); double inputOffset = 0.440602; RotaryPositionSensor sensor = new AS5048RotaryPositionSensor( diff --git a/lib/src/main/java/org/team100/frc2025/Climber/ClimberIntake.java b/lib/src/main/java/org/team100/frc2025/Climber/ClimberIntake.java index fcfbb4f..8313e58 100644 --- a/lib/src/main/java/org/team100/frc2025/Climber/ClimberIntake.java +++ b/lib/src/main/java/org/team100/frc2025/Climber/ClimberIntake.java @@ -1,6 +1,5 @@ package org.team100.frc2025.Climber; -import org.team100.lib.config.Feedforward100; import org.team100.lib.config.Identity; import org.team100.lib.config.PIDConstants; import org.team100.lib.logging.LoggerFactory; @@ -30,7 +29,7 @@ public ClimberIntake(LoggerFactory parent, CanId canID) { 20, // og 50 20, // og 2 PIDConstants.zero(log), - Feedforward100.makeKrakenClimberIntake(log)); + Kraken6Motor.highFrictionFF(log)); } default -> { m_motor = new LazySimulatedBareMotor( diff --git a/lib/src/main/java/org/team100/frc2025/grip/Manipulator.java b/lib/src/main/java/org/team100/frc2025/grip/Manipulator.java index 0cd76f6..84910ea 100644 --- a/lib/src/main/java/org/team100/frc2025/grip/Manipulator.java +++ b/lib/src/main/java/org/team100/frc2025/grip/Manipulator.java @@ -2,7 +2,6 @@ import java.util.List; -import org.team100.lib.config.Feedforward100; import org.team100.lib.config.Identity; import org.team100.lib.config.PIDConstants; import org.team100.lib.logging.Level; @@ -12,6 +11,7 @@ import org.team100.lib.motor.BareMotor; import org.team100.lib.motor.MotorPhase; import org.team100.lib.motor.NeutralMode; +import org.team100.lib.motor.ctre.Falcon6Motor; import org.team100.lib.motor.ctre.Kraken6Motor; import org.team100.lib.motor.sim.LazySimulatedBareMotor; import org.team100.lib.motor.sim.SimulatedBareMotor; @@ -56,19 +56,19 @@ public Manipulator(LoggerFactory parent) { 40, // og 40 40, // og 40 PIDConstants.zero(leftMotorLog), - Feedforward100.makeShooterFalcon6(leftMotorLog)); + Falcon6Motor.ff(leftMotorLog)); Kraken6Motor rightMotor = new Kraken6Motor(rightMotorLog, new CanId(20), NeutralMode.COAST, MotorPhase.REVERSE, 40, // og 40 40, // og 40 PIDConstants.zero(rightMotorLog), - Feedforward100.makeShooterFalcon6(rightMotorLog)); + Falcon6Motor.ff(rightMotorLog)); Kraken6Motor algaeMotor = new Kraken6Motor(algaeMotorLog, new CanId(21), NeutralMode.COAST, MotorPhase.FORWARD, 120, // og 120 120, // og 120 PIDConstants.zero(algaeMotorLog), - Feedforward100.makeShooterFalcon6(algaeMotorLog)); + Falcon6Motor.ff(algaeMotorLog)); algaeMotor.setTorqueLimit(4); m_algaeMotor = algaeMotor; m_rightLaser = new LaserCan100(new CanId(17)); diff --git a/lib/src/main/java/org/team100/lib/config/Feedforward100.java b/lib/src/main/java/org/team100/lib/config/Feedforward100.java index 6cc85a2..1887376 100644 --- a/lib/src/main/java/org/team100/lib/config/Feedforward100.java +++ b/lib/src/main/java/org/team100/lib/config/Feedforward100.java @@ -1,33 +1,10 @@ package org.team100.lib.config; -import java.util.ArrayList; -import java.util.List; - import org.team100.lib.logging.LoggerFactory; import org.team100.lib.tuning.Mutable; /** - * The motor feedforward model includes four constants. - * - * @param kV Velocity: ratio between unloaded speed (rev/s) and voltage, so the - * units are VOLT-SEC/REV. This reflects the "Back EMF" aspect of a - * motor: it produces a voltage proportional to speed. The value is - * an intrinsic property of the motor. - * @param kA Acceleration: ratio between acceleration (rev/s^2) and voltage, so - * the units are VOLT-SEC^2/REV. This reflects torque production in - * the motor: torque is proportional to current, which is - * proportional to (net) voltage. The value will depend on the - * inertia of the mechanism. - * @param kD Deceleration: like kA but when the motor is braking, i.e. - * acceleration is opposite to the current speed. Motors seem to - * decelerate much better than they accelerate -- perhaps the current - * limit acts differently (or not at all)? - * @param kSS Static friction: voltage to get the motor moving from a stop, so - * the units are VOLTS. The value will depend on the "stickiness" of - * the mechanism. - * @param kDS Dynamic friction: voltage to just barely keep the motor moving, - * the units are VOLTS. The value will depend on the "viscosity" of - * the mechanism. + * Feedforward model with three constants, and friction. * * @see https://en.wikipedia.org/wiki/Motor_constants * @see {@link edu.wpi.first.math.controller.SimpleMotorFeedforward} which uses @@ -36,144 +13,52 @@ * models. */ public class Feedforward100 { - private final List m_listeners = new ArrayList<>(); - private final Mutable kV; private final Mutable kA; private final Mutable kD; - private final Mutable kSS; - private final Mutable kDS; - private final double staticFrictionSpeedLimit; + private final Friction friction; - Feedforward100( + /** + * @param kV Back-EMF. Voltage to maintain speed against the back-EMF of + * the motor. V = kV * omega, so kV units are volt-sec/rev. + * The value is an intrinsic property of the motor. + * @param kA Acceleration. Voltage to produce acceleration of the motor + * shaft. V = kA * alpha, so kA units are volt-sec^2/rev. + * This reflects the motor torque and mechanism inertia. Torque + * is proportional to current, which is proportional to (net) + * voltage. The value will depend on the inertia of the + * mechanism. + * @param kD Deceleration. like kA but when the motor is braking, i.e. + * acceleration is opposite to the current speed. Motors + * typically decelerate ("plugging") much better than they + * accelerate ("motoring"). + * @param friction Models static, dynamic, and viscous friction. + */ + public Feedforward100( LoggerFactory log, double kV, double kA, double kD, - double kSS, - double kDS, - double staticFrictionSpeedLimit) { - this.kV = new Mutable(log, "kV", kV, this::onChange); - this.kA = new Mutable(log, "kA", kA, this::onChange); - this.kD = new Mutable(log, "kD", kD, this::onChange); - this.kSS = new Mutable(log, "kSS", kSS, this::onChange); - this.kDS = new Mutable(log, "kDS", kDS, this::onChange); - this.staticFrictionSpeedLimit = staticFrictionSpeedLimit; - } - - public static Feedforward100 makeNeo(LoggerFactory log) { - return new Feedforward100(log, 0.122, 0.000, 0.000, 0.5, 0.5, 0.5); //0.122, 0.000, 0.000, 0.1, 0.065, 0.5 - } - - public static Feedforward100 makeNeo2(LoggerFactory log) { - return new Feedforward100(log, 0.122, 0.05, 0.0250, 0.100, 0.065, 0.5); - } - - public static Feedforward100 makeNeoArm(LoggerFactory log) { - return new Feedforward100(log, 0.22, 1, 1, 0, 0, 0); - } - - public static Feedforward100 makeNeo550(LoggerFactory log) { - return new Feedforward100(log, 0.12, 0, 0, 0, .07, 0); - } - - public static Feedforward100 makeArmPivot(LoggerFactory log) { - return new Feedforward100( - log, - 0.09, - 0.005, - 0.005, - 0.100, - 0.065, - 0.5); - } - - public static Feedforward100 makeClimber(LoggerFactory log) { - return new Feedforward100( - log, - 0.14, - 0.05, - 0.05, - 0.100, - 0.1, - 0.0); - } - - public static Feedforward100 zero(LoggerFactory log) { - return new Feedforward100(log, 0, 0, 0, 0, 0, 0); - } - - public static Feedforward100 makeNeoVortex(LoggerFactory log) { - return new Feedforward100(log, 0.122, 0.000, 0.000, 0.100, 0.065, 0.5); - } - - public static Feedforward100 makeWCPSwerveTurningFalcon(LoggerFactory log) { - return new Feedforward100(log, 0.110, 0.000, 0.000, 0.180, 0.010, 0.5); + Friction friction) { + LoggerFactory ffLog = log.type(this); + this.kV = new Mutable(ffLog, "kV", kV); + this.kA = new Mutable(ffLog, "kA", kA); + this.kD = new Mutable(ffLog, "kD", kD); + this.friction = friction; } /** - * 9/24/04 - * Voltage feedforward for steering motors in air. - * Tuned in air, not on carpet, so probably the velocity number is too low. + * Voltage to maintain the specified velocity. + * + * @param motorRev_S setpoint speed */ - public static Feedforward100 makeWCPSwerveTurningFalcon6(LoggerFactory log) { - return new Feedforward100(log, 0.150, 0.010, 0.010, 0.080, 0.100, 0.5); - } - - public static Feedforward100 makeKrakenTurret(LoggerFactory log) { - return new Feedforward100(log, 0.150, 0.010, 0.010, 0.080, 0.100, 0.5); - } - - public static Feedforward100 makeWCPSwerveDriveFalcon(LoggerFactory log) { - return new Feedforward100(log, 0.110, 0.000, 0.000, 0.375, 0.270, 0.5); - } - - public static Feedforward100 makeWCPSwerveDriveFalcon6(LoggerFactory log) { - return new Feedforward100(log, 0.13, 0.017, 0.017, 0, 0.26, 0.06); - } - - public static Feedforward100 makeWCPSwerveDriveKraken6(LoggerFactory log) { - return new Feedforward100(log, 0.13, 0.022, 0.007, 0.26, 0.26, 0.06); // WAS 0.115 - } - - public static Feedforward100 makeAMSwerveDriveFalcon6(LoggerFactory log) { - return new Feedforward100(log, 0.110, 0.000, 0.000, 0.180, 0.010, 0.1); - } - - public static Feedforward100 makeSimple(LoggerFactory log) { - return new Feedforward100(log, 0.100, 0.100, 0.100, 0.100, 0.100, 0.1); - } - - public static Feedforward100 makeShooterFalcon6(LoggerFactory log) { - return new Feedforward100(log, 0.110, 0.000, 0.000, 0.000, 0.900, 0.1); - } - - public static Feedforward100 makeKraken6Elevator(LoggerFactory log) { - return new Feedforward100(log, 0.135, 0.005, 0.005, 0.005, 0.005, 0.1); - // return new Feedforward100(0, 0, 0, 0, 0); - - } - - public static Feedforward100 makeKraken6Wrist(LoggerFactory log) { - return new Feedforward100(log, 0.137, 0.0085, 0.002, 0.11, 0.11, 0.1); - // return new Feedforward100(log, 0.055, 0.025, 0.015, 0.005, 0.005, 0.1); - - } - - public static Feedforward100 makeKraken6WristWithLowerKd(LoggerFactory log) { - return new Feedforward100(log, 0.14, 0.013, 0.005, 0.2, 0.23, 0.1); - // return new Feedforward100(0, 0, 0, 0, 0); - } - - public static Feedforward100 makeKrakenClimberIntake(LoggerFactory log) { - return new Feedforward100(log, 0.13, 0.022, 0.007, 0.26, 0.26, 0.06); - } - public double velocityFFVolts(double motorRev_S) { return kV.getAsDouble() * motorRev_S; } /** + * Voltage to produce the specified acceleration. + * * Uses kA when speed and accel are in the same direction. * Uses kD when speed and accel are opposite. * @@ -203,27 +88,22 @@ public double accelFFVolts(double motorRev_S, double motorRev_S_S) { } /** + * Voltage to balance friction (i.e. this has the same sign as the supplied + * speed). + * * @param motorRev_S setpoint speed */ public double frictionFFVolts(double motorRev_S) { - double direction = Math.signum(motorRev_S); - if (Math.abs(motorRev_S) < staticFrictionSpeedLimit) { - return kSS.getAsDouble() * direction; - } - return kDS.getAsDouble() * direction; + return friction.frictionFFVolts(motorRev_S); } - public static Feedforward100 makeTest1(LoggerFactory log) { - return new Feedforward100(log, 0.3, 0.000, 0.000, 0.000, 0.100, 3.5); - } - - ///////////////////////////////////////////////////////////////////////// - - public void register(Runnable listener) { - m_listeners.add(listener); + public static Feedforward100 zero(LoggerFactory log) { + return new Feedforward100(log, 0, 0, 0, + new Friction(log, 0, 0, 0.0, 0)); } - private void onChange(double ignored) { - m_listeners.stream().forEach(r -> r.run()); + public static Feedforward100 test(LoggerFactory log) { + return new Feedforward100(log, 0.100, 0.100, 0.100, + new Friction(log, 0.100, 0.100, 0.0, 0.1)); } } diff --git a/lib/src/main/java/org/team100/lib/config/Friction.java b/lib/src/main/java/org/team100/lib/config/Friction.java new file mode 100644 index 0000000..5371175 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/config/Friction.java @@ -0,0 +1,61 @@ +package org.team100.lib.config; + +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.tuning.Mutable; + +/** + * Friction model with three constants, applicable for motor feedforward. These + * values describe the entire mechanism; the motor friction itself is usually + * negligible. + * + * TODO: make a little library of friction configs based on experiment. + * + * @param kS Static friction. Voltage to just barely get the mechanism moving + * from a stop. + * @param kD Dynamic friction. Voltage to just barely keep the mechanism moving. + * @param kV Viscous friction. Voltage to keep moving at a constant velocity. + * Volt-sec/rev. + * @param vS Velocity threshold for static friction, rev/s. + * + * @see https://mogi.bme.hu/TAMOP/robot_applications/ch07.html + * @see https://en.wikipedia.org/wiki/Friction + * @see https://en.wikipedia.org/wiki/Stribeck_curve + * @see https://engee.com/helpcenter/stable/en/fmod-mechanical-translational-elements/translational-friction.html + */ +public class Friction { + private final Mutable kS; + private final Mutable kD; + private final Mutable kV; + private final double vS; + + public Friction( + LoggerFactory log, + double kS, + double kD, + double kV, + double vS) { + if (kS < kD) + throw new IllegalArgumentException("static friction is always at least as high as dynamic friction"); + LoggerFactory fLog = log.type(this); + this.kS = new Mutable(fLog, "kS", kS); + this.kD = new Mutable(fLog, "kD", kD); + this.kV = new Mutable(fLog, "kV", kV); + this.vS = vS; + } + + /** + * Voltage to balance friction (i.e. this has the same sign as the supplied + * speed). + * + * @param motorRev_S setpoint speed + */ + public double frictionFFVolts(double motorRev_S) { + double viscous = kV.getAsDouble() * motorRev_S; + double direction = Math.signum(motorRev_S); + if (Math.abs(motorRev_S) < vS) { + return viscous + kS.getAsDouble() * direction; + } + return viscous + kD.getAsDouble() * direction; + } + +} diff --git a/lib/src/main/java/org/team100/lib/examples/motion/OpenLoopSubsystem.java b/lib/src/main/java/org/team100/lib/examples/motion/OpenLoopSubsystem.java index 01cc565..de3f430 100644 --- a/lib/src/main/java/org/team100/lib/examples/motion/OpenLoopSubsystem.java +++ b/lib/src/main/java/org/team100/lib/examples/motion/OpenLoopSubsystem.java @@ -44,7 +44,7 @@ public OpenLoopSubsystem(LoggerFactory parent) { int statorLimit = 90; PIDConstants PID = PIDConstants.makeVelocityPID(log, 0.3); // you should make a case in the feedforward class for your constants - Feedforward100 FF = Feedforward100.makeSimple(log); + Feedforward100 FF = Feedforward100.test(log); m_motor = new Falcon6Motor( log, canId, NeutralMode.COAST, MotorPhase.FORWARD, supplyLimit, statorLimit, PID, FF); } diff --git a/lib/src/main/java/org/team100/lib/examples/motion/OutboardRotaryPositionSubsystem.java b/lib/src/main/java/org/team100/lib/examples/motion/OutboardRotaryPositionSubsystem.java index 59494db..e152a00 100644 --- a/lib/src/main/java/org/team100/lib/examples/motion/OutboardRotaryPositionSubsystem.java +++ b/lib/src/main/java/org/team100/lib/examples/motion/OutboardRotaryPositionSubsystem.java @@ -1,6 +1,5 @@ package org.team100.lib.examples.motion; -import org.team100.lib.config.Feedforward100; import org.team100.lib.config.Identity; import org.team100.lib.config.PIDConstants; import org.team100.lib.logging.LoggerFactory; @@ -70,7 +69,7 @@ private RotaryMechanism mech(LoggerFactory log) { NeutralMode.BRAKE, MotorPhase.FORWARD, 10, // Stator current limit, amps - Feedforward100.makeNeo(log), + NeoCANSparkMotor.ff(log), PIDConstants.makePositionPID(log, 0.0003) // duty-cycle/RPM ); IncrementalBareEncoder encoder = motor.encoder(); diff --git a/lib/src/main/java/org/team100/lib/examples/motion/RotaryPositionSubsystem1d.java b/lib/src/main/java/org/team100/lib/examples/motion/RotaryPositionSubsystem1d.java index 208d2f7..6fb560a 100644 --- a/lib/src/main/java/org/team100/lib/examples/motion/RotaryPositionSubsystem1d.java +++ b/lib/src/main/java/org/team100/lib/examples/motion/RotaryPositionSubsystem1d.java @@ -92,7 +92,7 @@ public RotaryPositionSubsystem1d(LoggerFactory parent) { double inputOffset = 0.135541; PIDConstants PID = PIDConstants.makeVelocityPID(log, 0.3); // you should make a case in the feedforward class for your constants - Feedforward100 FF = Feedforward100.makeSimple(log); + Feedforward100 FF = Feedforward100.test(log); Kraken6Motor motor = new Kraken6Motor( log, new CanId(1), NeutralMode.COAST, MotorPhase.REVERSE, supplyLimit, statorLimit, PID, FF); RotaryPositionSensor sensor = new AS5048RotaryPositionSensor( diff --git a/lib/src/main/java/org/team100/lib/motor/ctre/Falcon6Motor.java b/lib/src/main/java/org/team100/lib/motor/ctre/Falcon6Motor.java index 2d886cd..58b987c 100644 --- a/lib/src/main/java/org/team100/lib/motor/ctre/Falcon6Motor.java +++ b/lib/src/main/java/org/team100/lib/motor/ctre/Falcon6Motor.java @@ -1,6 +1,7 @@ package org.team100.lib.motor.ctre; import org.team100.lib.config.Feedforward100; +import org.team100.lib.config.Friction; import org.team100.lib.config.PIDConstants; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.motor.MotorPhase; @@ -35,4 +36,39 @@ public double kROhms() { public double kTNm_amp() { return 0.018; } + + /** Generic feedforward with considerable friction */ + public static Feedforward100 ff(LoggerFactory log) { + // FOC free speed is 6079 RPM at 12.0 V = 0.118 v-s/rev + // there's very little friction in the direct drive + return new Feedforward100(log, 0.118, 0.000, 0.000, + new Friction(log, 0.900, 0.900, 0.000, 0.100)); + } + + /** Feedforward with less friction */ + public static Feedforward100 ff2(LoggerFactory log) { + // FOC free speed is 6079 RPM at 12.0 V = 0.118 v-s/rev + return new Feedforward100(log, 0.118, 0.005, 0.005, + new Friction(log, 0.100, 0.065, 0.0, 0.5)); + } + + /** Feedforward for swerve drive motor */ + public static Feedforward100 swerveDriveFF(LoggerFactory log) { + // FOC free speed is 6079 RPM at 12.0 V = 0.118 v-s/rev + // there's a little bit of viscous friction here. + return new Feedforward100(log, 0.118, 0.017, 0.017, + new Friction(log, 0.260, 0.260, 0.012, 0.060)); + } + + /** + * Voltage feedforward for steering motors in air. + * 9/24/04 Tuned in air, not on carpet, so friction is too low. + */ + public static Feedforward100 swerveSteerFF(LoggerFactory log) { + // FOC free speed is 6079 RPM at 12.0 V = 0.118 v-s/rev + // TODO: the friction here is probably too low + return new Feedforward100(log, 0.118, 0.010, 0.010, + new Friction(log, 0.100, 0.100, 0.032, 0.5)); + } + } diff --git a/lib/src/main/java/org/team100/lib/motor/ctre/Kraken6Motor.java b/lib/src/main/java/org/team100/lib/motor/ctre/Kraken6Motor.java index a8511db..97aaca1 100644 --- a/lib/src/main/java/org/team100/lib/motor/ctre/Kraken6Motor.java +++ b/lib/src/main/java/org/team100/lib/motor/ctre/Kraken6Motor.java @@ -1,6 +1,7 @@ package org.team100.lib.motor.ctre; import org.team100.lib.config.Feedforward100; +import org.team100.lib.config.Friction; import org.team100.lib.config.PIDConstants; import org.team100.lib.logging.LoggerFactory; import org.team100.lib.motor.MotorPhase; @@ -10,6 +11,10 @@ /** * Kraken using Phoenix 6. * + * TODO: make a separate class for the x44, since its free speed is higher. + * x60 is 5800 RPM at 12.0 V = 0.124 v-s/rev + * x44 is 7530 RPM at 12.0 V = 0.096 v-s/rev + * * @see https://store.ctr-electronics.com/content/datasheet/Motor%20Performance%20Analysis%20Report.pdf */ public class Kraken6Motor extends Talon6Motor { @@ -23,8 +28,8 @@ public Kraken6Motor( double statorLimit, PIDConstants pid, Feedforward100 ff) { - super(parent, canId, neutral, motorPhase, supplyLimit, statorLimit, pid, ff); - } + super(parent, canId, neutral, motorPhase, supplyLimit, statorLimit, pid, ff); + } @Override public double kROhms() { @@ -36,4 +41,23 @@ public double kTNm_amp() { return 0.019; } + /** Feedforward for swerve drive axis */ + public static Feedforward100 swerveDriveFF(LoggerFactory log) { + // FOC free speed is 5800 RPM at 12.0 V = 0.124 v-s/rev + // TODO: friction here is probably too low. + return new Feedforward100(log, 0.124, 0.022, 0.007, + new Friction(log, 0.26, 0.26, 0.006, 0.06)); + } + + public static Feedforward100 highFrictionFF(LoggerFactory log) { + // FOC free speed is 5800 RPM at 12.0 V = 0.124 v-s/rev + return new Feedforward100(log, 0.124, 0.022, 0.007, + new Friction(log, 0.26, 0.26, 0.006, 0.06)); + } + + public static Feedforward100 lowFrictionFF(LoggerFactory log) { + // FOC free speed is 5800 RPM at 12.0 V = 0.124 v-s/rev + return new Feedforward100(log, 0.124, 0.005, 0.005, + new Friction(log, 0.005, 0.005, 0.011, 0.1)); + } } diff --git a/lib/src/main/java/org/team100/lib/motor/rev/Neo550CANSparkMotor.java b/lib/src/main/java/org/team100/lib/motor/rev/Neo550CANSparkMotor.java index b1168a4..7cbffda 100644 --- a/lib/src/main/java/org/team100/lib/motor/rev/Neo550CANSparkMotor.java +++ b/lib/src/main/java/org/team100/lib/motor/rev/Neo550CANSparkMotor.java @@ -1,6 +1,7 @@ package org.team100.lib.motor.rev; import org.team100.lib.config.Feedforward100; +import org.team100.lib.config.Friction; import org.team100.lib.config.Identity; import org.team100.lib.config.PIDConstants; import org.team100.lib.logging.LoggerFactory; @@ -53,4 +54,11 @@ public double kROhms() { public double kTNm_amp() { return 0.009; } + + public static Feedforward100 ff(LoggerFactory log) { + // free speed is 11000 RPM at 12.0 V so kV = 0.065 + // TODO: friction? + return new Feedforward100(log, 0.065, 0, 0, + new Friction(log, 0, 0.07, 0.055, 0)); + } } diff --git a/lib/src/main/java/org/team100/lib/motor/rev/NeoCANSparkMotor.java b/lib/src/main/java/org/team100/lib/motor/rev/NeoCANSparkMotor.java index 77ef2ec..08d76c7 100644 --- a/lib/src/main/java/org/team100/lib/motor/rev/NeoCANSparkMotor.java +++ b/lib/src/main/java/org/team100/lib/motor/rev/NeoCANSparkMotor.java @@ -1,6 +1,7 @@ package org.team100.lib.motor.rev; import org.team100.lib.config.Feedforward100; +import org.team100.lib.config.Friction; import org.team100.lib.config.Identity; import org.team100.lib.config.PIDConstants; import org.team100.lib.logging.LoggerFactory; @@ -51,4 +52,11 @@ public double kROhms() { public double kTNm_amp() { return 0.028; } + + public static Feedforward100 ff(LoggerFactory log) { + // free speed is 5676 RPM at 12.0 V so kV = 0.127 + // TODO: friction + return new Feedforward100(log, 0.127, 0.050, 0.050, + new Friction(log, 0.5, 0.5, 0.0, 0.5)); + } } diff --git a/lib/src/main/java/org/team100/lib/motor/rev/NeoVortexCANSparkMotor.java b/lib/src/main/java/org/team100/lib/motor/rev/NeoVortexCANSparkMotor.java index a034ebd..c0b0ff4 100644 --- a/lib/src/main/java/org/team100/lib/motor/rev/NeoVortexCANSparkMotor.java +++ b/lib/src/main/java/org/team100/lib/motor/rev/NeoVortexCANSparkMotor.java @@ -1,6 +1,7 @@ package org.team100.lib.motor.rev; import org.team100.lib.config.Feedforward100; +import org.team100.lib.config.Friction; import org.team100.lib.config.Identity; import org.team100.lib.config.PIDConstants; import org.team100.lib.logging.LoggerFactory; @@ -58,4 +59,11 @@ public double kROhms() { public double kTNm_amp() { return 0.017; } + + public static Feedforward100 ff(LoggerFactory log) { + // free speed is 6784 RPM at 12.0 V so kV = 0.106 + // TODO: friction + return new Feedforward100(log, 0.106, 0.000, 0.000, + new Friction(log, 0.100, 0.065, 0.0, 0.5)); + } } diff --git a/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDriveFactory.java b/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDriveFactory.java index 383b3f5..c3f7f7f 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDriveFactory.java +++ b/lib/src/main/java/org/team100/lib/subsystems/mecanum/MecanumDriveFactory.java @@ -36,7 +36,7 @@ public static MecanumDrive100 make( LoggerFactory logRR = log.name("rearRight"); // all wheels need the same ff/pid values - Feedforward100 ff = Feedforward100.makeNeo(log); + Feedforward100 ff = NeoCANSparkMotor.ff(log); // 10/22/25: Anay found this value worked well // PIDConstants pid = PIDConstants.makeVelocityPID(log, 0.00005); // Doubled the value for auton diff --git a/lib/src/main/java/org/team100/lib/subsystems/shooter/DrumShooterFactory.java b/lib/src/main/java/org/team100/lib/subsystems/shooter/DrumShooterFactory.java index ffe739a..f95e040 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/shooter/DrumShooterFactory.java +++ b/lib/src/main/java/org/team100/lib/subsystems/shooter/DrumShooterFactory.java @@ -23,7 +23,7 @@ public static DualDrumShooter make( CanId canL = new CanId(39); CanId canR = new CanId(19); - Feedforward100 ff = Feedforward100.makeNeo550(log); + Feedforward100 ff = Neo550CANSparkMotor.ff(log); // This P value is a guess PIDConstants pid = PIDConstants.makeVelocityPID(log, 0.0002); diff --git a/lib/src/main/java/org/team100/lib/subsystems/shooter/PivotSubsystem.java b/lib/src/main/java/org/team100/lib/subsystems/shooter/PivotSubsystem.java index fbc4c4d..69e93aa 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/shooter/PivotSubsystem.java +++ b/lib/src/main/java/org/team100/lib/subsystems/shooter/PivotSubsystem.java @@ -1,6 +1,5 @@ package org.team100.lib.subsystems.shooter; -import org.team100.lib.config.Feedforward100; import org.team100.lib.config.PIDConstants; import org.team100.lib.logging.Level; import org.team100.lib.logging.LoggerFactory; @@ -25,7 +24,7 @@ public PivotSubsystem(LoggerFactory parent, int currentLimit) { new CanId(5), MotorPhase.FORWARD, currentLimit, - Feedforward100.makeNeo550(logger), + Neo550CANSparkMotor.ff(logger), PIDConstants.zero(logger)); } diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/module/WCPSwerveModule100.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/module/WCPSwerveModule100.java index b278dec..9176e8a 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/swerve/module/WCPSwerveModule100.java +++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/module/WCPSwerveModule100.java @@ -144,7 +144,7 @@ private static LinearVelocityServo driveKrakenServo( double statorLimit, CanId driveMotorCanId, DriveRatio ratio) { - Feedforward100 ff = Feedforward100.makeWCPSwerveDriveKraken6(parent); + Feedforward100 ff = Kraken6Motor.swerveDriveFF(parent); // note (10/2/24) 0.4 produces oscillation, on carpet. PIDConstants pid = PIDConstants.makeVelocityPID(parent, 0.3); Kraken6Motor driveMotor = new Kraken6Motor( @@ -173,7 +173,7 @@ private static LinearVelocityServo driveFalconServo( double statorLimit, CanId driveMotorCanId, DriveRatio ratio) { - Feedforward100 ff = Feedforward100.makeWCPSwerveDriveFalcon6(parent); + Feedforward100 ff = Falcon6Motor.swerveDriveFF(parent); PIDConstants pid = PIDConstants.makeVelocityPID(parent, 0.3); Falcon6Motor driveMotor = new Falcon6Motor( parent, @@ -210,7 +210,7 @@ private static AngularPositionServo turningServo( PIDConstants lowLevelPID = PIDConstants.makePositionPID(parent, 10.0); // java uses this to calculate feedforward voltages from target velocities etc - Feedforward100 ff = Feedforward100.makeWCPSwerveTurningFalcon6(parent); + Feedforward100 ff = Falcon6Motor.swerveSteerFF(parent); Falcon6Motor turningMotor = new Falcon6Motor( parent, diff --git a/lib/src/main/java/org/team100/lib/subsystems/tank/TankDriveFactory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/TankDriveFactory.java index 1318598..3c7d217 100644 --- a/lib/src/main/java/org/team100/lib/subsystems/tank/TankDriveFactory.java +++ b/lib/src/main/java/org/team100/lib/subsystems/tank/TankDriveFactory.java @@ -25,7 +25,7 @@ public static TankDrive make( LoggerFactory logR = log.name("right"); // all wheels need the same ff/pid values - Feedforward100 ff = Feedforward100.makeNeo(log); + Feedforward100 ff = NeoCANSparkMotor.ff(log); // 10/22/25: Anay found this value worked well PIDConstants pid = PIDConstants.makeVelocityPID(log, 0.00005); // 10/22/25: Lincoln used this value diff --git a/lib/src/test/java/org/team100/lib/config/FeedforwardTest.java b/lib/src/test/java/org/team100/lib/config/FeedforwardTest.java index 8e201f5..b30c34d 100644 --- a/lib/src/test/java/org/team100/lib/config/FeedforwardTest.java +++ b/lib/src/test/java/org/team100/lib/config/FeedforwardTest.java @@ -43,7 +43,9 @@ void testWPI() { @Test void test100() { // behaves the same as the naive model above, ignoring friction. - Feedforward100 ff100 = new Feedforward100(log, 1, 1, 1, 0, 0, 0); + Feedforward100 ff100 = new Feedforward100( + log, 1, 1, 1, + new Friction(log, 0, 0, 0, 0)); assertEquals(1, ff100.velocityFFVolts(1), DELTA); assertEquals(1, ff100.accelFFVolts(1, 1), DELTA); } @@ -51,7 +53,9 @@ void test100() { @Test void testkD() { // kd is lower - Feedforward100 ff100 = new Feedforward100(log, 1, 1, 0.1, 0, 0, 0); + Feedforward100 ff100 = new Feedforward100( + log, 1, 1, 0.1, + new Friction(log, 0, 0, 0, 0)); assertEquals(1, ff100.accelFFVolts(1, 1), DELTA); assertEquals(0.1, ff100.accelFFVolts(-1, 1), DELTA); } @@ -60,7 +64,9 @@ void testkD() { @Test void testFriction() { // static friction = 2, dynamic friction = 1 - Feedforward100 ff100 = new Feedforward100(log, 1, 1, 1, 2, 1, 1); + Feedforward100 ff100 = new Feedforward100( + log, 1, 1, 1, + new Friction(log, 2, 1, 0, 1)); // under the static friction limit, so this is static assertEquals(2, ff100.frictionFFVolts(0.5), DELTA); // over the static friction limit, so sliding diff --git a/lib/src/test/java/org/team100/lib/mechanism/LinearMechanismTest.java b/lib/src/test/java/org/team100/lib/mechanism/LinearMechanismTest.java index a15ebc7..41335d9 100644 --- a/lib/src/test/java/org/team100/lib/mechanism/LinearMechanismTest.java +++ b/lib/src/test/java/org/team100/lib/mechanism/LinearMechanismTest.java @@ -18,7 +18,7 @@ public class LinearMechanismTest implements Timeless { /** Show that the limits have effect. */ @Test void testLimits() { - Feedforward100 ff = Feedforward100.makeSimple(logger); + Feedforward100 ff = Feedforward100.test(logger); MockBareMotor motor = new MockBareMotor(ff); MockIncrementalBareEncoder encoder = new MockIncrementalBareEncoder(); double gearRatio = 1; @@ -58,7 +58,7 @@ void testLimits() { /** Same cases as above, but unlimited */ @Test void testUnlimited() { - Feedforward100 ff = Feedforward100.makeSimple(logger); + Feedforward100 ff = Feedforward100.test(logger); MockBareMotor motor = new MockBareMotor(ff); MockIncrementalBareEncoder encoder = new MockIncrementalBareEncoder(); double gearRatio = 1; diff --git a/lib/src/test/java/org/team100/lib/mechanism/RotaryMechanismTest.java b/lib/src/test/java/org/team100/lib/mechanism/RotaryMechanismTest.java index 8b1b403..6cabd4e 100644 --- a/lib/src/test/java/org/team100/lib/mechanism/RotaryMechanismTest.java +++ b/lib/src/test/java/org/team100/lib/mechanism/RotaryMechanismTest.java @@ -18,7 +18,7 @@ public class RotaryMechanismTest implements Timeless { /** Show that the limits have effect. */ @Test void testLimits() { - Feedforward100 ff = Feedforward100.makeSimple(logger); + Feedforward100 ff = Feedforward100.test(logger); MockBareMotor motor = new MockBareMotor(ff); MockRotaryPositionSensor sensor = new MockRotaryPositionSensor(); double gearRatio = 1; @@ -56,7 +56,7 @@ void testLimits() { /** Same cases as above, but unlimited */ @Test void testUnlimited() { - Feedforward100 ff = Feedforward100.makeSimple(logger); + Feedforward100 ff = Feedforward100.test(logger); MockBareMotor motor = new MockBareMotor(ff); MockRotaryPositionSensor sensor = new MockRotaryPositionSensor(); double gearRatio = 1; @@ -95,7 +95,7 @@ void testUnlimited() { @Test void testWrapNearMeasurement() { LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); - MockBareMotor motor = new MockBareMotor(Feedforward100.makeSimple(log)); + MockBareMotor motor = new MockBareMotor(Feedforward100.test(log)); MockRotaryPositionSensor sensor = new MockRotaryPositionSensor(); RotaryMechanism mech = new RotaryMechanism( log, motor, sensor, 1, Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); diff --git a/lib/src/test/java/org/team100/lib/sensor/position/absolute/CombinedRotaryPositionSensorTest.java b/lib/src/test/java/org/team100/lib/sensor/position/absolute/CombinedRotaryPositionSensorTest.java index d7e5f45..46a710f 100644 --- a/lib/src/test/java/org/team100/lib/sensor/position/absolute/CombinedRotaryPositionSensorTest.java +++ b/lib/src/test/java/org/team100/lib/sensor/position/absolute/CombinedRotaryPositionSensorTest.java @@ -18,7 +18,7 @@ class CombinedRotaryPositionSensorTest implements Timeless { @Test void testZeroing() { - MockBareMotor motor = new MockBareMotor(Feedforward100.makeSimple(logger)); + MockBareMotor motor = new MockBareMotor(Feedforward100.test(logger)); // this is the "correct" value MockRotaryPositionSensor sensor = new MockRotaryPositionSensor(); diff --git a/lib/src/test/java/org/team100/lib/servo/AnglePositionServoProfileTest.java b/lib/src/test/java/org/team100/lib/servo/AnglePositionServoProfileTest.java index a420e68..a1e31ff 100644 --- a/lib/src/test/java/org/team100/lib/servo/AnglePositionServoProfileTest.java +++ b/lib/src/test/java/org/team100/lib/servo/AnglePositionServoProfileTest.java @@ -31,7 +31,7 @@ class AnglePositionServoProfileTest implements Timeless { double previousMotorSpeed = 0; public AnglePositionServoProfileTest() { - motor = new MockBareMotor(Feedforward100.makeSimple(logger)); + motor = new MockBareMotor(Feedforward100.test(logger)); sensor = new MockRotaryPositionSensor(); RotaryMechanism mech = new RotaryMechanism( logger, diff --git a/lib/src/test/java/org/team100/lib/servo/AngularPositionProfileTest.java b/lib/src/test/java/org/team100/lib/servo/AngularPositionProfileTest.java index 57bf313..f90fb3d 100644 --- a/lib/src/test/java/org/team100/lib/servo/AngularPositionProfileTest.java +++ b/lib/src/test/java/org/team100/lib/servo/AngularPositionProfileTest.java @@ -32,7 +32,7 @@ class AngularPositionProfileTest implements Timeless { private OnboardAngularPositionServo servo; public AngularPositionProfileTest() { - motor = new MockBareMotor(Feedforward100.makeSimple(logger)); + motor = new MockBareMotor(Feedforward100.test(logger)); sensor = new MockRotaryPositionSensor(); mech = new RotaryMechanism( logger, motor, sensor, 1, Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); diff --git a/lib/src/test/java/org/team100/lib/servo/GravityServoTest.java b/lib/src/test/java/org/team100/lib/servo/GravityServoTest.java index 3e80277..45fde33 100644 --- a/lib/src/test/java/org/team100/lib/servo/GravityServoTest.java +++ b/lib/src/test/java/org/team100/lib/servo/GravityServoTest.java @@ -66,7 +66,7 @@ void testSetPosition() { /** For refactoring the gravity servo */ @Test void testGravity() { - MockBareMotor motor = new MockBareMotor(Feedforward100.makeSimple(logger)); + MockBareMotor motor = new MockBareMotor(Feedforward100.test(logger)); MockRotaryPositionSensor sensor = new MockRotaryPositionSensor(); RotaryMechanism mech = new RotaryMechanism( logger, motor, sensor, 1, Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); diff --git a/lib/src/test/java/org/team100/lib/servo/LinearVelocityServoTest.java b/lib/src/test/java/org/team100/lib/servo/LinearVelocityServoTest.java index adaa412..d3d0190 100644 --- a/lib/src/test/java/org/team100/lib/servo/LinearVelocityServoTest.java +++ b/lib/src/test/java/org/team100/lib/servo/LinearVelocityServoTest.java @@ -16,7 +16,7 @@ class LinearVelocityServoTest { @Test void testSimple() { - MockBareMotor driveMotor = new MockBareMotor(Feedforward100.makeSimple(logger)); + MockBareMotor driveMotor = new MockBareMotor(Feedforward100.test(logger)); MockIncrementalBareEncoder driveEncoder = new MockIncrementalBareEncoder(); LinearMechanism mech = new LinearMechanism(logger, driveMotor, driveEncoder, 1, 1, Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY); diff --git a/lib/src/test/java/org/team100/lib/servo/OnboardAngularPositionServoTest.java b/lib/src/test/java/org/team100/lib/servo/OnboardAngularPositionServoTest.java index d34a6c9..76a23f8 100644 --- a/lib/src/test/java/org/team100/lib/servo/OnboardAngularPositionServoTest.java +++ b/lib/src/test/java/org/team100/lib/servo/OnboardAngularPositionServoTest.java @@ -27,7 +27,7 @@ public class OnboardAngularPositionServoTest implements Timeless { @Test void testOnboard() { - final MockBareMotor turningMotor = new MockBareMotor(Feedforward100.makeSimple(logger)); + final MockBareMotor turningMotor = new MockBareMotor(Feedforward100.test(logger)); final MockRotaryPositionSensor positionSensor = new MockRotaryPositionSensor(); final RotaryMechanism mech = new RotaryMechanism( logger, turningMotor, positionSensor, 1, Double.NEGATIVE_INFINITY, diff --git a/lib/src/test/java/org/team100/lib/servo/OutboardAngularPositionServoTest.java b/lib/src/test/java/org/team100/lib/servo/OutboardAngularPositionServoTest.java index 08183d9..303cbe3 100644 --- a/lib/src/test/java/org/team100/lib/servo/OutboardAngularPositionServoTest.java +++ b/lib/src/test/java/org/team100/lib/servo/OutboardAngularPositionServoTest.java @@ -32,7 +32,7 @@ public class OutboardAngularPositionServoTest implements Timeless { @Test void testProfiled() { - final MockBareMotor motor = new MockBareMotor(Feedforward100.makeSimple(log)); + final MockBareMotor motor = new MockBareMotor(Feedforward100.test(log)); final MockIncrementalBareEncoder encoder = new MockIncrementalBareEncoder(); final MockRotaryPositionSensor sensor = new MockRotaryPositionSensor();