From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Wed, 18 Feb 2026 19:08:57 +0000 (-0800) Subject: clean up X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=6bc347554e17d9c34cacf25c42435f2bafd32133;p=FRC2026.git clean up --- diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index ec744f4..8338dba 100644 --- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -1,7 +1,5 @@ package frc.robot.commands.gpm; -import java.util.Optional; - import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; @@ -22,7 +20,6 @@ import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.hood.HoodConstants; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.spindexer.Spindexer; import frc.robot.subsystems.turret.ShotInterpolation; import frc.robot.subsystems.turret.Turret; @@ -80,12 +77,10 @@ public class AutoShootCommand extends Command { private TurretState goalState; - private final double phaseDelay = 0.03; + private final double phaseDelay = 0.03; // Extrapolation delay due to latency private Translation2d target = FieldConstants.getHubTranslation().toTranslation2d(); - private double velocityAdjustment = 0; - public AutoShootCommand(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) { this.turret = turret; this.drivetrain = drivetrain; @@ -104,12 +99,12 @@ public class AutoShootCommand extends Command { public void updateSetpoints(Pose2d drivepose) { Pose2d turretPosition = drivepose.transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d())); - double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); // If the robot is moving, adjust the target position based on velocity ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); + // Rotational adjustment is not being used, since turret is in center of robot double turretVelocityX = fieldRelVel.vxMetersPerSecond + fieldRelVel.omegaRadiansPerSecond @@ -123,7 +118,6 @@ public class AutoShootCommand extends Command { double timeOfFlight; Pose2d lookaheadPose = turretPosition; - //double lookaheadTurretToTargetDistance = turretToTargetDistance; /* * Loop (20) until lookaheadPose converges BECAUSE --> @@ -145,25 +139,29 @@ public class AutoShootCommand extends Command { new Pose2d( turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), turretPosition.getRotation()); - //lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); } + // Get the field angle relative to the target pose turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); if (lastTurretAngle == null) { lastTurretAngle = turretAngle; } + + // Take the filtered average as the turret's velocity when robot is moving translationally turretVelocity = - turretAngleFilter.calculate( - turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME); + turretAngleFilter.calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME); + lastTurretAngle = turretAngle; Logger.recordOutput("Lookahead Pose", lookaheadPose); + // Subtract the rotational angle of the robot from the setpoint double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians()); // Shortest path double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180); double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error; + // Stay within +/- 200 -- if shortest path is past 200, we go long way around double turretRange = TurretConstants.MAX_ANGLE - TurretConstants.MIN_ANGLE; if (potentialSetpoint > turretRange/2) { @@ -180,19 +178,19 @@ public class AutoShootCommand extends Command { hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME); lastHoodAngle = hoodAngle; - if (Math.abs(lastTurretAngle.getDegrees() - turretSetpoint) > 90) { - velocityAdjustment = -drivetrain.getAngularRate(2) * 1.0; - } } public void updateDrivePose(){ Pose2d currentPose = drivetrain.getPose(); - // Add 180 degrees to the rotation bc robot is backwards + drivepose = new Pose2d( currentPose.getTranslation(), + // Uncomment this if robot is backwards currentPose.getRotation()//.plus(new Rotation2d(Math.PI)) ); ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); + + // Add a phase delay extrapolation component for latency delay drivepose.exp( new Twist2d( robotRelVel.vxMetersPerSecond * phaseDelay, @@ -206,8 +204,8 @@ public class AutoShootCommand extends Command { updateSetpoints(drivepose); turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2)); - //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity); - //shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); + hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity); + shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint); SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint); @@ -222,10 +220,10 @@ public class AutoShootCommand extends Command { @Override public void end(boolean interrupted) { - // Set the turret to a safe position when the command ends + // Set the turret and hood to a safe position when the command ends turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0); shooter.setShooter(0.0); - //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0); + hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0); if(spindexer != null){ spindexer.stopSpindexer(); } diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java index 07581ef..d8df775 100644 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -2,8 +2,6 @@ package frc.robot.subsystems.hood; import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -11,20 +9,14 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; -import yams.gearing.GearBox; public class Hood extends SubsystemBase implements HoodIO{ private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.SUBSYSTEM_CANIVORE_CAN); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 11320ee..3fd48ac 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -10,16 +10,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import au.grapplerobotics.ConfigurationFailedException; -import au.grapplerobotics.LaserCan; -import au.grapplerobotics.interfaces.LaserCanInterface.Measurement; -import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; -import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest; -import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -33,20 +24,15 @@ public class Shooter extends SubsystemBase implements ShooterIO { // Goal Velocity / Double theCircumfrence private double shooterTargetSpeed = 0; - private double feederPower = 0; - - public boolean shooterAtMaxSpeed = false; - public boolean ballDetected = false; - //Velocity in rotations per second + // Velocity in rotations per second VelocityVoltage voltageRequest = new VelocityVoltage(0); private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - double powerModifier; + double powerModifier = 1.0; public Shooter(){ - updateInputs(); TalonFXConfiguration config = new TalonFXConfiguration(); @@ -75,12 +61,21 @@ public class Shooter extends SubsystemBase implements ShooterIO { powerModifier = SmartDashboard.getNumber("shooter power modifier", powerModifier); SmartDashboard.putNumber("shooter power modifier", powerModifier); - shooterMotorLeft.setControl(voltageRequest.withVelocity(Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)))); - shooterMotorRight.setControl(voltageRequest.withVelocity(Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)))); + + // Convert to RPS + double targetVelocityRPS = Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)); + + // Sets the motor control to target velocity + shooterMotorLeft.setControl(voltageRequest.withVelocity(targetVelocityRPS)); + shooterMotorRight.setControl(voltageRequest.withVelocity(targetVelocityRPS)); Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed); } + /** + * Sets the target speed of the shooter + * @param linearVelocityMps + */ public void setShooter(double linearVelocityMps) { shooterTargetSpeed = linearVelocityMps; } @@ -97,10 +92,16 @@ public class Shooter extends SubsystemBase implements ShooterIO { Logger.processInputs("Shooter", inputs); } + /** + * @return Whether the shooter is at the target speed with tolerance of 1 m/s + */ public boolean atTargetSpeed(){ return Math.abs(getShooterVelcoity() - shooterTargetSpeed) < 1.0; } + /** + * @return Gets the target velocity in m/s + */ @AutoLogOutput(key="Shooter/TargetSpeed") public double getTargetVelocity(){ return Units.rotationsToRadians(shooterTargetSpeed); diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index a0106a4..40d94f3 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -3,10 +3,8 @@ package frc.robot.subsystems.spindexer; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.IdConstants; -import frc.robot.subsystems.spindexer.SpindexerIO; public class Spindexer extends SubsystemBase implements SpindexerIO{ TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID); @@ -23,14 +21,18 @@ public class Spindexer extends SubsystemBase implements SpindexerIO{ public void periodic() { power = SmartDashboard.getNumber("Spindexer Power", power); SmartDashboard.putNumber("Spindexer Power", power); + motor.set(power); if (inputs.spindexerVelocity < SpindexerConstants.spindexerVelocityWithBall) { ballCount++; } } + /** + * @return + */ public void maxSpindexer(){ - power = 1.0; + power = 0.5; } public void stopSpindexer(){ diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 7e4c6af..1db640e 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -33,8 +33,8 @@ public class Turret extends SubsystemBase implements TurretIO{ private static final double MIN_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MIN_ANGLE); private static final double MAX_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MAX_ANGLE); - private static final double MAX_VEL_RAD_PER_SEC = 600; - private static final double MAX_ACCEL_RAD_PER_SEC2 = 120.0; + private static final double MAX_VEL_RAD_PER_SEC = TurretConstants.MAX_VELOCITY; + private static final double MAX_ACCEL_RAD_PER_SEC2 = TurretConstants.MAX_ACCELERATION; private static final double EXTRAPOLATION_TIME_CONSTANT = 0.06; @@ -42,8 +42,8 @@ public class Turret extends SubsystemBase implements TurretIO{ private double FEEDFORWARD_KV = 0.185; - private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02 - , 0.02); + // Super low magnitude filter for the position to make it less jittery + private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02); private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); @@ -70,7 +70,6 @@ public class Turret extends SubsystemBase implements TurretIO{ private final MechanismRoot2d root = mech.getRoot("turret", 50, 50); private final MechanismLigament2d ligament = root.append(new MechanismLigament2d("barrel", 30, 0)); - // private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0.010); private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0); /* ---------------- Constructor ---------------- */ @@ -107,6 +106,8 @@ public class Turret extends SubsystemBase implements TurretIO{ 0.0); } + //TODO: replace this stuff with Chinese Remainder Theorem calculator -- ignore this for now + double leftAbs = wrapUnit(encoderLeft.getAbsolutePosition().getValueAsDouble() - TurretConstants.LEFT_ENCODER_OFFSET); double rightAbs = wrapUnit(encoderRight.getAbsolutePosition().getValueAsDouble() - TurretConstants.RIGHT_ENCODER_OFFSET); @@ -126,7 +127,7 @@ public class Turret extends SubsystemBase implements TurretIO{ double motorRotations = turretRotations * TurretConstants.TURRET_GEAR_RATIO; motor.setPosition(motorRotations); - motor.setPosition(0.0); //TODO: remove after hardcrt works + motor.setPosition(0.0); //TODO: remove after chinese remainder theorem works SmartDashboard.putData("Turret Mech", mech); @@ -134,6 +135,11 @@ public class Turret extends SubsystemBase implements TurretIO{ /* ---------------- Public API ---------------- */ + /** + * Sets the setpoint position and velocity of the turret. Call in command execute. + * @param angle + * @param velocityRadPerSec + */ public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) { goalAngle = angle; goalVelocityRadPerSec = velocityRadPerSec; @@ -146,10 +152,20 @@ public class Turret extends SubsystemBase implements TurretIO{ return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(2.0); } + /** + * @return Posiiton of the turret in radians + */ public double getPositionRad() { return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()) / GEAR_RATIO; } + /** + * @return Posiiton of the turret in radians + */ + public double getPositionDeg() { + return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO; + } + /* ---------------- Periodic ---------------- */ @Override @@ -193,10 +209,13 @@ public class Turret extends SubsystemBase implements TurretIO{ // Tells the Kraken to get to this position using 1000Hz profile double motorGoalRotations = Units.radiansToRotations(best) * GEAR_RATIO; + // Clamp position setpoint to min and max angles motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(-180) * GEAR_RATIO, Units.degreesToRotations(180) * GEAR_RATIO); + // Multiply goal velocity by kV double robotTurnCompensation = goalVelocityRadPerSec * FEEDFORWARD_KV; + // Sets motor control with feedforward motor.setControl(mmVoltageRequest .withPosition(motorGoalRotations) .withFeedForward(robotTurnCompensation)); @@ -236,6 +255,7 @@ public class Turret extends SubsystemBase implements TurretIO{ inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble(); } + // Also ignore this for now private double wrapUnit(double value) { value %= 1.0; if (value < 0) value += 1.0; diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 10fee61..009c049 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -7,9 +7,10 @@ public class TurretConstants { public static double MAX_ANGLE = 200; public static double MIN_ANGLE = -200; - public static double MAX_VELOCITY = 10000000; // m/s - public static double MAX_ACCELERATION = 10000000; // m/s^2 + public static double MAX_VELOCITY = 600; // rad/s + public static double MAX_ACCELERATION = 120.0; // rad/s^2 + // Not using this, but just in case public static double TURRET_WIDTH = Units.inchesToMeters(6.4); public static double TURRET_RADIUS = TURRET_WIDTH / 2; @@ -22,6 +23,7 @@ public class TurretConstants { public static double LEFT_ENCODER_OFFSET = 9.52; // degrees 9.52 rot public static double RIGHT_ENCODER_OFFSET = 6.53; // degrees 6.53 rot + // Turret is in center of robot, but make use of the height in shooter physics public static Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters public static double CRT_TOLERANCE = 0.01; diff --git a/src/test/java/frc/robot/util/ChineseRemainderTheorumTest.java b/src/test/java/frc/robot/util/ChineseRemainderTheorumTest.java deleted file mode 100644 index 932f5f5..0000000 --- a/src/test/java/frc/robot/util/ChineseRemainderTheorumTest.java +++ /dev/null @@ -1,31 +0,0 @@ - -package frc.robot.util; - -import static org.junit.jupiter.api.Assertions.assertEquals; - -import org.junit.jupiter.api.AfterEach; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; - -import frc.robot.util.ChineseRemainderTheorum.Encoder; - -public class ChineseRemainderTheorumTest { - - @BeforeEach - public void prepare() { - } - - @AfterEach - public void cleanup() { - } - - @Test - public void test() { - double tolerance = 0.01; - - Encoder a = new Encoder(5000 % 123, 123); - Encoder b = new Encoder(5000 % 321, 321); - double val = ChineseRemainderTheorum.compute(a, b, tolerance); - assertEquals(5000, val, tolerance); - } -}