001/************************ PROJECT ZIXI ************************/ 002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved.*/ 003/* This work is licensed under the terms of the MIT license. */ 004/**************************************************************/ 005package com.stuypulse.robot.commands.subsystems; 006 007import static org.wpilib.units.Units.Meters; 008import static org.wpilib.units.Units.Radians; 009import static org.wpilib.units.Units.RadiansPerSecond; 010import static org.wpilib.units.Units.RadiansPerSecondPerSecond; 011import static org.wpilib.units.Units.Seconds; 012 013import com.stuypulse.robot.Robot; 014import com.stuypulse.robot.constants.DriverConstants; 015import com.stuypulse.robot.subsystems.swerve.Swerve; 016import com.stuypulse.robot.subsystems.swerve.SwerveConstants; 017import com.stuypulse.robot.util.swerve.DriveInputProcessor; 018import com.stuypulse.robot.util.swerve.DriveTurnInputProcessor; 019 020import org.wpilib.command3.*; 021import org.wpilib.command3.button.CommandGamepad; 022import org.wpilib.math.filter.SlewRateLimiter; 023import org.wpilib.math.geometry.Pose2d; 024import org.wpilib.math.geometry.Rotation2d; 025import org.wpilib.math.geometry.Translation2d; 026import org.wpilib.math.kinematics.ChassisVelocities; 027import org.wpilib.system.Timer; 028import org.wpilib.units.measure.AngularVelocity; 029 030import java.util.LinkedList; 031import java.util.List; 032import org.littletonrobotics.junction.Logger; 033 034public interface DriveCommands { 035 public static Command resetPose(Swerve swerve, Pose2d pose) { 036 return Command.noRequirements( 037 coroutine -> { 038 swerve.resetPose(pose); 039 }) 040 .named("Swerve ResetPose"); 041 } 042 043 public static Command resetHeading(Swerve swerve, Rotation2d heading) { 044 return Command.noRequirements( 045 coroutine -> { 046 swerve.resetRotation(heading); 047 }) 048 .named("Swerve Reset Heading"); 049 } 050 051 public static Command xMode(Swerve swerve) { 052 return swerve.run(coroutine -> swerve.stopWithX()).named("Swerve X Mode"); 053 } 054 055 /** 056 * Field relative drive command using two joysticks (controlling linear and angular velocities). 057 */ 058 public static Command joystickDrive(Swerve swerve, CommandGamepad driver) { 059 DriveInputProcessor driveInputProcessor = 060 new DriveInputProcessor( 061 driver, 062 DriverConstants.Driver.Drive.DEADBAND, 063 DriverConstants.Driver.Drive.POWER, 064 DriverConstants.DriveConstraints.MAX_VELOCITY, 065 DriverConstants.DriveConstraints.MAX_ACCEL, 066 DriverConstants.Driver.Drive.RC); 067 DriveTurnInputProcessor driveTurnInputProcessor = 068 new DriveTurnInputProcessor( 069 driver, 070 DriverConstants.Driver.Turn.DEADBAND, 071 DriverConstants.Driver.Turn.POWER, 072 DriverConstants.DriveConstraints.MAX_ANGULAR_VEL, 073 DriverConstants.Driver.Turn.RC); 074 075 return swerve.runRepeatedly( 076 () -> { 077 driveInputProcessor.update(); 078 driveTurnInputProcessor.update(); 079 // Get linear velocity 080 Translation2d linearVelocity = driveInputProcessor.get(); 081 082 // Get angular velocity 083 AngularVelocity angularVelocity = driveTurnInputProcessor.get(); 084 085 // Convert to field relative speeds & send command 086 ChassisVelocities speeds = 087 new ChassisVelocities( 088 linearVelocity.getX(), 089 linearVelocity.getY(), 090 angularVelocity.in(RadiansPerSecond)); 091 swerve.runVelocity( 092 speeds.toRobotRelative( 093 !Robot.isBlue() 094 ? swerve.getRotation() 095 .plus(new Rotation2d(Math.PI)) 096 : swerve.getRotation())); 097 }) 098 .named("Joystick Drive"); 099 } 100 101 /** 102 * Measures the velocity feedforward constants for the drive motors. 103 * 104 * <p>This command should only be used in voltage control mode. 105 */ 106 public static Command feedforwardCharacterization(Swerve drive) { 107 List<Double> velocitySamples = new LinkedList<>(); 108 List<Double> voltageSamples = new LinkedList<>(); 109 Timer timer = new Timer(); 110 111 Command resetData = 112 Command.noRequirements( 113 coroutine -> { 114 velocitySamples.clear(); 115 voltageSamples.clear(); 116 }) 117 .named("Reset data"); 118 119 Command orientModules = 120 drive.runRepeatedly(() -> drive.runCharacterization(0.0)) 121 .named("Orient modules") 122 .withTimeout(SwerveConstants.Characterization.FF_START_DELAY); 123 124 Command startTimer = 125 Command.noRequirements(coroutine -> timer.restart()).named("Start timer"); 126 127 Command accelerateAndGatherData = 128 drive.runRepeatedly( 129 () -> { 130 double voltage = 131 timer.get() 132 * SwerveConstants.Characterization.FF_RAMP_RATE; 133 drive.runCharacterization(voltage); 134 velocitySamples.add(drive.getFFCharacterizationVelocity()); 135 voltageSamples.add(voltage); 136 }) 137 .whenCanceled( 138 () -> { 139 int n = velocitySamples.size(); 140 double sumX = 0.0; 141 double sumY = 0.0; 142 double sumXY = 0.0; 143 double sumX2 = 0.0; 144 for (int i = 0; i < n; i++) { 145 sumX += velocitySamples.get(i); 146 sumY += voltageSamples.get(i); 147 sumXY += velocitySamples.get(i) * voltageSamples.get(i); 148 sumX2 += velocitySamples.get(i) * velocitySamples.get(i); 149 } 150 double kS = 151 (sumY * sumX2 - sumX * sumXY) 152 / (n * sumX2 - sumX * sumX); 153 double kV = 154 (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); 155 156 Logger.recordOutput("Swerve/FF Char/kS", kS); 157 Logger.recordOutput("Swerve/FF Char/kV", kV); 158 }) 159 .named("Accelerate and gather data"); 160 161 return resetData 162 .andThen(orientModules) 163 .andThen(startTimer) 164 .andThen(accelerateAndGatherData) 165 .named("Swerve FF Characterization"); 166 } 167 168 /** Measures the robot's wheel radius by spinning in a circle. */ 169 public static Command wheelRadiusCharacterization(Swerve drive) { 170 SlewRateLimiter limiter = 171 new SlewRateLimiter( 172 SwerveConstants.Characterization.WHEEL_RADIUS_RAMP_RATE.in( 173 RadiansPerSecondPerSecond)); 174 WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); 175 176 return Command.parallel( 177 // Drive control sequence 178 Command.sequence( 179 // Reset acceleration limiter 180 Command.noRequirements( 181 coroutine -> { 182 limiter.reset(0.0); 183 }) 184 .named("Wheel Radius Char Reset Limiter"), 185 186 // Turn in place, accelerating up to full speed 187 drive.runRepeatedly( 188 () -> { 189 double speed = 190 limiter.calculate( 191 SwerveConstants 192 .Characterization 193 .WHEEL_RADIUS_MAX_VELOCITY 194 .in( 195 RadiansPerSecond)); 196 drive.runVelocity( 197 new ChassisVelocities( 198 0.0, 0.0, speed)); 199 }) 200 .named("Wheel Radius Char Accelerate")) 201 .named("Wheel Radius Char Drive Control Sequence"), 202 203 // Measurement sequence 204 Command.sequence( 205 // Wait for modules to fully orient before starting 206 // measurement 207 Command.waitFor(Seconds.of(1.0)) 208 .named("Wheel Radius Char Wait For Measurement"), 209 210 // Record starting measurement 211 Command.noRequirements( 212 coroutine -> { 213 state.positions = 214 drive 215 .getWheelRadiusCharacterizationPositions(); 216 state.lastAngle = drive.getRotation(); 217 state.gyroDelta = 0.0; 218 }) 219 .named( 220 "Wheel Radius Char Record Starting Measurement"), 221 222 // Update gyro delta 223 Command.noRequirements( 224 coroutine -> { 225 var rotation = drive.getRotation(); 226 state.gyroDelta += 227 Math.abs( 228 rotation.minus( 229 state.lastAngle) 230 .getRadians()); 231 state.lastAngle = rotation; 232 }) 233 234 // When cancelled, calculate and print results 235 .whenCanceled( 236 () -> { 237 double[] positions = 238 drive 239 .getWheelRadiusCharacterizationPositions(); 240 double wheelDelta = 0.0; 241 for (int i = 0; i < 4; i++) { 242 wheelDelta += 243 Math.abs( 244 positions[i] 245 - state.positions[ 246 i]) 247 / 4.0; 248 } 249 double wheelRadius = 250 (state.gyroDelta 251 * Swerve 252 .DRIVE_BASE_RADIUS) 253 / wheelDelta; 254 255 Logger.recordOutput( 256 "Swerve/Wheel Radius Char/Delta", 257 Radians.of(wheelDelta)); 258 Logger.recordOutput( 259 "Swerve/Wheel Radius Char/Gyro Delta", 260 Radians.of(state.gyroDelta)); 261 Logger.recordOutput( 262 "Swerve/Wheel Radius Char/Wheel Radius", 263 Meters.of(wheelRadius)); 264 }) 265 .named("Wheel Radius Char Measure")) 266 .named("Wheel Radius Char Measurement Sequence")) 267 .named("Wheel Radius Char"); 268 } 269 270 class WheelRadiusCharacterizationState { 271 double[] positions = new double[4]; 272 Rotation2d lastAngle = new Rotation2d(); 273 double gyroDelta = 0.0; 274 } 275}