From 232ca44aad20059fcdb0c30687fa86ec39d113fd Mon Sep 17 00:00:00 2001 From: rmheuer <63077980+rmheuer@users.noreply.github.com> Date: Tue, 27 Feb 2024 15:18:00 -0600 Subject: [PATCH] [Robot] Clean up shooter + calculate optimal idle angle --- .../com/swrobotics/robot/config/NTData.java | 2 +- .../robot/control/ControlBoard.java | 97 +++++++++++-------- .../subsystems/lights/LightsSubsystem.java | 2 +- .../subsystems/speaker/PivotSubsystem.java | 30 +++--- .../subsystems/speaker/ShooterSubsystem.java | 76 +++++++-------- .../subsystems/speaker/aim/AimCalculator.java | 1 - .../speaker/aim/ManualAimCalculator.java | 4 +- .../speaker/aim/TableAimCalculator.java | 41 +++++++- 8 files changed, 153 insertions(+), 100 deletions(-) diff --git a/Robot/src/main/java/com/swrobotics/robot/config/NTData.java b/Robot/src/main/java/com/swrobotics/robot/config/NTData.java index 2ed5bb5..097e475 100644 --- a/Robot/src/main/java/com/swrobotics/robot/config/NTData.java +++ b/Robot/src/main/java/com/swrobotics/robot/config/NTData.java @@ -3,6 +3,7 @@ import com.swrobotics.lib.net.NTBoolean; import com.swrobotics.lib.net.NTDouble; import com.swrobotics.lib.net.NTEntry; +import edu.wpi.first.networktables.NetworkTableInstance; public final class NTData { // GOOD @@ -96,7 +97,6 @@ public final class NTData { public static final NTEntry SHOOTER_FLYWHEEL_KV = new NTDouble("Shooter/Flywheel/PIDV/kV", 0.126).setPersistent(); // GOOD (except for allowable pct err) - public static final NTEntry SHOOTER_PIVOT_IDLE_ANGLE = new NTDouble("Shooter/Pivot/Idle Angle (deg)", 35).setPersistent(); public static final NTEntry SHOOTER_PIVOT_KP = new NTDouble("Shooter/Pivot/PID/kP", 600).setPersistent(); public static final NTEntry SHOOTER_PIVOT_KD = new NTDouble("Shooter/Pivot/PID/kD", 0).setPersistent(); public static final NTEntry SHOOTER_PIVOT_KV = new NTDouble("Shooter/Pivot/PID/kV", 0).setPersistent(); diff --git a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java index cc7a59a..302f38e 100644 --- a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java +++ b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java @@ -1,14 +1,11 @@ package com.swrobotics.robot.control; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; import com.swrobotics.lib.input.XboxController; import com.swrobotics.mathlib.MathUtil; import com.swrobotics.robot.RobotContainer; import com.swrobotics.robot.commands.AimTowardsSpeakerCommand; -import com.swrobotics.robot.commands.ShootCommand; import com.swrobotics.robot.config.NTData; import com.swrobotics.robot.subsystems.amp.AmpArmSubsystem; import com.swrobotics.robot.subsystems.amp.AmpIntakeSubsystem; @@ -17,13 +14,10 @@ import com.swrobotics.robot.subsystems.speaker.ShooterSubsystem; import com.swrobotics.robot.subsystems.speaker.aim.AmpAimCalculator; import com.swrobotics.robot.subsystems.swerve.SwerveDrive; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -126,16 +120,7 @@ private boolean getRobotRelativeDrive() { return driver.leftBumper.isPressed(); } - @Override - public void periodic() { - if (!DriverStation.isTeleop()) { - climberState = ClimberArm.State.RETRACTED_IDLE; - robot.indexer.setReverse(false); - robot.intake.setReverse(false); - robot.shooter.setFlywheelControl(ShooterSubsystem.FlywheelControl.SHOOT); // Always aim with note when not teleop - return; - } - + private void drivePeriodic() { Translation2d translation = getDriveTranslation(); if (DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Red) { translation = translation.rotateBy(Rotation2d.fromDegrees(180)); @@ -145,10 +130,10 @@ public void periodic() { Rotation2d rotation = new Rotation2d(MathUtil.TAU * rawRotation); ChassisSpeeds chassisRequest = ChassisSpeeds.fromFieldRelativeSpeeds( - translation.getX(), - translation.getY(), - rotation.getRadians(), - robot.drive.getEstimatedPose().getRotation()); + translation.getX(), + translation.getY(), + rotation.getRadians(), + robot.drive.getEstimatedPose().getRotation()); if (getRobotRelativeDrive()) { chassisRequest = new ChassisSpeeds(-translation.getX(), -translation.getY(), rotation.getRadians()); @@ -158,16 +143,20 @@ public void periodic() { SwerveDrive.DRIVER_PRIORITY, chassisRequest, DriveRequestType.Velocity); + } - IntakeSubsystem.State intakeState = IntakeSubsystem.State.OFF; - if (operator.a.isPressed()) - intakeState = IntakeSubsystem.State.INTAKE; - - // Indexer uses the intake state also - robot.intake.set(intakeState); - boolean operatorWantsShoot = operator.b.isPressed(); - robot.indexer.setFeedToShooter(operatorWantsShoot); + private void climberPeriodic() { + if (operator.rightBumper.isRising()) { + boolean extended = climberState == ClimberArm.State.EXTENDED; + climberState = extended ? ClimberArm.State.RETRACTED_IDLE : ClimberArm.State.EXTENDED; + } + if (operator.rightTrigger.isOutside(TRIGGER_BUTTON_THRESHOLD)) { + climberState = ClimberArm.State.RETRACTED_HOLD; + } + robot.climber.setState(climberState); + } + private void ampArmPeriodic() { AmpArmSubsystem.Position ampArmPosition = AmpArmSubsystem.Position.STOW; AmpIntakeSubsystem.State ampIntakeState = AmpIntakeSubsystem.State.OFF; if (operator.x.isPressed()) { @@ -183,31 +172,55 @@ public void periodic() { } robot.ampArm.setPosition(ampArmPosition); robot.ampIntake.setState(ampIntakeState); + } - if (operator.rightBumper.isRising()) { - boolean extended = climberState == ClimberArm.State.EXTENDED; - climberState = extended ? ClimberArm.State.RETRACTED_IDLE : ClimberArm.State.EXTENDED; - } - if (operator.rightTrigger.isOutside(TRIGGER_BUTTON_THRESHOLD)) { - climberState = ClimberArm.State.RETRACTED_HOLD; - } - robot.climber.setState(climberState); + private void intakePeriodic() { + IntakeSubsystem.State intakeState = IntakeSubsystem.State.OFF; + if (operator.a.isPressed()) + intakeState = IntakeSubsystem.State.INTAKE; + // Indexer uses the intake state also (feeds in when intake is on) + robot.intake.set(intakeState); robot.intake.setReverse(operator.back.isPressed()); + } + + private void shooterPeriodic() { + boolean aimAtSpeaker = driverWantsAim(); + + boolean shoot = operator.b.isPressed(); + robot.indexer.setFeedToShooter(shoot); robot.indexer.setReverse(operator.start.isPressed()); - boolean shootAmp = operator.y.isPressed(); - if (shootAmp) - robot.shooter.setTempAimCalculator(AmpAimCalculator.INSTANCE); + boolean aimAtAmp = operator.y.isPressed(); + if (aimAtAmp) + robot.shooter.setTempAimCalculator(AmpAimCalculator.INSTANCE); // TODO: Probably better way to do this + + boolean runFlywheelForShooting = aimAtSpeaker || aimAtAmp || driverWantsFlywheels(); -// robot.shooter.setFlywheelControl(driverWantsAim() || driverWantsFlywheels()); ShooterSubsystem.FlywheelControl flywheelControl = ShooterSubsystem.FlywheelControl.IDLE; if (operator.start.isPressed()) flywheelControl = ShooterSubsystem.FlywheelControl.REVERSE; - else if (driverWantsAim() || driverWantsFlywheels() || shootAmp) + else if (runFlywheelForShooting) flywheelControl = ShooterSubsystem.FlywheelControl.SHOOT; - else if (operatorWantsShoot) + else if (shoot) flywheelControl = ShooterSubsystem.FlywheelControl.POOP; robot.shooter.setFlywheelControl(flywheelControl); } + + @Override + public void periodic() { + if (!DriverStation.isTeleop()) { + climberState = ClimberArm.State.RETRACTED_IDLE; + robot.indexer.setReverse(false); + robot.intake.setReverse(false); + robot.shooter.setFlywheelControl(ShooterSubsystem.FlywheelControl.SHOOT); // Always aim with note when not teleop + return; + } + + drivePeriodic(); + climberPeriodic(); + // ampArmPeriodic(); + intakePeriodic(); + shooterPeriodic(); + } } diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java index 71f2cc8..d3c6661 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java @@ -109,7 +109,7 @@ public void periodic() { prideSequencer.apply(this); } else if (robot.climber.getCurrentState() == ClimberArm.State.RETRACTED_HOLD) { showClimberHold(); - } else if (robot.shooter.isPreparing()) { + } else if (robot.shooter.isPreparingToShoot()) { showShooterStatus(); } else if (robot.drive.getLastSelectedPriority() == SwerveDrive.AUTO_PRIORITY) { showAutoDriving(); diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/PivotSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/PivotSubsystem.java index 0aa501d..f435a4f 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/PivotSubsystem.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/PivotSubsystem.java @@ -13,14 +13,17 @@ import com.swrobotics.robot.config.IOAllocation; import com.swrobotics.robot.config.NTData; import com.swrobotics.robot.logging.SimView; +import com.swrobotics.robot.subsystems.speaker.aim.TableAimCalculator; import com.swrobotics.robot.utils.TalonFXWithSim; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; public final class PivotSubsystem extends SubsystemBase { private enum State { CALIBRATING, IDLE, + OFF, SHOOTING } @@ -62,28 +65,33 @@ public PivotSubsystem() { setpoint = Double.NEGATIVE_INFINITY; } - public void setTargetAngle(double angleRot) { - if (state == State.CALIBRATING) - return; - + private void setTargetAngle(double angleRot) { angleRot = MathUtil.clamp( angleRot, hardStopAngle + 2 / 360.0, maxAngle - 2 / 360.0); motor.setControl(new PositionVoltage(angleRot)); - state = State.SHOOTING; setpoint = angleRot; } + public void setShooting(double angleRot) { + if (state == State.CALIBRATING) + return; + + setTargetAngle(angleRot); + state = State.SHOOTING; + } + public void setIdle() { if (state == State.CALIBRATING) return; - if (state != State.IDLE) + // Don't recalibrate in auto to save time + if (state != State.IDLE && !DriverStation.isAutonomous()) calibrated = false; - setTargetAngle(NTData.SHOOTER_PIVOT_IDLE_ANGLE.get() / 360.0); + setTargetAngle(TableAimCalculator.INSTANCE.getIdleAngle() / MathUtil.TAU); state = State.IDLE; } @@ -91,11 +99,8 @@ public void setNeutral() { if (state == State.CALIBRATING) return; - if (state != State.IDLE) - calibrated = false; - motor.setControl(new NeutralOut()); - state = State.IDLE; + state = State.OFF; } @Override @@ -106,13 +111,14 @@ public void periodic() { calibrated = false; } - if (state != State.SHOOTING && !calibrated) { + if ((state == State.CALIBRATING || state == State.IDLE) && !calibrated) { motor.setControl(new VoltageOut(-NTData.SHOOTER_PIVOT_CALIBRATE_VOLTS.get())); limitSwitch.refresh(); boolean atLimit = limitSwitch.getValue() == ReverseLimitValue.ClosedToGround; if (atLimit) { motor.setPosition(hardStopAngle); + motor.setControl(new NeutralOut()); calibrated = true; if (state == State.CALIBRATING) diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java index 3fc9eb0..efbfc30 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java @@ -32,11 +32,10 @@ public enum FlywheelControl { private final IndexerSubsystem indexer; private Debouncer afterShootDelay; - private boolean isPreparing; + private boolean isPreparingToShoot; private AimCalculator.Aim targetAim; // Target aim is null if not currently aiming private AimCalculator aimCalculator; - private final AimCalculator tableAimCalculator; private FlywheelControl flywheelControl; @@ -48,8 +47,7 @@ public ShooterSubsystem(SwerveDrive drive, IndexerSubsystem indexer) { this.indexer = indexer; // aimCalculator = new ManualAimCalculator(); - tableAimCalculator = new TableAimCalculator(); - aimCalculator = tableAimCalculator; + aimCalculator = TableAimCalculator.INSTANCE; NTData.SHOOTER_AFTER_DELAY.nowAndOnChange((delay) -> afterShootDelay = new Debouncer(delay, Debouncer.DebounceType.kFalling)); @@ -62,68 +60,70 @@ public Translation2d getSpeakerPosition() { @Override public void periodic() { + isPreparingToShoot = false; + double distToSpeaker = getSpeakerPosition().getDistance(drive.getEstimatedPose().getTranslation()); AimCalculator.Aim aim = aimCalculator.calculateAim(distToSpeaker); targetAim = aim; + // Wait until the pivot is calibrated to do any shooting if (DriverStation.isDisabled() || !pivot.hasCalibrated()) return; - // TODO: Make this not a mess - isPreparing = false; - if ((aim != null) && DriverStation.isAutonomous()) { - // Have the shooter be constantly active during auto - isPreparing = true; - flywheel.setTargetVelocity(aim.flywheelVelocity()); - pivot.setTargetAngle(aim.pivotAngle() / MathUtil.TAU); - } else if (flywheelControl == FlywheelControl.REVERSE) { - // Don't touch pivot so it won't move and squish stuck note + // To get note unstuck if we squish it + if (flywheelControl == FlywheelControl.REVERSE) { + pivot.setNeutral(); // Don't squish note any more than we are currently flywheel.setDutyCycle(-NTData.SHOOTER_FLYWHEEL_REVERSE_SPEED.get()); - } else if (flywheelControl == FlywheelControl.SHOOT || afterShootDelay.calculate(indexer.hasPiece())) { - if (aim != null) { - isPreparing = true; - if (flywheelControl == FlywheelControl.SHOOT) - flywheel.setTargetVelocity(aim.flywheelVelocity()); - else if (flywheelControl == FlywheelControl.POOP) - flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get()); - else - flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_IDLE_SPEED.get()); - pivot.setTargetAngle(aim.pivotAngle() / MathUtil.TAU); - } else { - if (flywheelControl == FlywheelControl.POOP) - flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get()); - else - flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_IDLE_SPEED.get()); - pivot.setIdle(); - } - } else { - flywheel.setNeutral(); - pivot.setNeutral(); + return; + } + + if (flywheelControl == FlywheelControl.POOP) { + pivot.setIdle(); + flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get()); + return; } + boolean inAuto = DriverStation.isAutonomous(); + boolean hasPiece = indexer.hasPiece(); + boolean shouldContinue = afterShootDelay.calculate(hasPiece); + + if (flywheelControl == FlywheelControl.SHOOT && (shouldContinue || inAuto)) { + // Actually do shooting + isPreparingToShoot = true; + flywheel.setTargetVelocity(aim.flywheelVelocity()); + pivot.setShooting(aim.pivotAngle() / MathUtil.TAU); + } else { + // In IDLE and continue time has elapsed + pivot.setIdle(); + + if (hasPiece) + flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_IDLE_SPEED.get()); + else + flywheel.setNeutral(); + } NTData.SHOOTER_READY.set(isReadyToShoot()); pctErr.set(flywheel.getPercentErr()); - aimCalculator = tableAimCalculator; + aimCalculator = TableAimCalculator.INSTANCE; } NTDouble pctErr = new NTDouble("Shooter/Debug/Percent Error", 0); @Override public void simulationPeriodic() { - if (isPreparing) + if (isPreparingToShoot) SimView.targetTrajectory.update(targetAim); else SimView.targetTrajectory.clear(); } - public boolean isPreparing() { - return isPreparing; + public boolean isPreparingToShoot() { + return isPreparingToShoot; } public boolean isReadyToShoot() { - return isPreparing && flywheel.isReadyToShoot() && pivot.isAtSetpoint(); + return isPreparingToShoot && flywheel.isReadyToShoot() && pivot.isAtSetpoint(); } public void setFlywheelControl(FlywheelControl flywheelControl) { diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/AimCalculator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/AimCalculator.java index 9385706..c91184e 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/AimCalculator.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/AimCalculator.java @@ -6,6 +6,5 @@ public interface AimCalculator { final record Aim(double flywheelVelocity, double pivotAngle) {} // Distance is in meters - // May return null if shot is impossible Aim calculateAim(double distanceToSpeaker); } diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/ManualAimCalculator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/ManualAimCalculator.java index 8897115..1b6894a 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/ManualAimCalculator.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/ManualAimCalculator.java @@ -10,15 +10,13 @@ public final class ManualAimCalculator implements AimCalculator { private static final NTBoolean setFromTable = new NTBoolean("Shooter/Manual Aim/Set From Table", false); - private final TableAimCalculator tableRef = new TableAimCalculator(); - @Override public Aim calculateAim(double distanceToSpeaker) { logDistance.set(distanceToSpeaker); if (setFromTable.get()) { // setFromTable.set(false); - Aim ref = tableRef.calculateAim(distanceToSpeaker); + Aim ref = TableAimCalculator.INSTANCE.calculateAim(distanceToSpeaker); velocitySetpoint.set(ref.flywheelVelocity()); pivotSetpoint.set(Math.toDegrees(ref.pivotAngle())); } diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java index 06414c7..07377ea 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java @@ -3,15 +3,21 @@ import com.swrobotics.lib.net.NTDouble; import com.swrobotics.mathlib.MathUtil; +import java.util.ArrayList; +import java.util.List; +import java.util.Map; import java.util.TreeMap; public final class TableAimCalculator implements AimCalculator { + public static final TableAimCalculator INSTANCE = new TableAimCalculator(); + private static final NTDouble logDistance = new NTDouble("Shooter/Manual Aim/Est Distance (m)", 0); + private final TreeMap flywheelVelocityMap = new TreeMap<>(); private final TreeMap pivotAngleMap = new TreeMap<>(); - private static final NTDouble logDistance = new NTDouble("Shooter/Manual Aim/Est Distance (m)", 0); + private final double idleAngle; - public TableAimCalculator() { + private TableAimCalculator() { // Calibration 1 (MURA) double off = 5; addCalibrationSample(1.1429, 63 - off, 35); @@ -27,6 +33,8 @@ public TableAimCalculator() { // Calibration 3 (MURA) addCalibrationSample(3.549, 31, 74); addCalibrationSample(4.917, 24, 80); + + idleAngle = calculateIdleAngle(); } private void addCalibrationSample(double distanceM, double angleDeg, double velocityRPS) { @@ -70,4 +78,33 @@ public Aim calculateAim(double distanceToSpeaker) { sample(pivotAngleMap, distanceToSpeaker) ); } + + // Finds the optimal idle angle to get to positions in the least + // time on average + private double calculateIdleAngle() { + // Convert to list so we can access by index + List> entries = new ArrayList<>(pivotAngleMap.entrySet()); + if (entries.size() == 1) + return entries.get(0).getValue(); + + // Find the integral of the shooting curve from min to max distance + double width = 0; + double area = 0; + for (int i = 1; i < entries.size(); i++) { + Map.Entry prev = entries.get(i - 1); + Map.Entry entry = entries.get(i); + + double span = entry.getKey() - prev.getKey(); + width += span; + area += span * (prev.getValue() + entry.getValue()) / 2; + } + + // Find average angle over the range + return area / width; + } + + // Radians + public double getIdleAngle() { + return idleAngle; + } }