diff --git a/.vscode/settings.json b/.vscode/settings.json index b1d5e10..088504f 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -44,6 +44,7 @@ "Autons", "bezier", "deadband", + "Desmos", "Feedforward", "Holonomic", "keybinds", diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 099e596..b33058c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -183,18 +183,6 @@ public RobotContainer() { NamedCommands.registerCommand("ShootFarPosition", ShootFar.run(sArm).withTimeout(2.0)); - // NamedCommands.registerCommand( - // "FloorPickupPosition", new FloorPickupCommand(sArm).withTimeout(2.0)); - - // NamedCommands.registerCommand( - // "ShootClosePosition", new ShootCloseCommand(sArm).withTimeout(2.0)); - - // NamedCommands.registerCommand( - // "ShootMediumPosition", new ShootMediumCommand(sArm).withTimeout(2.0)); - - // NamedCommands.registerCommand("ShootFarPosition", new - // ShootFarCommand(sArm).withTimeout(2.0)); - NamedCommands.registerCommand( "PreRunShooter", new PreRunShooter(sFlywheel, sIntake).withTimeout(4.0)); @@ -210,9 +198,8 @@ public RobotContainer() { NamedCommands.registerCommand( // The name is inaccurate "ShootMediumGroup", new ShootNote(sIntake, sFlywheel, sArm).withTimeout(3.0)); - // In testing - NamedCommands.registerCommand( - "SpeakerShot", SpeakerShot.run(1, sArm).withTimeout(5.0)); // TODO Replace SpeakerDistance + // TEST + NamedCommands.registerCommand("SpeakerShot", SpeakerShot.run(sArm, sDrive).withTimeout(5.0)); SendableChooser chooser = new SendableChooser<>(); diff --git a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java index 7654cd5..b6c53f7 100644 --- a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java @@ -1,4 +1,4 @@ -// Copyright 2016-2024 FRC 6328, FRC 5829 +// Copyright 2016-2024 FRC 6328, FRC 5172, FRC 5829 // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License diff --git a/src/main/java/frc/robot/commands/Positions/AmpShot.java b/src/main/java/frc/robot/commands/Positions/AmpShot.java index ef1b453..8d06ab4 100644 --- a/src/main/java/frc/robot/commands/Positions/AmpShot.java +++ b/src/main/java/frc/robot/commands/Positions/AmpShot.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class AmpShot { @@ -22,14 +23,15 @@ public static Command run(Arm arm) { return Commands.run( () -> { // Position preset settings - double ARMANGLE = 2.093; + double ArmAngle = 2.093; - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); }, arm); } } -// TODO check to add the `.until( +// TODO check if .until needed: +// .until( // () -> { // return arm.getIsFinished(); // });` diff --git a/src/main/java/frc/robot/commands/Positions/AmpShotCommand.java b/src/main/java/frc/robot/commands/Positions/AmpShotCommand.java index 791c146..11a226b 100644 --- a/src/main/java/frc/robot/commands/Positions/AmpShotCommand.java +++ b/src/main/java/frc/robot/commands/Positions/AmpShotCommand.java @@ -14,12 +14,13 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class AmpShotCommand extends Command { private Arm arm; - double ARMANGLE = 2.093; + double ArmAngle = 2.093; public AmpShotCommand(Arm arm) { this.arm = arm; @@ -33,7 +34,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); } // Called once the command ends or is interrupted. @@ -42,6 +43,6 @@ public void end(boolean interrupted) {} @Override public boolean isFinished() { - return arm.getIsFinished(); + return arm.isRoughlyAtSetpoint(); } } diff --git a/src/main/java/frc/robot/commands/Positions/Climb.java b/src/main/java/frc/robot/commands/Positions/Climb.java index 6ac95a9..1256436 100644 --- a/src/main/java/frc/robot/commands/Positions/Climb.java +++ b/src/main/java/frc/robot/commands/Positions/Climb.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class Climb { @@ -22,14 +23,15 @@ public static Command run(Arm arm) { return Commands.run( () -> { // Position preset settings - double ARMANGLE = 2.53; + double ArmAngle = 2.53; - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); }, arm); } } -// TODO check to add the `.until( +// TODO check if .until needed: +// .until( // () -> { // return arm.getIsFinished(); // });` diff --git a/src/main/java/frc/robot/commands/Positions/FloorPickup.java b/src/main/java/frc/robot/commands/Positions/FloorPickup.java index 1b96f58..03bc408 100644 --- a/src/main/java/frc/robot/commands/Positions/FloorPickup.java +++ b/src/main/java/frc/robot/commands/Positions/FloorPickup.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class FloorPickup { @@ -22,14 +23,14 @@ public static Command run(Arm arm) { return Commands.run( () -> { // Position preset settings - double ARMANGLE = 0.09; + double ArmAngle = 0.09; - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); }, arm) .until( () -> { - return arm.getIsFinished(); + return arm.isRoughlyAtSetpoint(); }); } } diff --git a/src/main/java/frc/robot/commands/Positions/FloorPickupCommand.java b/src/main/java/frc/robot/commands/Positions/FloorPickupCommand.java index a2abf89..7fb8d33 100644 --- a/src/main/java/frc/robot/commands/Positions/FloorPickupCommand.java +++ b/src/main/java/frc/robot/commands/Positions/FloorPickupCommand.java @@ -14,12 +14,13 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class FloorPickupCommand extends Command { private Arm arm; - double ARMANGLE = 0.135; + double ArmAngle = 0.135; public FloorPickupCommand(Arm arm) { this.arm = arm; @@ -33,7 +34,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); } // Called once the command ends or is interrupted. @@ -42,6 +43,6 @@ public void end(boolean interrupted) {} @Override public boolean isFinished() { - return arm.getIsFinished(); + return arm.isRoughlyAtSetpoint(); } } diff --git a/src/main/java/frc/robot/commands/Positions/ShootClose.java b/src/main/java/frc/robot/commands/Positions/ShootClose.java index d9225ae..93b4737 100644 --- a/src/main/java/frc/robot/commands/Positions/ShootClose.java +++ b/src/main/java/frc/robot/commands/Positions/ShootClose.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class ShootClose { // bumper pressed against the wall @@ -22,14 +23,14 @@ public static Command run(Arm arm) { return Commands.run( () -> { // Position preset settings - double ARMANGLE = 0.747; + double ArmAngle = 0.747; - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); }, arm) .until( () -> { - return arm.getIsFinished(); + return arm.isRoughlyAtSetpoint(); }); } } diff --git a/src/main/java/frc/robot/commands/Positions/ShootCloseCommand.java b/src/main/java/frc/robot/commands/Positions/ShootCloseCommand.java index cc9c332..b1a47c6 100644 --- a/src/main/java/frc/robot/commands/Positions/ShootCloseCommand.java +++ b/src/main/java/frc/robot/commands/Positions/ShootCloseCommand.java @@ -14,12 +14,13 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class ShootCloseCommand extends Command { private Arm arm; - double ARMANGLE = 0.687; + double ArmAngle = 0.687; public ShootCloseCommand(Arm arm) { this.arm = arm; @@ -33,7 +34,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); } // Called once the command ends or is interrupted. @@ -42,6 +43,6 @@ public void end(boolean interrupted) {} @Override public boolean isFinished() { - return arm.getIsFinished(); + return arm.isRoughlyAtSetpoint(); } } diff --git a/src/main/java/frc/robot/commands/Positions/ShootFar.java b/src/main/java/frc/robot/commands/Positions/ShootFar.java index e65bfad..dcb7d2f 100644 --- a/src/main/java/frc/robot/commands/Positions/ShootFar.java +++ b/src/main/java/frc/robot/commands/Positions/ShootFar.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class ShootFar { // two robots away from wall @@ -22,14 +23,13 @@ public static Command run(Arm arm) { return Commands.run( () -> { // Position preset settings - double ARMANGLE = 0.799; - - arm.runTargetAngle(ARMANGLE); + double ArmAngle = 0.799; + arm.runTargetAngle(Optional.of(ArmAngle)); }, arm) .until( () -> { - return arm.getIsFinished(); + return arm.isRoughlyAtSetpoint(); }); } } diff --git a/src/main/java/frc/robot/commands/Positions/ShootFarCommand.java b/src/main/java/frc/robot/commands/Positions/ShootFarCommand.java index 40169f4..c65d9ae 100644 --- a/src/main/java/frc/robot/commands/Positions/ShootFarCommand.java +++ b/src/main/java/frc/robot/commands/Positions/ShootFarCommand.java @@ -14,12 +14,13 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class ShootFarCommand extends Command { private Arm arm; - double ARMANGLE = 0.799; + double ArmAngle = 0.799; public ShootFarCommand(Arm arm) { this.arm = arm; @@ -33,7 +34,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); } // Called once the command ends or is interrupted. @@ -42,6 +43,6 @@ public void end(boolean interrupted) {} @Override public boolean isFinished() { - return arm.getIsFinished(); + return arm.isRoughlyAtSetpoint(); } } diff --git a/src/main/java/frc/robot/commands/Positions/ShootMedium.java b/src/main/java/frc/robot/commands/Positions/ShootMedium.java index a49e447..394e839 100644 --- a/src/main/java/frc/robot/commands/Positions/ShootMedium.java +++ b/src/main/java/frc/robot/commands/Positions/ShootMedium.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class ShootMedium { // one robot length away @@ -22,14 +23,14 @@ public static Command run(Arm arm) { return Commands.run( () -> { // Position preset settings - double ARMANGLE = 0.933; + double ArmAngle = 0.933; - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); }, arm) .until( () -> { - return arm.getIsFinished(); + return arm.isRoughlyAtSetpoint(); }); } } diff --git a/src/main/java/frc/robot/commands/Positions/ShootMediumCommand.java b/src/main/java/frc/robot/commands/Positions/ShootMediumCommand.java index dacedf1..6e6532c 100644 --- a/src/main/java/frc/robot/commands/Positions/ShootMediumCommand.java +++ b/src/main/java/frc/robot/commands/Positions/ShootMediumCommand.java @@ -14,12 +14,13 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class ShootMediumCommand extends Command { private Arm arm; - double ARMANGLE = 0.933; + double ArmAngle = 0.933; public ShootMediumCommand(Arm arm) { this.arm = arm; @@ -33,7 +34,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); } // Called once the command ends or is interrupted. @@ -42,6 +43,6 @@ public void end(boolean interrupted) {} @Override public boolean isFinished() { - return arm.getIsFinished(); + return arm.isRoughlyAtSetpoint(); } } diff --git a/src/main/java/frc/robot/commands/Positions/SpeakerShot.java b/src/main/java/frc/robot/commands/Positions/SpeakerShot.java index a38ac0d..b626de3 100644 --- a/src/main/java/frc/robot/commands/Positions/SpeakerShot.java +++ b/src/main/java/frc/robot/commands/Positions/SpeakerShot.java @@ -12,28 +12,81 @@ package frc.robot.commands.Positions; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.drive.Drive; +import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.FieldConstants; +import java.util.Optional; public class SpeakerShot { - public static Command run(double SpeakerDistance, Arm arm) { + public static Command run(Arm arm, Drive drive) { return Commands.run( () -> { - /* Physics calculation for the note: + Pose2d currentPose = drive.getPose(); + Optional speakerDistance = Optional.empty(); + try { + speakerDistance = + Optional.of( + Math.sqrt( + Math.pow( + currentPose.getX() + - AllianceFlipUtil.apply( + FieldConstants.Speaker.centerSpeakerOpening + .getTranslation()) + .getX(), + 2) + + Math.pow( + currentPose.getY() + - AllianceFlipUtil.apply( + FieldConstants.Speaker.centerSpeakerOpening + .getTranslation()) + .getY(), + 2))); + } catch (Exception e) { + } - */ - - // Position settings - double ARMANGLE = ((0.345 * Math.PI * 2.0) - 0.18) / 2; // TODO Temporary - - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(getRequiredAngle(speakerDistance)); }, arm); } + + /** + * Gets the arm angle required for the note to score into the speaker. Calculates this using a + * polynomial regression of 5 known points. + */ + public static Optional getRequiredAngle(Optional speakerDistance) { + // Numbers calculated using Desmos. Note that the range is between 1.753 (upwards) and 0.09 + // (floor pickup) at the moment. Polynomial form ax^2 + bx + c. + final double a = 1.0; + final double b = 1.0; + final double c = 1.0; + + final double calculatedArmAngle = + a * Math.pow(speakerDistance.get(), 2) + b * speakerDistance.get() + c; + + // TODO once this is merged into main, make the FloorPickup and the Upwards angles be constants + // and reference them for here so they can update nicely. + + if (Math.abs(calculatedArmAngle) < 1.753 && Math.abs(calculatedArmAngle) > 0.09) { + System.err.println( + "ERROR: The calculated SpeakerShot angle is outside of the allowed range for the arm."); + return Optional.empty(); + } + + if (speakerDistance.isEmpty()) { + System.err.println("WARNING: No speakerDistance detected for SpeakerShot."); + return Optional.empty(); + } + + return Optional.of(calculatedArmAngle); + } } -// TODO check to add the `.until( +// TODO check if .until needed: +// .until() // () -> { // return arm.getIsFinished(); // });` diff --git a/src/main/java/frc/robot/commands/Positions/StowPosition.java b/src/main/java/frc/robot/commands/Positions/StowPosition.java index 3705b68..a861a8f 100644 --- a/src/main/java/frc/robot/commands/Positions/StowPosition.java +++ b/src/main/java/frc/robot/commands/Positions/StowPosition.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class StowPosition { @@ -22,15 +23,16 @@ public static Command run(Arm arm) { return Commands.run( () -> { // Position preset settings - double ARMANGLE = 0.35; + double ArmAngle = 0.35; - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); }, arm); } } -// TODO check to add the `.until( +// TODO check if .until needed: +// .until( // () -> { // return arm.getIsFinished(); // });` diff --git a/src/main/java/frc/robot/commands/Positions/StraightForwards.java b/src/main/java/frc/robot/commands/Positions/StraightForwards.java index 4d1fd19..d1f125d 100644 --- a/src/main/java/frc/robot/commands/Positions/StraightForwards.java +++ b/src/main/java/frc/robot/commands/Positions/StraightForwards.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class StraightForwards { @@ -22,14 +23,15 @@ public static Command run(Arm arm) { return Commands.run( () -> { // Position preset settings - double ARMANGLE = 0.111 * Math.PI * 2.0; + double ArmAngle = 0.111 * Math.PI * 2.0; - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); }, arm); } } -// TODO check to add the `.until( +// TODO check if .until needed: +// .until( // () -> { // return arm.getIsFinished(); // });` diff --git a/src/main/java/frc/robot/commands/Positions/Upwards.java b/src/main/java/frc/robot/commands/Positions/Upwards.java index 9a3242b..b25ac75 100644 --- a/src/main/java/frc/robot/commands/Positions/Upwards.java +++ b/src/main/java/frc/robot/commands/Positions/Upwards.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.arm.Arm; +import java.util.Optional; public class Upwards { @@ -22,9 +23,9 @@ public static Command run(Arm arm) { return Commands.run( () -> { // Position preset settings - double ARMANGLE = 1.753; + double ArmAngle = 1.753; - arm.runTargetAngle(ARMANGLE); + arm.runTargetAngle(Optional.of(ArmAngle)); }, arm); } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 16b85ad..ce05753 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.ArmConstants; import frc.robot.Constants.EnvironmentalConstants; +import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -85,8 +86,8 @@ public void runVelocity(double velocityRPM) { // , ffModel.calculate(velocityRadPerSec)); } - public boolean getIsFinished() { - return io.getIsFinished(); + public boolean isRoughlyAtSetpoint() { + return inputs.isRoughlyAtSetpoint; } /** @@ -94,7 +95,7 @@ public boolean getIsFinished() { * * @param position Angle in radians. */ - public void runTargetAngle(double position) { + public void runTargetAngle(Optional position) { io.setTargetAngle(position); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index ed84b2d..260985a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -12,6 +12,7 @@ package frc.robot.subsystems.arm; +import java.util.Optional; import org.littletonrobotics.junction.AutoLog; public interface ArmIO { @@ -22,6 +23,7 @@ public static class ArmIOInputs { public double appliedVolts = 0.0; public double[] currentAmps = new double[] {}; public double targetPositionRad; + public boolean isRoughlyAtSetpoint; } /** Updates the set of loggable inputs. */ @@ -34,7 +36,7 @@ public default void setVoltage(double volts) {} public default void setVelocity(double velocityRadPerSec) {} /** Run loop for the specified target angle. */ - public default void setTargetAngle(double angle) {} + public default void setTargetAngle(Optional angle) {} /** Update motor speeds according to PID values from target angle. */ public default void updateMotorSpeeds() {} @@ -44,8 +46,4 @@ public default void stop() {} /** Set velocity PID constants. */ public default void configurePID(double kP, double kI, double kD) {} - - public default boolean getIsFinished() { - return false; - } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 3f1ccab..a71dca0 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.robot.Constants.ArmConstants; +import frc.robot.Constants.EnvironmentalConstants; public class ArmIOSim implements ArmIO { // ! The settings on this simulation are wrong, fix later (or not lol) @@ -36,7 +37,7 @@ public void updateInputs(ArmIOInputs inputs) { sim.setInputVoltage(appliedVolts); } - sim.update(0.02); + sim.update(EnvironmentalConstants.loopPeriodMs); inputs.positionRad = 0.0; inputs.velocityRadPerSec = sim.getVelocityRadPerSec(); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java index 4bd74e6..a234c5d 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java @@ -19,6 +19,8 @@ import edu.wpi.first.math.util.Units; import frc.robot.Constants; import frc.robot.Constants.ArmConstants; +import frc.robot.Constants.EnvironmentalConstants; +import java.util.Optional; /** * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with @@ -98,6 +100,7 @@ public void updateInputs(ArmIOInputs inputs) { inputs.appliedVolts = leftMotor.getAppliedOutput() * leftMotor.getBusVoltage(); inputs.currentAmps = new double[] {leftMotor.getOutputCurrent()}; inputs.targetPositionRad = targetAngle; + inputs.isRoughlyAtSetpoint = isRoughlyAtSetpoint(); } private double getSmoothedPosition() { @@ -118,8 +121,7 @@ public void setVoltage(double volts) { } // Returns true if the arm is stationary. - @Override - public boolean getIsFinished() { + public boolean isRoughlyAtSetpoint() { double velocity = Units.rotationsPerMinuteToRadiansPerSecond(leftAbsoluteEncoder.getVelocity() / GEAR_RATIO); return (Math.abs(getSmoothedPosition() - targetAngle) < 0.01) && (Math.abs(velocity) < 0.001); @@ -129,16 +131,20 @@ public boolean getIsFinished() { /** Sets the velocity at the specified velocity. Needs to be run periodically. */ public void setVelocity(double velocityRPM) { setTargetAngle( - targetAngle - + 0.02 // TODO correct cycle time here needed - // * ArmConstants.armConversion - * Units.rotationsToRadians(velocityRPM)); + Optional.of( + targetAngle + + EnvironmentalConstants.loopPeriodMs + // * ArmConstants.armConversion + * Units.rotationsToRadians(velocityRPM))); } @Override /** Set the target angle. In radians. */ - public void setTargetAngle(double angle) { - targetAngle = MathUtil.clamp(angle, ArmConstants.minimumAngle, ArmConstants.maximumAngle); + public void setTargetAngle(Optional angle) { + if (!angle.isEmpty() && angle.isPresent()) { + targetAngle = + MathUtil.clamp(angle.get(), ArmConstants.minimumAngle, ArmConstants.maximumAngle); + } // Note that updateMotorSpeeds(); is run periodically, which actually applies these changes. // pid.setReference( diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java index 6a54a7e..80da0dc 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.robot.Constants.ClimberConstants; +import frc.robot.Constants.EnvironmentalConstants; public class ClimberIOSim implements ClimberIO { // ! The settings on this simulation are wrong, fix later (or not lol) @@ -37,7 +38,7 @@ public void updateInputs(ClimberIOInputs inputs) { sim.setInputVoltage(appliedVolts); } - sim.update(0.02); + sim.update(EnvironmentalConstants.loopPeriodMs); inputs.rightPosition = 0.0; inputs.rightVelocity = sim.getVelocityRadPerSec(); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index f7bbb74..afb8a69 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -42,6 +42,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants.EnvironmentalConstants; import frc.robot.util.LocalADStarAK; import frc.robot.util.VisionHelpers.TimestampedVisionUpdate; import java.util.List; @@ -217,7 +218,8 @@ public void periodic() { */ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + ChassisSpeeds discreteSpeeds = + ChassisSpeeds.discretize(speeds, EnvironmentalConstants.loopPeriodMs); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, CurrentMaxLinearSpeed); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index bce4657..755cb62 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants.EnvironmentalConstants; /** * Physics sim implementation of module IO. @@ -26,7 +27,7 @@ * approximation for the behavior of the module. */ public class ModuleIOSim implements ModuleIO { - private static final double LOOP_PERIOD_SECS = 0.02; + private static final double LOOP_PERIOD_SECS = EnvironmentalConstants.loopPeriodMs; private DCMotorSim driveSim = new DCMotorSim(DCMotor.getNEO(1), 6.75, 0.025); private DCMotorSim turnSim = new DCMotorSim(DCMotor.getNEO(1), 150.0 / 7.0, 0.004); diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java index 84f311d..089166a 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.robot.Constants.EnvironmentalConstants; public class FlywheelIOSim implements FlywheelIO { // Note that this entire simulation is incorrect. @@ -34,7 +35,7 @@ public void updateInputs(FlywheelIOInputs inputs) { sim.setInputVoltage(appliedVolts); } - sim.update(0.02); + sim.update(EnvironmentalConstants.loopPeriodMs); inputs.positionRadTop = 0.0; inputs.velocityRadPerSecTop = sim.getAngularVelocityRadPerSec(); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 19f363b..3e6b678 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.robot.Constants.EnvironmentalConstants; public class IntakeIOSim implements IntakeIO { private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004); @@ -33,7 +34,7 @@ public void updateInputs(IntakeIOInputs inputs) { sim.setInputVoltage(appliedVolts); } - sim.update(0.02); + sim.update(EnvironmentalConstants.loopPeriodMs); inputs.positionRad = 0.0; inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec();