diff --git a/kls_database.db b/kls_database.db new file mode 100644 index 0000000..d5a20ba Binary files /dev/null and b/kls_database.db differ diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..42025c8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.1168032786885242, + "y": 4.180840163934427 + }, + "prevControl": null, + "nextControl": { + "x": 4.11680327868852, + "y": 4.180840163934427 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.873975409836065, + "y": 7.8610655737704915 + }, + "prevControl": { + "x": 4.873975409836065, + "y": 7.8610655737704915 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/cshcyberhawks/lib/hardware/AbsoluteDutyCycleEncoder.java b/src/main/java/cshcyberhawks/lib/hardware/AbsoluteDutyCycleEncoder.java new file mode 100644 index 0000000..e5a0d4b --- /dev/null +++ b/src/main/java/cshcyberhawks/lib/hardware/AbsoluteDutyCycleEncoder.java @@ -0,0 +1,323 @@ +package cshcyberhawks.lib.hardware; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.*; + +public class AbsoluteDutyCycleEncoder implements Sendable, AutoCloseable { + private final DutyCycle m_dutyCycle; + private boolean m_ownsDutyCycle; + private DigitalInput m_digitalInput; + private AnalogTrigger m_analogTrigger; + private Counter m_counter; + private int m_frequencyThreshold = 100; + private double m_positionOffset; + private double m_distancePerRotation = 1.0; + private double m_lastPosition; + private double m_sensorMin; + private double m_sensorMax = 1.0; + + private SimDevice m_simDevice; + private SimDouble m_simPosition; + private SimDouble m_simAbsolutePosition; + private SimDouble m_simDistancePerRotation; + private SimBoolean m_simIsConnected; + + /** + * Construct a new DutyCycleEncoder on a specific channel. + * + * @param channel the channel to attach to + */ + @SuppressWarnings("this-escape") + public AbsoluteDutyCycleEncoder(int channel) { + m_digitalInput = new DigitalInput(channel); + m_ownsDutyCycle = true; + m_dutyCycle = new DutyCycle(m_digitalInput); + init(); + } + + /** + * Construct a new DutyCycleEncoder attached to an existing DutyCycle object. + * + * @param dutyCycle the duty cycle to attach to + */ + @SuppressWarnings("this-escape") + public AbsoluteDutyCycleEncoder(DutyCycle dutyCycle) { + m_dutyCycle = dutyCycle; + init(); + } + + /** + * Construct a new DutyCycleEncoder attached to a DigitalSource object. + * + * @param source the digital source to attach to + */ + @SuppressWarnings("this-escape") + public AbsoluteDutyCycleEncoder(DigitalSource source) { + m_ownsDutyCycle = true; + m_dutyCycle = new DutyCycle(source); + init(); + } + + private void init() { + m_simDevice = SimDevice.create("DutyCycle:DutyCycleEncoder", m_dutyCycle.getSourceChannel()); + + if (m_simDevice != null) { + m_simPosition = m_simDevice.createDouble("position", SimDevice.Direction.kInput, 0.0); + m_simDistancePerRotation = + m_simDevice.createDouble("distance_per_rot", SimDevice.Direction.kOutput, 1.0); + m_simAbsolutePosition = + m_simDevice.createDouble("absPosition", SimDevice.Direction.kInput, 0.0); + m_simIsConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true); + } else { + m_counter = new Counter(); + m_analogTrigger = new AnalogTrigger(m_dutyCycle); + m_analogTrigger.setLimitsDutyCycle(0.25, 0.75); + m_counter.setUpSource(m_analogTrigger, AnalogTriggerOutput.AnalogTriggerType.kRisingPulse); + m_counter.setDownSource(m_analogTrigger, AnalogTriggerOutput.AnalogTriggerType.kFallingPulse); + } + + SendableRegistry.addLW(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel()); + } + + private double mapSensorRange(double pos) { + // map sensor range + if (pos < m_sensorMin) { + pos = m_sensorMin; + } + if (pos > m_sensorMax) { + pos = m_sensorMax; + } + pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin); + return pos; + } + + private boolean doubleEquals(double a, double b) { + double epsilon = 0.00001d; + return Math.abs(a - b) < epsilon; + } + + /** + * Get the encoder value since the last reset. + * + *

This is reported in rotations since the last reset. + * + * @return the encoder value in rotations + */ + public double get() { + if (m_simPosition != null) { + return m_simPosition.get(); + } + + // As the values are not atomic, keep trying until we get 2 reads of the same + // value + // If we don't within 10 attempts, error + for (int i = 0; i < 10; i++) { + double counter = m_counter.get(); + double pos = m_dutyCycle.getOutput(); + double counter2 = m_counter.get(); + double pos2 = m_dutyCycle.getOutput(); + if (counter == counter2 && doubleEquals(pos, pos2)) { + // map sensor range + pos = mapSensorRange(pos); + double position = counter + pos - m_positionOffset; + m_lastPosition = position; + return position; + } + } + + DriverStation.reportWarning( + "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); + return m_lastPosition; + } + + /** + * Get the absolute position of the duty cycle encoder. + * + *

getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative + * to the last reset. This could potentially be negative, which needs to be accounted for. + * + *

This will not account for rollovers, and will always be just the raw absolute position. + * + * @return the absolute position + */ + public double getAbsolutePosition() { + if (m_simAbsolutePosition != null) { + return m_simAbsolutePosition.get(); + } + + return mapSensorRange(m_dutyCycle.getOutput()); + } + + /** + * Get the offset of position relative to the last reset. + * + *

getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative + * to the last reset. This could potentially be negative, which needs to be accounted for. + * + * @return the position offset + */ + public double getPositionOffset() { + return m_positionOffset; + } + + /** + * Set the position offset. + * + *

This must be in the range of 0-1. + * + * @param offset the offset + */ + public void setPositionOffset(double offset) { + m_positionOffset = MathUtil.clamp(offset, 0.0, 1.0); + } + + /** + * Set the encoder duty cycle range. As the encoder needs to maintain a duty cycle, the duty cycle + * cannot go all the way to 0% or all the way to 100%. For example, an encoder with a 4096 us + * period might have a minimum duty cycle of 1 us / 4096 us and a maximum duty cycle of 4095 / + * 4096 us. Setting the range will result in an encoder duty cycle less than or equal to the + * minimum being output as 0 rotation, the duty cycle greater than or equal to the maximum being + * output as 1 rotation, and values in between linearly scaled from 0 to 1. + * + * @param min minimum duty cycle (0-1 range) + * @param max maximum duty cycle (0-1 range) + */ + public void setDutyCycleRange(double min, double max) { + m_sensorMin = MathUtil.clamp(min, 0.0, 1.0); + m_sensorMax = MathUtil.clamp(max, 0.0, 1.0); + } + + /** + * Set the distance per rotation of the encoder. This sets the multiplier used to determine the + * distance driven based on the rotation value from the encoder. Set this value based on how far + * the mechanism travels in 1 rotation of the encoder, and factor in gearing reductions following + * the encoder shaft. This distance can be in any units you like, linear or angular. + * + * @param distancePerRotation the distance per rotation of the encoder + */ + public void setDistancePerRotation(double distancePerRotation) { + m_distancePerRotation = distancePerRotation; + if (m_simDistancePerRotation != null) { + m_simDistancePerRotation.set(distancePerRotation); + } + } + + /** + * Get the distance per rotation for this encoder. + * + * @return The scale factor that will be used to convert rotation to useful units. + */ + public double getDistancePerRotation() { + return m_distancePerRotation; + } + + /** + * Get the distance the sensor has driven since the last reset as scaled by the value from {@link + * #setDistancePerRotation(double)}. + * + * @return The distance driven since the last reset + */ + public double getDistance() { + return get() * getDistancePerRotation(); + } + + /** + * Get the frequency in Hz of the duty cycle signal from the encoder. + * + * @return duty cycle frequency in Hz + */ + public int getFrequency() { + return m_dutyCycle.getFrequency(); + } + + /** Reset the Encoder distance to zero. */ + public void reset() { + if (m_counter != null) { + m_counter.reset(); + } + if (m_simPosition != null) { + m_simPosition.set(0); + } + m_positionOffset = getAbsolutePosition(); + } + + /** + * Get if the sensor is connected + * + *

This uses the duty cycle frequency to determine if the sensor is connected. By default, a + * value of 100 Hz is used as the threshold, and this value can be changed with {@link + * #setConnectedFrequencyThreshold(int)}. + * + * @return true if the sensor is connected + */ + public boolean isConnected() { + if (m_simIsConnected != null) { + return m_simIsConnected.get(); + } + return getFrequency() > m_frequencyThreshold; + } + + /** + * Change the frequency threshold for detecting connection used by {@link #isConnected()}. + * + * @param frequency the minimum frequency in Hz. + */ + public void setConnectedFrequencyThreshold(int frequency) { + if (frequency < 0) { + frequency = 0; + } + + m_frequencyThreshold = frequency; + } + + @Override + public void close() { + if (m_counter != null) { + m_counter.close(); + } + if (m_analogTrigger != null) { + m_analogTrigger.close(); + } + if (m_ownsDutyCycle) { + m_dutyCycle.close(); + } + if (m_digitalInput != null) { + m_digitalInput.close(); + } + if (m_simDevice != null) { + m_simDevice.close(); + } + } + + /** + * Get the FPGA index for the DutyCycleEncoder. + * + * @return the FPGA index + */ + public int getFPGAIndex() { + return m_dutyCycle.getFPGAIndex(); + } + + /** + * Get the channel of the source. + * + * @return the source channel + */ + public int getSourceChannel() { + return m_dutyCycle.getSourceChannel(); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("AbsoluteEncoder"); + builder.addDoubleProperty("Distance", this::getDistance, null); + builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null); + builder.addBooleanProperty("Is Connected", this::isConnected, null); + } +} diff --git a/src/main/java/frc/robot/math/MiscCalculations.kt b/src/main/java/cshcyberhawks/lib/math/MiscCalculations.kt similarity index 95% rename from src/main/java/frc/robot/math/MiscCalculations.kt rename to src/main/java/cshcyberhawks/lib/math/MiscCalculations.kt index 00fcefe..c1f713e 100644 --- a/src/main/java/frc/robot/math/MiscCalculations.kt +++ b/src/main/java/cshcyberhawks/lib/math/MiscCalculations.kt @@ -1,9 +1,9 @@ -package frc.robot.math +package cshcyberhawks.lib.math import edu.wpi.first.math.geometry.Translation2d -import edu.wpi.first.util.WPIUtilJNI import kotlin.math.PI import kotlin.math.abs +import kotlin.math.sign /** Miscellaneous calculations. */ object MiscCalculations { @@ -96,4 +96,6 @@ object MiscCalculations { fun positionToRotations(speed: Double, radius: Double, reduction: Double = 1.0): Double { return (speed / (2 * PI * radius)) * reduction } + + fun wrapAroundAngles(angle: Double) = ((abs(angle) % 360.0) * sign(angle) + 360.0) % 360.0 } diff --git a/src/main/java/frc/robot/AutoTargeting.kt b/src/main/java/frc/robot/AutoTargeting.kt new file mode 100644 index 0000000..bdbf14d --- /dev/null +++ b/src/main/java/frc/robot/AutoTargeting.kt @@ -0,0 +1,46 @@ +package frc.robot + +import com.pathplanner.lib.auto.AutoBuilder +import com.pathplanner.lib.path.PathConstraints +import edu.wpi.first.math.geometry.Pose2d +import edu.wpi.first.wpilibj2.command.Command +import frc.robot.constants.AutoScoringConstants +import frc.robot.subsystems.swerve.SwerveConstants + +object AutoTargeting { + val pathConstraint: PathConstraints = PathConstraints(SwerveConstants.maxAutoSpeed, SwerveConstants.maxAutoAccel, SwerveConstants.maxAutoTwist, SwerveConstants.maxAutoTwistAccel) + + val leftCoralPoses = arrayOf(AutoScoringConstants.CoralScoring.A.left, AutoScoringConstants.CoralScoring.B.left, AutoScoringConstants.CoralScoring.C.left, AutoScoringConstants.CoralScoring.D.left, AutoScoringConstants.CoralScoring.E.left, AutoScoringConstants.CoralScoring.F.left) + val rightCoralPoses = arrayOf(AutoScoringConstants.CoralScoring.A.right, AutoScoringConstants.CoralScoring.B.right, AutoScoringConstants.CoralScoring.C.right, AutoScoringConstants.CoralScoring.D.right, AutoScoringConstants.CoralScoring.E.right, AutoScoringConstants.CoralScoring.F.right) + val centerCoralPoses = arrayOf(AutoScoringConstants.CoralScoring.A.center, AutoScoringConstants.CoralScoring.B.center, AutoScoringConstants.CoralScoring.C.center, AutoScoringConstants.CoralScoring.D.center, AutoScoringConstants.CoralScoring.E.center, AutoScoringConstants.CoralScoring.F.center) + + + private fun goToPose(pose2d: Pose2d): Command { + return AutoBuilder.pathfindToPose(pose2d, pathConstraint, 0.0) + } + + fun autoToNearestCoral(targetSide: CoralSide, currentPose2d: Pose2d): Command { + var nearestCenter = currentPose2d.nearest(centerCoralPoses.toList()) + var nearestCenterIdx = centerCoralPoses.indexOf(nearestCenter) + + var targetPose: Pose2d; + + if (targetSide == CoralSide.Left) { + targetPose = leftCoralPoses[nearestCenterIdx] + } + else { + targetPose = rightCoralPoses[nearestCenterIdx] + } + + return goToPose(targetPose) + } + + enum class CoralSide { + Left, + Right + } + + enum class CoralLevel { + L2, L3, L4 + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.kt b/src/main/java/frc/robot/Robot.kt index 7b9ba55..79eb790 100644 --- a/src/main/java/frc/robot/Robot.kt +++ b/src/main/java/frc/robot/Robot.kt @@ -1,30 +1,20 @@ package frc.robot +import au.grapplerobotics.CanBridge import edu.wpi.first.hal.FRCNetComm.tInstances import edu.wpi.first.hal.FRCNetComm.tResourceType import edu.wpi.first.hal.HAL -import edu.wpi.first.math.MathUtil -import edu.wpi.first.math.geometry.Pose2d -import edu.wpi.first.math.geometry.Pose3d -import edu.wpi.first.math.geometry.Rotation3d -import edu.wpi.first.math.geometry.Translation3d -import edu.wpi.first.math.util.Units -import edu.wpi.first.networktables.NetworkTableInstance -import edu.wpi.first.wpilibj.RobotController import edu.wpi.first.wpilibj.TimedRobot -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard import edu.wpi.first.wpilibj.smartdashboard.SendableChooser import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj.util.WPILibVersion import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.CommandScheduler import edu.wpi.first.wpilibj2.command.Commands -import frc.robot.RobotContainer.leftJoystick -import frc.robot.RobotContainer.vision +import frc.robot.commands.TeleopDriveCommand import frc.robot.subsystems.superstructure.Superstructure import frc.robot.util.Visualizer import frc.robot.util.input.DriverAction -import org.xml.sax.SAXNotSupportedException /** * The VM is configured to automatically run this object (which basically functions as a singleton class), @@ -42,8 +32,12 @@ object Robot : TimedRobot() { * the [autonomousInit] method will set it to the value selected in *the AutoChooser on the dashboard. */ -// private var autonomousCommand: Command = Commands.runOnce({}) - private var autonomousCommand: Command = Commands.runOnce({ Superstructure.scoreL4() }) + private var autonomousCommand: Command = Commands.runOnce({}) +// private var autonomousCommand: Command = Commands.sequence( +// Commands.run({ RobotContainer.drivetrain.applyDriveRequest(-1.0, 0.0, 0.0) }).raceWith(Commands.waitSeconds(1.0)), +// Commands.runOnce({ RobotContainer.drivetrain.applyDriveRequest(0.0, 0.0, 0.0) }), +// ) +// private var autonomousCommand = Commands.run({ RobotContainer.drivetrain.applyDriveRequest(-1.0, 0.0, 0.0) }) // val elevatorPosePublisher = // NetworkTableInstance.getDefault().getStructTopic("Elevator Pose", Pose3d.struct).publish(); @@ -51,18 +45,14 @@ object Robot : TimedRobot() { // val wristPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Wrist Pose", Pose3d.struct).publish(); // - private val actionChooser = SendableChooser() init { - for (action in DriverAction.entries) { - actionChooser.addOption(action.name, action) - } - - SmartDashboard.putData(actionChooser) SmartDashboard.putBoolean("Action", false) SmartDashboard.putBoolean("Confirm", false) SmartDashboard.putBoolean("Cancel", false) + + CanBridge.runTCP() } /** @@ -75,6 +65,8 @@ object Robot : TimedRobot() { // Access the RobotContainer object so that it is initialized. This will perform all our // button bindings, and put our autonomous chooser on the dashboard. + SmartDashboard.putBoolean("full reset with vision", false) + RobotContainer } @@ -92,6 +84,8 @@ object Robot : TimedRobot() { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run() +// SmartDashboard.putNumber("robot pose x: ", RobotContainer.drivetrain.getSwervePose().x) + Visualizer.periodic() } @@ -101,7 +95,7 @@ object Robot : TimedRobot() { } override fun disabledPeriodic() { -// vision.updateOdometryFromDisabled() +// RobotContainer.vision.updateOdometryFromDisabled() } /** This autonomous runs the autonomous command selected by your [RobotContainer] class. */ @@ -110,6 +104,9 @@ object Robot : TimedRobot() { // is modified while the command is running since we need to access it again in teleopInit() autonomousCommand.schedule() + + + Superstructure.initialize() } @@ -122,31 +119,13 @@ object Robot : TimedRobot() { // autonomous to continue until interrupted by another command, remove this line or comment it out. autonomousCommand.cancel() +// TeleopDriveCommand().schedule() Superstructure.initialize() } /** This method is called periodically during operator control. */ override fun teleopPeriodic() { -// vision.updateOdometry(1, false) - - if (SmartDashboard.getBoolean("Action", false)) { - actionChooser.selected.cmd.schedule() - println("Scheduled action") - - SmartDashboard.putBoolean("Action", false) - } - - if (SmartDashboard.getBoolean("Confirm", false)) { - RobotState.actionConfirmed = true - SmartDashboard.putBoolean("Confirm", false) - } - - - if (SmartDashboard.getBoolean("Cancel", false)) { - println("Cancelled") - RobotState.actionCancelled = true - SmartDashboard.putBoolean("Cancel", false) - } +// RobotContainer.vision.updateOdometry(1, false) } override fun testInit() { @@ -160,7 +139,6 @@ object Robot : TimedRobot() { } // var lastLoopTime = 0.0 - /** This method is called once when the robot is first started up. */ override fun simulationInit() { // lastLoopTime = MiscCalculations.getCurrentTime() diff --git a/src/main/java/frc/robot/RobotConfiguration.kt b/src/main/java/frc/robot/RobotConfiguration.kt index 52d9938..61aa1ec 100644 --- a/src/main/java/frc/robot/RobotConfiguration.kt +++ b/src/main/java/frc/robot/RobotConfiguration.kt @@ -12,7 +12,8 @@ enum class OperatorType { } object RobotConfiguration { - val robotType = RobotType.Simulated + val robotType = RobotType.Real val operatorType = OperatorType.Manual -} \ No newline at end of file +} + diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt index 470cf36..fe1994e 100644 --- a/src/main/java/frc/robot/RobotContainer.kt +++ b/src/main/java/frc/robot/RobotContainer.kt @@ -1,5 +1,6 @@ package frc.robot +import com.fasterxml.jackson.databind.JsonSerializer.None import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.button.CommandJoystick @@ -18,7 +19,9 @@ import frc.robot.util.VisionSystem import com.pathplanner.lib.auto.NamedCommands import edu.wpi.first.wpilibj.GenericHID import edu.wpi.first.wpilibj2.command.button.CommandGenericHID +import frc.robot.util.input.ManualDriverInput import frc.robot.util.input.TestingOperatorInput +import java.util.* object RobotContainer { val leftJoystick: CommandJoystick = CommandJoystick(0) @@ -39,10 +42,13 @@ object RobotContainer { val teleopDriveCommand = when (RobotConfiguration.robotType) { RobotType.Real -> TeleopDriveCommand() +// RobotType.Real -> Commands.run({}) RobotType.Simulated -> SimTeleopDriveCommand() RobotType.Empty -> Commands.run({}) } + var currentDriveCommand: Optional = Optional.empty(); + init { configureBindings() @@ -53,7 +59,7 @@ object RobotContainer { } private fun configureBindings() { - teleopDriveCommand.addRequirements(drivetrain) +// teleopDriveCommand.addRequirements(drivetrain) drivetrain.setDefaultCommand(teleopDriveCommand) // We might need this? @@ -64,6 +70,10 @@ object RobotContainer { OperatorType.Manual -> ManualOperatorInput.configureBindings() OperatorType.Testing -> TestingOperatorInput.configureBindings() } + + ManualDriverInput.configureBindings() + + ManualOperatorInput.configureBindings() } val autonomousCommand: Command diff --git a/src/main/java/frc/robot/RobotState.kt b/src/main/java/frc/robot/RobotState.kt index 0b3db05..f1be27f 100644 --- a/src/main/java/frc/robot/RobotState.kt +++ b/src/main/java/frc/robot/RobotState.kt @@ -8,4 +8,6 @@ object RobotState { var actionCancelled = false var actionConfirmed = false + + var autoDriving = false } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/SimTeleopDriveCommand.kt b/src/main/java/frc/robot/commands/SimTeleopDriveCommand.kt index f1e20d9..ba8569c 100644 --- a/src/main/java/frc/robot/commands/SimTeleopDriveCommand.kt +++ b/src/main/java/frc/robot/commands/SimTeleopDriveCommand.kt @@ -4,8 +4,7 @@ import edu.wpi.first.math.MathUtil import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Command import frc.robot.RobotContainer -import frc.robot.math.MiscCalculations -import frc.robot.subsystems.ExampleSubsystem.runOnce +import cshcyberhawks.lib.math.MiscCalculations import frc.robot.subsystems.swerve.SwerveConstants diff --git a/src/main/java/frc/robot/commands/TeleopAutoScore.kt b/src/main/java/frc/robot/commands/TeleopAutoScore.kt new file mode 100644 index 0000000..7ad1fb3 --- /dev/null +++ b/src/main/java/frc/robot/commands/TeleopAutoScore.kt @@ -0,0 +1,21 @@ +package frc.robot.commands + +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands +import frc.robot.AutoTargeting +import frc.robot.RobotContainer +import frc.robot.subsystems.superstructure.Superstructure +import frc.robot.util.input.OperatorControls +import java.util.* + +object TeleopAutoScore { + fun score(): Command { + val driveCommand = AutoTargeting.autoToNearestCoral(OperatorControls.coralSideChooser.selected, RobotContainer.drivetrain.getSwervePose()) + + RobotContainer.currentDriveCommand = Optional.of(driveCommand); + + return Commands.parallel( + driveCommand + ) + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/TeleopDriveCommand.kt b/src/main/java/frc/robot/commands/TeleopDriveCommand.kt index d639757..9c68ad9 100644 --- a/src/main/java/frc/robot/commands/TeleopDriveCommand.kt +++ b/src/main/java/frc/robot/commands/TeleopDriveCommand.kt @@ -4,24 +4,29 @@ import edu.wpi.first.math.MathUtil import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Command import frc.robot.RobotContainer -import frc.robot.math.MiscCalculations +import cshcyberhawks.lib.math.MiscCalculations +import edu.wpi.first.wpilibj2.command.Commands +import frc.robot.RobotState import frc.robot.subsystems.ExampleSubsystem.runOnce import frc.robot.subsystems.swerve.SwerveConstants class TeleopDriveCommand : Command() { init { -// addRequirements(RobotContainer.drivetrain) + addRequirements(RobotContainer.drivetrain) } override fun initialize() { - RobotContainer.rightJoystick.button(2) - .onTrue(runOnce { RobotContainer.drivetrain.seedFieldCentric() }) super.initialize() } override fun execute() { + +// println("Execute") + + if (RobotState.autoDriving) return + val deadzonedLeftY = MiscCalculations.calculateDeadzone(RobotContainer.leftJoystick.y, .5) SwerveConstants.ControlledSpeed = MathUtil.clamp( @@ -37,9 +42,14 @@ class TeleopDriveCommand : Command() { SmartDashboard.putNumber("ControlledAngularRate", SwerveConstants.ControlledAngularRate) - val fieldOriented = !RobotContainer.rightJoystick.button(2).asBoolean + val fieldOriented = !RobotContainer.rightJoystick.button(1).asBoolean if (fieldOriented) { + SmartDashboard.putNumber("drive x req", -MiscCalculations.calculateDeadzone( + RobotContainer.rightJoystick.y, + .1 + ) * SwerveConstants.ControlledSpeed) + RobotContainer.drivetrain.applyDriveRequest( -MiscCalculations.calculateDeadzone( diff --git a/src/main/java/frc/robot/constants/AutoScoringConstants.kt b/src/main/java/frc/robot/constants/AutoScoringConstants.kt new file mode 100644 index 0000000..e157405 --- /dev/null +++ b/src/main/java/frc/robot/constants/AutoScoringConstants.kt @@ -0,0 +1,62 @@ +package frc.robot.constants + +import edu.wpi.first.math.geometry.Pose2d + +object AutoScoringConstants { + object CoralScoring { + object A { + val left: Pose2d = Pose2d(/*3.117, */); + val right: Pose2d = Pose2d(); + val center: Pose2d = Pose2d(); + } + + object B { + val left: Pose2d = Pose2d(); + val right: Pose2d = Pose2d(); + val center: Pose2d = Pose2d(); + + } + + object C { + val left: Pose2d = Pose2d(); + val right: Pose2d = Pose2d(); + val center: Pose2d = Pose2d(); + + } + + object D { + val left: Pose2d = Pose2d(); + val right: Pose2d = Pose2d(); + val center: Pose2d = Pose2d(); + + } + + object E { + val left: Pose2d = Pose2d(); + val right: Pose2d = Pose2d(); + val center: Pose2d = Pose2d(); + + } + + object F { + val left: Pose2d = Pose2d(); + val right: Pose2d = Pose2d(); + val center: Pose2d = Pose2d(); + + } + } + + object AlgaePickup { + val A: Pose2d = Pose2d(); + + val B: Pose2d = Pose2d(); + + val C: Pose2d = Pose2d(); + + val D: Pose2d = Pose2d(); + + val E: Pose2d = Pose2d(); + + val F: Pose2d = Pose2d(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/CANConstants.kt b/src/main/java/frc/robot/constants/CANConstants.kt index e21995a..01018c0 100644 --- a/src/main/java/frc/robot/constants/CANConstants.kt +++ b/src/main/java/frc/robot/constants/CANConstants.kt @@ -10,13 +10,15 @@ object CANConstants { object Pivot { const val motorId = 30 - const val encoderId = 0 // Roborio DIO not CAN + const val encoderId = 5 // Roborio DIO not CAN } object Intake { const val motorId = 40 + const val coralLaserCANId = 41 + const val algaeLaserCANId = 42 } // Climb -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt index f3dc1ae..0cdd864 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt @@ -2,50 +2,49 @@ package frc.robot.subsystems.superstructure import cshcyberhawks.lib.requests.* import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard -import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Commands -import edu.wpi.first.wpilibj2.command.ConditionalCommand -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import edu.wpi.first.wpilibj2.command.SubsystemBase import frc.robot.RobotConfiguration import frc.robot.RobotState import frc.robot.RobotType import frc.robot.subsystems.superstructure.elevator.ElevatorSystem import frc.robot.subsystems.superstructure.elevator.implementation.ElevatorIOEmpty -import frc.robot.subsystems.superstructure.elevator.implementation.ElevatorIOReal +import frc.robot.subsystems.superstructure.elevator.implementation.ElevatorIOPID import frc.robot.subsystems.superstructure.elevator.implementation.ElevatorIOSim import frc.robot.subsystems.superstructure.intake.GamePieceState -import frc.robot.subsystems.superstructure.intake.implementation.IntakeIOEmpty -import frc.robot.subsystems.superstructure.pivot.PivotSystem import frc.robot.subsystems.superstructure.intake.IntakeSystem +import frc.robot.subsystems.superstructure.intake.implementation.IntakeIOEmpty import frc.robot.subsystems.superstructure.intake.implementation.IntakeIOReal +import frc.robot.subsystems.superstructure.pivot.PivotSystem import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOEmpty -import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOReal +import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOPID import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOSim import java.util.Optional object Superstructure : SubsystemBase() { - val pivotSystem = PivotSystem( - when (RobotConfiguration.robotType) { - RobotType.Real -> PivotIOReal() - RobotType.Simulated -> PivotIOSim() - RobotType.Empty -> PivotIOEmpty() - } - ) - val elevatorSystem = ElevatorSystem( - when (RobotConfiguration.robotType) { - RobotType.Real -> ElevatorIOReal() - RobotType.Simulated -> ElevatorIOSim() - RobotType.Empty -> ElevatorIOEmpty() - } - ) - val intakeSystem = IntakeSystem( - when (RobotConfiguration.robotType) { - RobotType.Real -> IntakeIOReal() - RobotType.Simulated -> IntakeIOEmpty() - RobotType.Empty -> IntakeIOEmpty() - } - ) + val pivotSystem = + PivotSystem( + when (RobotConfiguration.robotType) { + RobotType.Real -> PivotIOPID() + RobotType.Simulated -> PivotIOSim() + RobotType.Empty -> PivotIOEmpty() + } + ) + val elevatorSystem = + ElevatorSystem( + when (RobotConfiguration.robotType) { + RobotType.Real -> ElevatorIOPID() + RobotType.Simulated -> ElevatorIOSim() + RobotType.Empty -> ElevatorIOEmpty() + } + ) + val intakeSystem = + IntakeSystem( + when (RobotConfiguration.robotType) { + RobotType.Real -> IntakeIOReal() + RobotType.Simulated -> IntakeIOEmpty() + RobotType.Empty -> IntakeIOEmpty() + } + ) // Requests system private var activeRequest: Optional = Optional.empty() @@ -76,23 +75,23 @@ object Superstructure : SubsystemBase() { // Should be called when robot is enabled (teleop and auto) fun initialize() { - clearRequestQueue(); + clearRequestQueue() } override fun periodic() { if (hasNewRequest && activeRequest.isPresent) { activeRequest.get().execute() - hasNewRequest = false; + hasNewRequest = false } if (activeRequest.isEmpty) { if (queuedRequests.isEmpty()) { - allRequestsComplete = true; + allRequestsComplete = true } else { - request(queuedRequests.removeAt(0)); + request(queuedRequests.removeAt(0)) } } else if (activeRequest.get().isFinished()) { - activeRequest = Optional.empty(); + activeRequest = Optional.empty() } SmartDashboard.putBoolean("Has new request", hasNewRequest) @@ -100,119 +99,134 @@ object Superstructure : SubsystemBase() { } private fun awaitAtDesiredPosition() = - ParallelRequest(elevatorSystem.awaitDesiredPosition(), pivotSystem.awaitDesiredAngle()) - - private fun stowRequest() = ParallelRequest( - elevatorSystem.stowPosition(), - pivotSystem.stowAngle() - ) - - - private fun safeRetractRequest() = SequentialRequest( - ParallelRequest( - pivotSystem.travelAngle(), - elevatorSystem.safeDownPosition() - ), - elevatorSystem.stowPosition().withPrerequisite(pivotSystem.safeTravelDown()), - pivotSystem.stowAngle().withPrerequisite(elevatorSystem.belowSafeUpPosition()) - ) - - fun stow() = request( - stowRequest() - ) + ParallelRequest(elevatorSystem.awaitDesiredPosition(), pivotSystem.awaitDesiredAngle()) - fun intakeFeeder() = request( - SuperstructureAction.create( - ParallelRequest( - elevatorSystem.feederPosition(), - pivotSystem.feederAngle(), - intakeSystem.coralIntake() - ), - EmptyRequest(), - ParallelRequest( - stowRequest() - ), - confirmed = { RobotState.gamePieceState == GamePieceState.Coral } - ) - ) + private fun stowRequest() = + ParallelRequest(elevatorSystem.stowPosition(), pivotSystem.stowAngle()) - fun scoreL2() = request( - SuperstructureAction.create( - ParallelRequest( - pivotSystem.l2Angle(), - elevatorSystem.stowPosition() - ), - intakeSystem.coralScore(), - stowRequest() - ) - ) - - fun scoreL3() = request( - SuperstructureAction.create( - ParallelRequest( - pivotSystem.l3Angle(), - elevatorSystem.l3Position() - ), - intakeSystem.coralScore(), - stowRequest() - ) - ) - - fun scoreL4() = request( - SuperstructureAction.create( + private fun safeRetractRequest() = SequentialRequest( - ParallelRequest( - pivotSystem.l4Angle(), - elevatorSystem.safeUpPosition() - ), - elevatorSystem.l4Position().withPrerequisite(pivotSystem.safeTravelUp()), - ), intakeSystem.coralScore(), safeRetractRequest() - ) - ) - - fun removeAlgaeLow() = request( + ParallelRequest(pivotSystem.travelAngle(), elevatorSystem.safeDownPosition()), + WaitRequest(0.1), + ParallelRequest(elevatorSystem.stowPosition().withPrerequisite(pivotSystem.safeTravelDown()), + pivotSystem.stowAngle().withPrerequisite(elevatorSystem.belowSafeUpPosition())) + ) + + fun stow() = request(stowRequest()) + + fun intakeFeeder() = + request( + SuperstructureAction.create( + ParallelRequest( + elevatorSystem.feederPosition(), + pivotSystem.feederAngle(), + intakeSystem.coralIntake() + ), + EmptyRequest(), + ParallelRequest(stowRequest(), intakeSystem.idle()), + confirmed = { RobotState.gamePieceState == GamePieceState.Coral } + ) + ) + + fun scoreL2() = + request( + SuperstructureAction.create( + ParallelRequest(pivotSystem.l2Angle(), elevatorSystem.stowPosition()), + intakeSystem.coralScore(), + stowRequest(), + forceManualRetract = true + ) + ) + + fun scoreL3() = + request( + SuperstructureAction.create( + ParallelRequest(pivotSystem.l3Angle(), elevatorSystem.l3Position()), + intakeSystem.coralScore(), + stowRequest(), + forceManualRetract = true + ) + ) + + fun scoreL4() = + request( + SuperstructureAction.create( + SequentialRequest( + ParallelRequest( + pivotSystem.l4Angle(), + elevatorSystem.safeUpPosition() + ), + elevatorSystem + .l4Position() + .withPrerequisite(pivotSystem.safeTravelUp()), + ), + intakeSystem.coralScore(), + safeRetractRequest(), + forceManualRetract = true + ) + ) + + fun removeAlgaeLow() = + request( + SuperstructureAction.create( + ParallelRequest( + pivotSystem.algaeRemoveAngle(), + elevatorSystem.algaeRemoveLowPosition(), + intakeSystem.algaeIntake() + ), + EmptyRequest(), + SequentialRequest( + ParallelRequest(pivotSystem.stowAngle(), elevatorSystem.stowPosition()), + intakeSystem.idle() + ), + { RobotState.gamePieceState == GamePieceState.Algae }, + ) + ) + + fun removeAlgaeHigh() = + request( + SuperstructureAction.create( + SequentialRequest( + ParallelRequest( + pivotSystem.algaeRemoveAngle(), + elevatorSystem.safeUpPosition(), + ), + elevatorSystem + .algaeRemoveHighPosition() + .withPrerequisite(pivotSystem.safeTravelUp()), + intakeSystem.algaeIntake() + ), + EmptyRequest(), + SequentialRequest( + safeRetractRequest(), + intakeSystem.idle() + ), + { RobotState.gamePieceState == GamePieceState.Algae } + ) + ) + + fun scoreProcessor() = request( SuperstructureAction.create( - ParallelRequest( - pivotSystem.algaeRemoveAngle(), - elevatorSystem.algaeRemoveLowPosition(), - intakeSystem.algaeIntake() - ), - EmptyRequest(), - ParallelRequest( - pivotSystem.stowAngle(), - elevatorSystem.stowPosition() - ), - { RobotState.gamePieceState == GamePieceState.Algae }, - ) - ) - - fun removeAlgaeHigh() = request( - SuperstructureAction.create( - SequentialRequest( - ParallelRequest( - pivotSystem.algaeRemoveAngle(), - elevatorSystem.safeUpPosition(), - ), - elevatorSystem.algaeRemoveHighPosition().withPrerequisite(pivotSystem.safeTravelUp()), - intakeSystem.algaeIntake() - ), - EmptyRequest(), - safeRetractRequest(), - { RobotState.gamePieceState == GamePieceState.Algae } - ) - ) - - fun scoreBarge() = request( - SuperstructureAction.create( - SequentialRequest( - ParallelRequest( - pivotSystem.bargeAngle(), - elevatorSystem.safeUpPosition(), - ), - elevatorSystem.bargePosition().withPrerequisite(pivotSystem.safeTravelUp()), - ), + ParallelRequest(elevatorSystem.processorPosition(), pivotSystem.processorAngle()), intakeSystem.algaeScore(), - safeRetractRequest() + stowRequest() ) ) -} \ No newline at end of file + + fun scoreBarge() = + request( + SuperstructureAction.create( + SequentialRequest( + ParallelRequest( + pivotSystem.bargeAngle(), + elevatorSystem.safeUpPosition(), + ), + elevatorSystem + .bargePosition() + .withPrerequisite(pivotSystem.safeTravelUp()), + ), + intakeSystem.algaeScore(), + safeRetractRequest() + ) + ) +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureAction.kt b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureAction.kt index ad8f418..d1bd9ed 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureAction.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureAction.kt @@ -9,7 +9,8 @@ object SuperstructureAction { confirmAction: Request, returnAction: Request, confirmed: () -> Boolean = { RobotState.actionConfirmed }, - cancelled: () -> Boolean = { RobotState.actionCancelled } + cancelled: () -> Boolean = { RobotState.actionCancelled }, + forceManualRetract: Boolean = false ): Request = SequentialRequest( Request.withAction { RobotState.actionCancelled = false @@ -20,6 +21,9 @@ object SuperstructureAction { confirmed() || cancelled() }, IfRequest(confirmed, confirmAction), + IfRequest({ forceManualRetract }, AwaitRequest { + cancelled() + }), returnAction ) } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt index fc26912..d6291cd 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt @@ -1,14 +1,18 @@ package frc.robot.subsystems.superstructure.elevator import edu.wpi.first.math.trajectory.TrapezoidProfile +import edu.wpi.first.math.util.Units object ElevatorConstants { - const val velocityInches = 26.5 // Guessed values for now + const val velocityInches = 40.0 // Guessed values for now const val accelationInches = 30.0 // Conversion so one (I guess its mechanism rotation) is 1 inch on the elevator const val conversionFactor = - 20.0 * 1.751 + 20.0 * 1.751 * Math.PI / 30.0 const val positionTolerance = 0.1 -} \ No newline at end of file + + // The torque to hold the elevator against gravity (Newton-Meters) + val kGNM = Units.inchesToMeters(1.751 / 2.0) * (25.6 * 4.4482216153) +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorIO.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorIO.kt index 0bbb41e..e67df67 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorIO.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorIO.kt @@ -2,6 +2,7 @@ package frc.robot.subsystems.superstructure.elevator import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.NeutralModeValue interface ElevatorIO { fun getPosition(): Double diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt index 9bdb822..67d2e34 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt @@ -24,7 +24,7 @@ class ElevatorSystem(private val io: ElevatorIO) : SubsystemBase() { fun awaitDesiredPosition() = AwaitRequest { atDesiredPosition() } - fun belowSafeUpPosition() = Prerequisite.withCondition { !aboveSafeUpPosition() } + fun belowSafeUpPosition() = Prerequisite.withCondition { getPosition() < safeUpPosition } private fun setPosition(positionInches: Double) = Request.withAction { io.setPosition(positionInches) @@ -38,15 +38,17 @@ class ElevatorSystem(private val io: ElevatorIO) : SubsystemBase() { fun feederPosition() = setPosition(6.5) - fun stowPosition() = setPosition(0.0) + fun stowPosition() = setPosition(1.0) fun safeUpPosition() = setPosition(safeUpPosition) fun safeDownPosition() = setPosition(safeDownPosition) - fun l3Position() = setPosition(2.5) - fun l4Position() = setPosition(30.0) + fun l3Position() = setPosition(4.0) + fun l4Position() = setPosition(29.5) // Should be 30 eventually but not safe right now fun algaeRemoveLowPosition() = setPosition(1.0) - fun algaeRemoveHighPosition() = setPosition(17.5) - fun bargePosition() = setPosition(30.0) + fun algaeRemoveHighPosition() = setPosition(18.0) + + fun processorPosition() = setPosition(1.0) + fun bargePosition() = setPosition(29.0) // Should be 30 eventually but not safe right now } diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOPID.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOPID.kt new file mode 100644 index 0000000..7558cca --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOPID.kt @@ -0,0 +1,139 @@ +package frc.robot.subsystems.superstructure.elevator.implementation + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.TorqueCurrentFOC +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.InvertedValue +import com.ctre.phoenix6.signals.NeutralModeValue +import edu.wpi.first.math.controller.ProfiledPIDController +import edu.wpi.first.math.trajectory.TrapezoidProfile +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import frc.robot.constants.CANConstants +import cshcyberhawks.lib.math.MiscCalculations +import frc.robot.subsystems.superstructure.elevator.ElevatorConstants +import frc.robot.subsystems.superstructure.elevator.ElevatorIO + +class ElevatorIOPID() : ElevatorIO { + private val rightMotor = TalonFX(CANConstants.Elevator.rightMotorId) + private val leftMotor = TalonFX(CANConstants.Elevator.leftMotorId) + + val elevatorPIDController = + ProfiledPIDController( + 11.5, + 0.0, + 0.6, + TrapezoidProfile.Constraints( + ElevatorConstants.velocityInches, + ElevatorConstants.accelationInches + ) + ) + + private var rightMotorConfiguration = TalonFXConfiguration() + private var leftMotorConfiguration = TalonFXConfiguration() + + private val torqueRequest = TorqueCurrentFOC(0.0) + + private var desiredPosition = 0.0 + + // private var currentNeutralMode = NeutralModeValue.Coast + + private var neutralCoast = false + + init { + val feedBackConfigs = rightMotorConfiguration.Feedback + + // Converts 1 rotation to 1 in on the elevator + feedBackConfigs.SensorToMechanismRatio = ElevatorConstants.conversionFactor + + val slot0Configs = rightMotorConfiguration.Slot0 + + // TODO: These values need to be changed + // slot0Configs.kS = 0.2; // Add 0.25 V output to overcome static friction + // slot0Configs.kV = 0.43; // A velocity target of 1 rps results in 0.12 V output + // slot0Configs.kA = 0.1; // An acceleration of 1 rps/s requires 0.01 V output + slot0Configs.kP = 0.0 // A position error of 2.5 rotations results in 12 V output + slot0Configs.kI = 0.0 // no output for integrated error + slot0Configs.kD = 0.0 // A velocity error of 1 rps results in 0.1 V output + + // set Motion Magic settings + // val motionMagicConfigs = rightMotorConfiguration.MotionMagic; + // motionMagicConfigs.MotionMagicCruiseVelocity = ElevatorConstants.velocityInches// + // Target cruise velocity in rps + // motionMagicConfigs.MotionMagicAcceleration = ElevatorConstants.accelationInches // + // Target acceleration in rps/s + // motionMagicConfigs.MotionMagicJerk = 1600; Optional // Target jerk of 1600 rps/s/s + // (0.1 seconds) + rightMotorConfiguration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive + leftMotorConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + + val rightCurrentConfigs = rightMotorConfiguration.CurrentLimits + rightCurrentConfigs.StatorCurrentLimitEnable = true + rightCurrentConfigs.StatorCurrentLimit = 60.0 + + val leftCurrentConfigs = leftMotorConfiguration.CurrentLimits + leftCurrentConfigs.StatorCurrentLimitEnable = true + leftCurrentConfigs.StatorCurrentLimit = 60.0 + + rightMotor.configurator.apply(rightMotorConfiguration) + leftMotor.configurator.apply(leftMotorConfiguration) + + SmartDashboard.putBoolean("Elevator coast", false) + + rightMotor.setNeutralMode( + if (neutralCoast) NeutralModeValue.Coast else NeutralModeValue.Brake + ) + leftMotor.setNeutralMode( + if (neutralCoast) NeutralModeValue.Coast else NeutralModeValue.Brake + ) + + // leftMotor.setControl(Follower(rightMotor.deviceID, true)) + + rightMotor.setPosition(0.0) + } + + override fun getPosition(): Double { + return rightMotor.position.valueAsDouble + } + + override fun atDesiredPosition(): Boolean = + MiscCalculations.appxEqual( + getPosition(), + desiredPosition, + ElevatorConstants.positionTolerance + ) + + override fun setPosition(positionInches: Double) { + desiredPosition = positionInches + // rightMotor.setControl( + // motionMagic.withPosition(positionInches) + // ) + elevatorPIDController.goal = TrapezoidProfile.State(positionInches, 0.0) + } + + override fun periodic() { + SmartDashboard.putNumber("Elevator Desired Position", desiredPosition) + + val gravityFF = 3.85 + val positionPIDOut = elevatorPIDController.calculate(getPosition()) + + SmartDashboard.putNumber("Elevator Position Error", elevatorPIDController.positionError) + + val motorSet = positionPIDOut + gravityFF + + SmartDashboard.putNumber("Elevator Output", motorSet) + + val sdCoast = SmartDashboard.getBoolean("Elevator coast", false) + if (sdCoast != neutralCoast) { + neutralCoast = sdCoast + rightMotor.setNeutralMode( + if (neutralCoast) NeutralModeValue.Coast else NeutralModeValue.Brake + ) + leftMotor.setNeutralMode( + if (neutralCoast) NeutralModeValue.Coast else NeutralModeValue.Brake + ) + } + + rightMotor.setControl(torqueRequest.withOutput(motorSet)) + leftMotor.setControl(torqueRequest.withOutput(motorSet)) + } +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOReal.kt index 4550425..915195a 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOReal.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOReal.kt @@ -5,8 +5,9 @@ import com.ctre.phoenix6.controls.Follower import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.InvertedValue +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import frc.robot.constants.CANConstants -import frc.robot.math.MiscCalculations +import cshcyberhawks.lib.math.MiscCalculations import frc.robot.subsystems.superstructure.elevator.ElevatorConstants import frc.robot.subsystems.superstructure.elevator.ElevatorIO @@ -15,8 +16,9 @@ class ElevatorIOReal() : ElevatorIO { private val leftMotor = TalonFX(CANConstants.Elevator.leftMotorId) private var rightMotorConfiguration = TalonFXConfiguration() + private var leftMotorConfiguration = TalonFXConfiguration() - private val motionMagic = MotionMagicTorqueCurrentFOC(0.0) + private val motionMagic = MotionMagicTorqueCurrentFOC(0.0).withFeedForward(1.0 * rightMotor.motorKT.valueAsDouble) private var desiredPosition = 0.0 @@ -30,12 +32,13 @@ class ElevatorIOReal() : ElevatorIO { val slot0Configs = rightMotorConfiguration.Slot0; // TODO: These values need to be changed - slot0Configs.kS = 0.25; // Add 0.25 V output to overcome static friction - slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output - slot0Configs.kA = 0.01; // An acceleration of 1 rps/s requires 0.01 V output - slot0Configs.kP = 4.8; // A position error of 2.5 rotations results in 12 V output + slot0Configs.kS = 0.2; // Add 0.25 V output to overcome static friction + slot0Configs.kV = 0.43; // A velocity target of 1 rps results in 0.12 V output + slot0Configs.kA = 0.1; // An acceleration of 1 rps/s requires 0.01 V output + slot0Configs.kG = 0.01; + slot0Configs.kP = 0.01; // A position error of 2.5 rotations results in 12 V output slot0Configs.kI = 0.0; // no output for integrated error - slot0Configs.kD = 0.1; // A velocity error of 1 rps results in 0.1 V output + slot0Configs.kD = 0.0; // A velocity error of 1 rps results in 0.1 V output // set Motion Magic settings val motionMagicConfigs = rightMotorConfiguration.MotionMagic; @@ -44,9 +47,20 @@ class ElevatorIOReal() : ElevatorIO { // motionMagicConfigs.MotionMagicJerk = 1600; Optional // Target jerk of 1600 rps/s/s (0.1 seconds) rightMotorConfiguration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive + val rightCurrentConfigs = rightMotorConfiguration.CurrentLimits + rightCurrentConfigs.StatorCurrentLimitEnable = true + rightCurrentConfigs.StatorCurrentLimit = 60.0 + + val leftCurrentConfigs = leftMotorConfiguration.CurrentLimits + leftCurrentConfigs.StatorCurrentLimitEnable = true + leftCurrentConfigs.StatorCurrentLimit = 60.0 + rightMotor.configurator.apply(rightMotorConfiguration); + leftMotor.configurator.apply(leftMotorConfiguration) leftMotor.setControl(Follower(rightMotor.deviceID, true)) + + rightMotor.setPosition(0.0) } override fun getPosition(): Double { @@ -58,8 +72,12 @@ class ElevatorIOReal() : ElevatorIO { override fun setPosition(positionInches: Double) { desiredPosition = positionInches - rightMotor.setControl(motionMagic.withPosition(positionInches)) + rightMotor.setControl( + motionMagic.withPosition(positionInches) + ) } - override fun periodic() {} + override fun periodic() { + SmartDashboard.putNumber("Elevator Desired Position", desiredPosition) + } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOSim.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOSim.kt index 95439ca..e7ed894 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOSim.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOSim.kt @@ -4,7 +4,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile import frc.robot.subsystems.superstructure.elevator.ElevatorConstants import frc.robot.subsystems.superstructure.elevator.ElevatorIO import cshcyberhawks.lib.math.Timer -import frc.robot.math.MiscCalculations +import cshcyberhawks.lib.math.MiscCalculations class ElevatorIOSim : ElevatorIO { private val trapProfile = TrapezoidProfile( @@ -33,4 +33,4 @@ class ElevatorIOSim : ElevatorIO { lastTime = currentTime } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt index 35b54b0..1166b00 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt @@ -2,7 +2,7 @@ package frc.robot.subsystems.superstructure.intake object IntakeConstants { const val coralIntakeTimeoutSeconds = 0.5 - const val coralScoreTimeoutSeconds = 0.2 + const val coralScoreTimeoutSeconds = 0.5 const val algaeIntakeTimeoutSeconds = 0.3 const val algaeScoreTimeoutSeconds = 0.5 diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIO.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIO.kt index 85d85fb..9912680 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIO.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIO.kt @@ -2,6 +2,10 @@ package frc.robot.subsystems.superstructure.intake interface IntakeIO { fun setIntakeState(state: IntakeState) + + fun hasCoral():Boolean + fun hasAlgae():Boolean + fun watchForIntake() fun periodic() diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeState.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeState.kt index 694cc92..6aa477b 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeState.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeState.kt @@ -1,12 +1,13 @@ package frc.robot.subsystems.superstructure.intake // Setting the motor positive will intake both coral and algae -enum class IntakeState(val speed: Double) { +enum class IntakeState(val current: Double) { Idle(0.0), - CoralIntake(0.75), - CoralScore(-0.5), - AlgaeIntake(0.8), - AlgaeScore(-0.5) + CoralIntake(-40.0), + CoralScore(200.0), + AlgaeIntake(40.0), + AlgaeScore(-40.0), + AlgaeHolding(1.0) } enum class GamePieceState { diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt index 2857a01..544379a 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt @@ -1,9 +1,7 @@ package frc.robot.subsystems.superstructure.intake -import cshcyberhawks.lib.requests.Prerequisite -import cshcyberhawks.lib.requests.Request -import cshcyberhawks.lib.requests.SequentialRequest -import cshcyberhawks.lib.requests.WaitRequest +import cshcyberhawks.lib.requests.* +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase import frc.robot.RobotState @@ -15,7 +13,7 @@ class IntakeSystem(private val io: IntakeIO) : SubsystemBase() { private fun setIntakeState(state: IntakeState) = Request.withAction { io.setIntakeState(state) } private fun watchForIntake() = Request.withAction { io.watchForIntake() } - private fun setGamePieceState(state: GamePieceState) = Request.withAction { RobotState.gamePieceState = state } + fun idle() = setIntakeState(IntakeState.Idle) fun coralIntake() = SequentialRequest( setIntakeState(IntakeState.CoralIntake), @@ -26,26 +24,30 @@ class IntakeSystem(private val io: IntakeIO) : SubsystemBase() { fun coralScore() = SequentialRequest( setIntakeState(IntakeState.CoralScore), WaitRequest(IntakeConstants.coralScoreTimeoutSeconds), - setGamePieceState(GamePieceState.Empty), setIntakeState(IntakeState.Idle) ) fun algaeIntake() = SequentialRequest( setIntakeState(IntakeState.AlgaeIntake), - WaitRequest(IntakeConstants.algaeIntakeTimeoutSeconds), watchForIntake() ) fun algaeScore() = SequentialRequest( - setIntakeState(IntakeState.AlgaeIntake), - WaitRequest(IntakeConstants.algaeScoreTimeoutSeconds), - setGamePieceState(GamePieceState.Empty), + setIntakeState(IntakeState.AlgaeScore), + WaitRequest(IntakeConstants.algaeScoreTimeoutSeconds).withPrerequisite(Prerequisite.withCondition { !io.hasAlgae() }), + Request.withAction { println("Done Scoring") }, setIntakeState(IntakeState.Idle) ) override fun periodic() { io.periodic() + + RobotState.gamePieceState = if (io.hasCoral()) GamePieceState.Coral else if (io.hasAlgae()) GamePieceState + .Algae else GamePieceState.Empty + + SmartDashboard.putBoolean("Has coral", io.hasCoral()) + SmartDashboard.putBoolean("Has algae", io.hasAlgae()) } override fun simulationPeriodic() {} diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt index 2b93c1d..a324221 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt @@ -6,6 +6,18 @@ import frc.robot.subsystems.superstructure.intake.IntakeState class IntakeIOEmpty() : IntakeIO { override fun setIntakeState(state: IntakeState) {} - override fun watchForIntake() {} + + override fun watchForIntake() { + + } + + override fun hasAlgae(): Boolean { + return false + } + + override fun hasCoral(): Boolean { + return false + } + override fun periodic() {} -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt index e28e469..6d40e7a 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt @@ -2,63 +2,81 @@ package frc.robot.subsystems.superstructure.intake.implementation import au.grapplerobotics.LaserCan import au.grapplerobotics.interfaces.LaserCanInterface -import au.grapplerobotics.interfaces.LaserCanInterface.Measurement import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.TorqueCurrentFOC import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.InvertedValue -import com.revrobotics.spark.SparkBase -import com.revrobotics.spark.SparkLowLevel -import com.revrobotics.spark.SparkMax -import com.revrobotics.spark.config.SparkMaxConfig import edu.wpi.first.math.util.Units -import frc.robot.RobotState +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import frc.robot.constants.CANConstants -import frc.robot.subsystems.superstructure.intake.GamePieceState -import frc.robot.subsystems.superstructure.intake.IntakeConstants import frc.robot.subsystems.superstructure.intake.IntakeIO import frc.robot.subsystems.superstructure.intake.IntakeState -class IntakeIOReal : IntakeIO { +class IntakeIOReal() : IntakeIO { private val intakeMotor = TalonFX(CANConstants.Intake.motorId) + private val coralLaserCAN = LaserCan(CANConstants.Intake.coralLaserCANId) + private val algaeLaserCAN = LaserCan(CANConstants.Intake.algaeLaserCANId) - private var intakeState = IntakeState.Idle + private val torqueRequest = TorqueCurrentFOC(0.0) - private var watchingCurrent = false + private var watchingForIntake = false init { val coralIntakeMotorConfiguration = TalonFXConfiguration() coralIntakeMotorConfiguration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive + val currentConfigs = coralIntakeMotorConfiguration.CurrentLimits + currentConfigs.StatorCurrentLimitEnable = true + currentConfigs.StatorCurrentLimit = 60.0 + intakeMotor.configurator.apply(coralIntakeMotorConfiguration) + + coralLaserCAN.setRangingMode(LaserCanInterface.RangingMode.SHORT) + //coralLaserCAN.setRegionOfInterest(RegionOfInterest(8, 8, 16, 16)) + coralLaserCAN.setTimingBudget(LaserCanInterface.TimingBudget.TIMING_BUDGET_33MS) + + algaeLaserCAN.setRangingMode(LaserCanInterface.RangingMode.SHORT) + //algaeLaserCAN.setRegionOfInterest(RegionOfInterest(8, 8, 16, 16)) + algaeLaserCAN.setTimingBudget(LaserCanInterface.TimingBudget.TIMING_BUDGET_33MS) } override fun setIntakeState(state: IntakeState) { - intakeState = state - intakeMotor.set(state.speed) + intakeMotor.setControl(torqueRequest.withOutput(state.current)) } override fun watchForIntake() { - watchingCurrent = true + watchingForIntake = true } - override fun periodic() { - if (watchingCurrent) { - if (intakeMotor.supplyCurrent.valueAsDouble > IntakeConstants.intakeCurrentThreshold) { - when (intakeState) { - IntakeState.CoralIntake -> { - RobotState.gamePieceState = GamePieceState.Coral - } - - IntakeState.AlgaeIntake -> { - RobotState.gamePieceState = GamePieceState.Algae - } - - else -> {} - } + override fun hasAlgae(): Boolean { + val measurement: LaserCanInterface.Measurement = algaeLaserCAN.measurement + SmartDashboard.putNumber("Algae Measurement", measurement.distance_mm.toDouble()) + @Suppress("SENSELESS_COMPARISON") + return (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT && measurement.distance_mm < Units.inchesToMeters( + 10.0 // Other side is ~15.5in from sensor + ) * 1000.0 + ) + } - setIntakeState(IntakeState.Idle) + override fun hasCoral(): Boolean { + val measurement: LaserCanInterface.Measurement = coralLaserCAN.measurement + SmartDashboard.putNumber("Coral Measurement", measurement.distance_mm.toDouble()) + @Suppress("SENSELESS_COMPARISON") + return (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT && measurement.distance_mm < 48.0) + } - watchingCurrent = false + override fun periodic() { + if (watchingForIntake) { + if (hasCoral()) { + intakeMotor.set(IntakeState.Idle.current) + watchingForIntake = false + } else if (hasAlgae()) { + intakeMotor.set(IntakeState.AlgaeHolding.current) + watchingForIntake = false } } + + SmartDashboard.putNumber("Intake position", intakeMotor.position.valueAsDouble) + SmartDashboard.putNumber("Intake velocity", intakeMotor.velocity.valueAsDouble) + SmartDashboard.putNumber("Intake acceleration", intakeMotor.acceleration.valueAsDouble) } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotConstants.kt index 3fa87d5..fe7a87d 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotConstants.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotConstants.kt @@ -4,11 +4,11 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile object PivotConstants { const val velocityDegrees = 180.0 // Guessed values for now - const val accelerationDegrees = 200.0 + const val accelerationDegrees = 250.0 // Conversion so one (I guess its mechanism rotation) is 1 rotation of the pivot const val conversionFactor = - 100.0 * (24 / 32) + 100.0 * (32.0 / 24.0) / 360.0 const val angleToleranceDegrees = 2.0 -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt index d2b3ff7..3148456 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt @@ -15,6 +15,11 @@ class PivotSystem(private val io: PivotIO) : SubsystemBase() { fun atDesiredAngle() = io.atDesiredAngle() + init { + SmartDashboard.putNumber("L3 Angle", 125.0) + SmartDashboard.putNumber("L4 Angle", 127.0) + } + private fun setAngle(angleDegrees: Double) = Request.withAction { io.setAngle(angleDegrees) } @@ -35,17 +40,19 @@ class PivotSystem(private val io: PivotIO) : SubsystemBase() { fun stowAngle() = setAngle(290.0) - fun travelAngle() = setAngle(230.0) + fun travelAngle() = setAngle(220.0) fun feederAngle() = setAngle(310.0) fun l2Angle() = setAngle(160.0) - fun l3Angle() = setAngle(125.0) - fun l4Angle() = setAngle(135.0) + fun l3Angle() = setAngle(SmartDashboard.getNumber("L3 Angle", 125.0)) + fun l4Angle() = setAngle(SmartDashboard.getNumber("L4 Angle", 127.0)) - fun algaeRemoveAngle() = setAngle(245.0) + fun algaeRemoveAngle() = setAngle(220.0) fun bargeAngle() = setAngle(130.0) + fun processorAngle() = setAngle(295.0) + override fun periodic() { io.periodic() diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt new file mode 100644 index 0000000..388daa9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt @@ -0,0 +1,136 @@ +package frc.robot.subsystems.superstructure.pivot.implementation + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.TorqueCurrentFOC +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.InvertedValue +import com.ctre.phoenix6.signals.NeutralModeValue +import cshcyberhawks.lib.hardware.AbsoluteDutyCycleEncoder +import edu.wpi.first.math.controller.ProfiledPIDController +import edu.wpi.first.math.trajectory.TrapezoidProfile +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import frc.robot.constants.CANConstants +import cshcyberhawks.lib.math.MiscCalculations +import edu.wpi.first.math.controller.ArmFeedforward +import edu.wpi.first.math.controller.PIDController +import edu.wpi.first.math.controller.SimpleMotorFeedforward +import frc.robot.subsystems.superstructure.pivot.PivotConstants +import frc.robot.subsystems.superstructure.pivot.PivotIO + +class PivotIOPID() : PivotIO { + private val motor = TalonFX(CANConstants.Pivot.motorId) +// private val encoderDutyCycle = DutyCycle() + private val encoder = AbsoluteDutyCycleEncoder(CANConstants.Pivot.encoderId) + + val pivotPIDController = + ProfiledPIDController( + 0.7, + 0.0, + 0.05, + TrapezoidProfile.Constraints( + PivotConstants.velocityDegrees, + PivotConstants.accelerationDegrees + ) + ) + +// val correctivePID = PIDController(0.3, 0.0, 0.0) + + private var motorConfig = TalonFXConfiguration() + + private val torqueRequest = TorqueCurrentFOC(0.0) + + private var desiredAngle = 290.0 + + private val tbOffset = 200.0 +// private fun getTBDegrees() = + + private var neutralCoast = false + + init { + val feedBackConfigs = motorConfig.Feedback + + // Converts 1 rotation to 1 in on the elevator + feedBackConfigs.SensorToMechanismRatio = PivotConstants.conversionFactor + + val slot0Configs = motorConfig.Slot0 + + slot0Configs.kP = 0.0 // A position error of 2.5 rotations results in 12 V output + slot0Configs.kI = 0.0 // no output for integrated error + slot0Configs.kD = 0.0 // A velocity error of 1 rps results in 0.1 V output + + motorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + + val rightCurrentConfigs = motorConfig.CurrentLimits + rightCurrentConfigs.StatorCurrentLimitEnable = true + rightCurrentConfigs.StatorCurrentLimit = 60.0 + + motor.configurator.apply(motorConfig) + + motor.setNeutralMode(NeutralModeValue.Brake) + + + // TODO: Also need to mess with this on bringup + // val currentPosition = + // encoder.get() * (24 / 32) * 360.0 // Clockwise should be positive and zero + // should probably be 45 degrees up from motor zero (position we likely won't start at but + // allowing for full rotation with the ratio) + // + // motor.setPosition((currentPosition - 45.0)) + + pivotPIDController.goal = TrapezoidProfile.State(290.0, 0.0) + + SmartDashboard.putBoolean("Pivot coast", false) + + motor.setNeutralMode( + if (neutralCoast) NeutralModeValue.Coast else NeutralModeValue.Brake + ) + } + + override fun getAngle(): Double { + return MiscCalculations.wrapAroundAngles((MiscCalculations.wrapAroundAngles(encoder + .absolutePosition * 360.0) - tbOffset)) + } + + override fun getDesiredAngle(): Double { + return desiredAngle + } + + override fun atDesiredAngle(): Boolean = + MiscCalculations.appxEqual( + getAngle(), + desiredAngle, + PivotConstants.angleToleranceDegrees + ) + + override fun setAngle(angleDegrees: Double) { + desiredAngle = angleDegrees + pivotPIDController.goal = TrapezoidProfile.State(angleDegrees, 0.0) + } + + override fun periodic() { + SmartDashboard.putNumber("Pivot Raw TB Angle", MiscCalculations.wrapAroundAngles(encoder.absolutePosition * 360.0)) + SmartDashboard.putNumber("Pivot Desired Angle", desiredAngle) + + val gravityFF = 0.0 + val positionPIDOut = pivotPIDController.calculate(getAngle()) +// val correctivePIDOut = correctivePID.calculate(getAngle(), desiredAngle) + + SmartDashboard.putNumber("Pivot Position Error", pivotPIDController.positionError) + SmartDashboard.putNumber("Position PID Output", positionPIDOut) + //SmartDashboard.putNumber("Corrective PID Output", correctivePIDOut) + + val motorSet = positionPIDOut + gravityFF// + correctivePIDOut + + SmartDashboard.putNumber("Pivot Output", motorSet) + + motor.setControl(torqueRequest.withOutput(motorSet)) + + val sdCoast = SmartDashboard.getBoolean("Pivot coast", false) + if (sdCoast != neutralCoast) { + neutralCoast = sdCoast + motor.setNeutralMode( + if (neutralCoast) NeutralModeValue.Coast else NeutralModeValue.Brake + ) + } + } +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOReal.kt index 1d93a43..8639bd2 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOReal.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOReal.kt @@ -3,11 +3,10 @@ package frc.robot.subsystems.superstructure.pivot.implementation import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC import com.ctre.phoenix6.hardware.TalonFX -import com.ctre.phoenix6.controls.MotionMagicVoltage import com.ctre.phoenix6.signals.InvertedValue import edu.wpi.first.wpilibj.DutyCycleEncoder import frc.robot.constants.CANConstants -import frc.robot.math.MiscCalculations +import cshcyberhawks.lib.math.MiscCalculations import frc.robot.subsystems.superstructure.pivot.PivotConstants import frc.robot.subsystems.superstructure.pivot.PivotIO @@ -47,6 +46,10 @@ class PivotIOReal() : PivotIO { // motionMagicConfigs.MotionMagicJerk = 1600; Optional // Target jerk of 1600 rps/s/s (0.1 seconds) motorConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + val currentConfigs = motorConfiguration.CurrentLimits + currentConfigs.StatorCurrentLimitEnable = true + currentConfigs.StatorCurrentLimit = 60.0 + motor.configurator.apply(motorConfiguration); // TODO: Also need to mess with this on bringup diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOSim.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOSim.kt index 9e651af..b19a669 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOSim.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOSim.kt @@ -4,7 +4,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile import frc.robot.subsystems.superstructure.pivot.PivotConstants import frc.robot.subsystems.superstructure.pivot.PivotIO import cshcyberhawks.lib.math.Timer -import frc.robot.math.MiscCalculations +import cshcyberhawks.lib.math.MiscCalculations class PivotIOSim : PivotIO { val trapProfile = TrapezoidProfile( @@ -35,4 +35,4 @@ class PivotIOSim : PivotIO { lastTime = currentTime } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.kt b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.kt index dc1c4e6..96e534d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.kt +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.kt @@ -12,4 +12,11 @@ object SwerveConstants { RotationsPerSecond.of(0.75).`in`(RadiansPerSecond) // 3/4 of a rotation per second max angular velocity var ControlledAngularRate = MaxAngularRateConst + + + val maxAutoSpeed = 2.0 + val maxAutoAccel = 3.0 + + val maxAutoTwist = 350.0 + val maxAutoTwistAccel = 450.0 } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIOBase.kt b/src/main/java/frc/robot/subsystems/swerve/SwerveIOBase.kt index 7bcdc2e..681abfc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveIOBase.kt +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIOBase.kt @@ -20,6 +20,7 @@ open class SwerveIOBase() : SubsystemBase() { open fun addVisionMeasurement(visionRobotPoseMeters: Pose2d, timestampSeconds: Double) {} open fun setVisionMeasurementStdDevs(visionMeasurementStdDevs: Matrix) {} + open fun resetPose(pose2d: Pose2d) {} open fun applyDriveRequest(x: Double, y: Double, twistRadians: Double) {} open fun applyRobotRelativeDriveRequest(x: Double, y: Double, twistRadians: Double) {} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIOReal.kt b/src/main/java/frc/robot/subsystems/swerve/SwerveIOReal.kt index c6054cd..2ddd61f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveIOReal.kt +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIOReal.kt @@ -3,11 +3,17 @@ package frc.robot.subsystems.swerve import com.ctre.phoenix6.swerve.SwerveDrivetrain import com.ctre.phoenix6.swerve.SwerveModule import com.ctre.phoenix6.swerve.SwerveRequest +import com.pathplanner.lib.auto.AutoBuilder +import com.pathplanner.lib.commands.PathPlannerAuto +import com.pathplanner.lib.config.PIDConstants +import com.pathplanner.lib.config.RobotConfig +import com.pathplanner.lib.controllers.PPHolonomicDriveController import edu.wpi.first.math.Matrix import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.numbers.N1 import edu.wpi.first.math.numbers.N3 import edu.wpi.first.units.Units.* +import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import frc.robot.RobotContainer @@ -26,12 +32,43 @@ class SwerveIOReal() : SwerveIOBase() { init { drivetrain.registerTelemetry(logger::telemeterize) + configurePathPlanner() } // override fun setDefaultCommand(command: Command) { // drivetrain.defaultCommand = command // } + override fun resetPose(pose2d: Pose2d) { + drivetrain.resetPose(pose2d) + } + private fun configurePathPlanner() { +// drivetrain.get + AutoBuilder.configure( + { getSwervePose() }, + { + drivetrain.resetPose(Pose2d()) + seedFieldCentric() + }, + { drivetrain.state.Speeds }, + { speeds -> applyRobotRelativeDriveRequest(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond) }, + PPHolonomicDriveController( + PIDConstants(5.0, 0.0, 0.0), + PIDConstants(1.0, 0.0, 0.1), + ), + RobotConfig.fromGUISettings(), + { + DriverStation.getAlliance().isPresent && DriverStation.getAlliance() + .get() == DriverStation.Alliance.Red + }, this + ) + } + + override fun getAutoPath(name: String): Command { + return PathPlannerAuto(name) + } + + override fun seedFieldCentric() { drivetrain.seedFieldCentric() } @@ -48,21 +85,24 @@ class SwerveIOReal() : SwerveIOBase() { /* Setting up bindings for necessary control of the swerve drive platform */ private val fieldCentric: SwerveRequest.FieldCentric = SwerveRequest.FieldCentric() - .withDeadband(SwerveConstants.MaxSpeedConst * 0.1) - .withRotationalDeadband(SwerveConstants.MaxAngularRateConst * 0.1) // Add a 10% deadband +// .withDeadband(SwerveConstants.MaxSpeedConst * 0.1) +// .withRotationalDeadband(SwerveConstants.MaxAngularRateConst * 0.1) // Add a 10% deadband .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage) // Use open-loop control for drive motors private val robotRelative: SwerveRequest.RobotCentric = SwerveRequest.RobotCentric() - .withDeadband(SwerveConstants.MaxSpeedConst * 0.1) - .withRotationalDeadband(SwerveConstants.MaxAngularRateConst * 0.1) // Add a 10% deadband +// .withDeadband(SwerveConstants.MaxSpeedConst * 0.1) +// .withRotationalDeadband(SwerveConstants.MaxAngularRateConst * 0.1) // Add a 10% deadband .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage) // Use open-loop control for drive motors private val brake = SwerveRequest.SwerveDriveBrake() private val point = SwerveRequest.PointWheelsAt() + + override fun applyDriveRequest(x: Double, y: Double, twistRadians: Double) { - drivetrain.applyRequest { +// println("x: " + x); + drivetrain.setControl( fieldCentric.withVelocityX( x ) // @@ -76,11 +116,14 @@ class SwerveIOReal() : SwerveIOBase() { .withRotationalRate( twistRadians ) - }.execute() + ) +// drivetrain.applyRequest { +// +// }.execute() } override fun applyRobotRelativeDriveRequest(x: Double, y: Double, twistRadians: Double) { - drivetrain.applyRequest { + drivetrain.setControl( robotRelative.withVelocityX( x ) // @@ -94,6 +137,6 @@ class SwerveIOReal() : SwerveIOBase() { .withRotationalRate( twistRadians ) - }.execute() + ) } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/TunerConstants.kt b/src/main/java/frc/robot/subsystems/swerve/TunerConstants.kt index 53406da..e5cb08c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/TunerConstants.kt +++ b/src/main/java/frc/robot/subsystems/swerve/TunerConstants.kt @@ -2,26 +2,25 @@ package frc.robot.subsystems.swerve import com.ctre.phoenix6.CANBus import com.ctre.phoenix6.configs.* -import com.ctre.phoenix6.hardware.* -import com.ctre.phoenix6.signals.* -import com.ctre.phoenix6.swerve.* +import com.ctre.phoenix6.hardware.CANcoder +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue +import com.ctre.phoenix6.swerve.SwerveDrivetrain import com.ctre.phoenix6.swerve.SwerveDrivetrain.DeviceConstructor -import com.ctre.phoenix6.swerve.SwerveModuleConstants.* +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants +import com.ctre.phoenix6.swerve.SwerveModuleConstants +import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory import edu.wpi.first.math.Matrix import edu.wpi.first.math.numbers.N1 import edu.wpi.first.math.numbers.N3 import edu.wpi.first.units.Units -import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Current -import edu.wpi.first.units.measure.Distance -import edu.wpi.first.units.measure.LinearVelocity -import edu.wpi.first.units.measure.MomentOfInertia -import edu.wpi.first.units.measure.Voltage +import edu.wpi.first.units.measure.* import frc.robot.subsystems.swerve.CommandSwerveDrivetrain -// import frc.robot.subsystems.swerve.CommandSwerveDrivetrain - - +// Generated by the Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html object TunerConstants { // Both sets of gains need to be tuned to your individual robot. // The steer motor uses any SwerveModule.SteerRequestType control request with the @@ -39,21 +38,21 @@ object TunerConstants { // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors - private val kSteerClosedLoopOutput: ClosedLoopOutputType = ClosedLoopOutputType.Voltage + private val kSteerClosedLoopOutput = SwerveModuleConstants.ClosedLoopOutputType.Voltage // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors - private val kDriveClosedLoopOutput: ClosedLoopOutputType = ClosedLoopOutputType.Voltage + private val kDriveClosedLoopOutput = SwerveModuleConstants.ClosedLoopOutputType.Voltage // The type of motor used for the drive motor - private val kDriveMotorType: DriveMotorArrangement = DriveMotorArrangement.TalonFX_Integrated + private val kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated // The type of motor used for the drive motor - private val kSteerMotorType: SteerMotorArrangement = SteerMotorArrangement.TalonFX_Integrated + private val kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder - private val kSteerFeedbackType: SteerFeedbackType = SteerFeedbackType.FusedCANcoder + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private val kSteerFeedbackType = SwerveModuleConstants.SteerFeedbackType.FusedCANcoder // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot @@ -61,7 +60,7 @@ object TunerConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private val driveInitialConfigs: TalonFXConfiguration = TalonFXConfiguration() + private val driveInitialConfigs = TalonFXConfiguration() private val steerInitialConfigs: TalonFXConfiguration = TalonFXConfiguration() .withCurrentLimits( CurrentLimitsConfigs() // Swerve azimuth does not require much torque output, so we can set a relatively low @@ -69,8 +68,7 @@ object TunerConstants { .withStatorCurrentLimit(Units.Amps.of(60.0)) .withStatorCurrentLimitEnable(true) ) - private val encoderInitialConfigs: CANcoderConfiguration = CANcoderConfiguration() - + private val encoderInitialConfigs = CANcoderConfiguration() // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private val pigeonConfigs: Pigeon2Configuration? = null @@ -81,20 +79,20 @@ object TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - val kSpeedAt12Volts: LinearVelocity = Units.MetersPerSecond.of(5.21) + val kSpeedAt12Volts: LinearVelocity = Units.MetersPerSecond.of(4.73) // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot private const val kCoupleRatio = 3.5714285714285716 - private const val kDriveGearRatio = 6.122448979591837 + private const val kDriveGearRatio = 6.746031746031747 private const val kSteerGearRatio = 21.428571428571427 private val kWheelRadius: Distance = Units.Inches.of(2.0) private const val kInvertLeftSide = false private const val kInvertRightSide = true - private const val kPigeonId = 30 + private const val kPigeonId = 15 // These are only used for simulation private val kSteerInertia: MomentOfInertia = Units.KilogramSquareMeters.of(0.01) @@ -105,7 +103,7 @@ object TunerConstants { private val kDriveFrictionVoltage: Voltage = Units.Volts.of(0.2) val DrivetrainConstants: SwerveDrivetrainConstants = SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) + .withCANBusName(kCANBus.name) .withPigeon2Id(kPigeonId) .withPigeon2Configs(pigeonConfigs) @@ -132,49 +130,51 @@ object TunerConstants { .withSteerFrictionVoltage(kSteerFrictionVoltage) .withDriveFrictionVoltage(kDriveFrictionVoltage) + // Front Left - private const val kFrontLeftDriveMotorId = 7 - private const val kFrontLeftSteerMotorId = 9 - private const val kFrontLeftEncoderId = 1 - private val kFrontLeftEncoderOffset: Angle = Units.Rotations.of(0.34228515625) + private const val kFrontLeftDriveMotorId = 8 + private const val kFrontLeftSteerMotorId = 12 + private const val kFrontLeftEncoderId = 4 + private val kFrontLeftEncoderOffset: Angle = Units.Rotations.of(0.098876953125) private const val kFrontLeftSteerMotorInverted = true private const val kFrontLeftEncoderInverted = false - private val kFrontLeftXPos: Distance = Units.Inches.of(12.125) - private val kFrontLeftYPos: Distance = Units.Inches.of(12.125) + private val kFrontLeftXPos: Distance = Units.Inches.of(12.0) + private val kFrontLeftYPos: Distance = Units.Inches.of(12.0) // Front Right - private const val kFrontRightDriveMotorId = 6 - private const val kFrontRightSteerMotorId = 10 - private const val kFrontRightEncoderId = 2 - private val kFrontRightEncoderOffset: Angle = Units.Rotations.of(-0.034912109375) + private const val kFrontRightDriveMotorId = 7 + private const val kFrontRightSteerMotorId = 9 + private const val kFrontRightEncoderId = 1 + private val kFrontRightEncoderOffset: Angle = Units.Rotations.of(-0.410888671875) private const val kFrontRightSteerMotorInverted = true private const val kFrontRightEncoderInverted = false - private val kFrontRightXPos: Distance = Units.Inches.of(12.125) - private val kFrontRightYPos: Distance = Units.Inches.of(-12.125) + private val kFrontRightXPos: Distance = Units.Inches.of(12.0) + private val kFrontRightYPos: Distance = Units.Inches.of(-12.0) // Back Left - private const val kBackLeftDriveMotorId = 5 - private const val kBackLeftSteerMotorId = 11 - private const val kBackLeftEncoderId = 3 - private val kBackLeftEncoderOffset: Angle = Units.Rotations.of(-0.101806640625) + private const val kBackLeftDriveMotorId = 6 + private const val kBackLeftSteerMotorId = 10 + private const val kBackLeftEncoderId = 2 + private val kBackLeftEncoderOffset: Angle = Units.Rotations.of(-0.044921875) private const val kBackLeftSteerMotorInverted = true private const val kBackLeftEncoderInverted = false - private val kBackLeftXPos: Distance = Units.Inches.of(-12.125) - private val kBackLeftYPos: Distance = Units.Inches.of(12.125) + private val kBackLeftXPos: Distance = Units.Inches.of(-12.0) + private val kBackLeftYPos: Distance = Units.Inches.of(12.0) // Back Right - private const val kBackRightDriveMotorId = 8 - private const val kBackRightSteerMotorId = 12 - private const val kBackRightEncoderId = 4 - private val kBackRightEncoderOffset: Angle = Units.Rotations.of(0.080322265625) + private const val kBackRightDriveMotorId = 5 + private const val kBackRightSteerMotorId = 11 + private const val kBackRightEncoderId = 3 + private val kBackRightEncoderOffset: Angle = Units.Rotations.of(-0.3505859375) private const val kBackRightSteerMotorInverted = true private const val kBackRightEncoderInverted = false - private val kBackRightXPos: Distance = Units.Inches.of(-12.125) - private val kBackRightYPos: Distance = Units.Inches.of(-12.125) + private val kBackRightXPos: Distance = Units.Inches.of(-12.0) + private val kBackRightYPos: Distance = Units.Inches.of(-12.0) + val FrontLeft: SwerveModuleConstants = ConstantCreator.createModuleConstants( @@ -208,14 +208,14 @@ object TunerConstants { * Creates a CommandSwerveDrivetrain instance. * This should only be called once in your robot program,. */ - fun createDrivetrain(): CommandSwerveDrivetrain { - return CommandSwerveDrivetrain( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ) - } + fun createDrivetrain(): CommandSwerveDrivetrain { + return CommandSwerveDrivetrain( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + ) + } - /** + /** * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ open class TunerSwerveDrivetrain : SwerveDrivetrain { @@ -234,32 +234,32 @@ object TunerConstants { drivetrainConstants: SwerveDrivetrainConstants?, vararg modules: SwerveModuleConstants<*, *, *>? ) : - super( - DeviceConstructor { deviceId: Int, canbus: String? -> TalonFX(deviceId, canbus) }, - DeviceConstructor { deviceId: Int, canbus: String? -> TalonFX(deviceId, canbus) }, - DeviceConstructor { deviceId: Int, canbus: String? -> CANcoder(deviceId, canbus) }, - drivetrainConstants, *modules - ) - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - * - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - constructor( - drivetrainConstants: SwerveDrivetrainConstants?, - odometryUpdateFrequency: Double, - vararg modules: SwerveModuleConstants<*, *, *>? - ) : + super( + DeviceConstructor { deviceId: Int, canbus: String? -> TalonFX(deviceId, canbus) }, + DeviceConstructor { deviceId: Int, canbus: String? -> TalonFX(deviceId, canbus) }, + DeviceConstructor { deviceId: Int, canbus: String? -> CANcoder(deviceId, canbus) }, + drivetrainConstants, *modules + ) + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + * + * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + constructor( + drivetrainConstants: SwerveDrivetrainConstants?, + odometryUpdateFrequency: Double, + vararg modules: SwerveModuleConstants<*, *, *>? + ) : super( DeviceConstructor { deviceId: Int, canbus: String? -> TalonFX(deviceId, canbus) }, DeviceConstructor { deviceId: Int, canbus: String? -> TalonFX(deviceId, canbus) }, @@ -267,33 +267,33 @@ object TunerConstants { drivetrainConstants, odometryUpdateFrequency, *modules ) - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - * - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - constructor( - drivetrainConstants: SwerveDrivetrainConstants?, - odometryUpdateFrequency: Double, - odometryStandardDeviation: Matrix?, - visionStandardDeviation: Matrix?, - vararg modules: SwerveModuleConstants<*, *, *>? - ): + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + * + * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param modules Constants for each specific module + */ + constructor( + drivetrainConstants: SwerveDrivetrainConstants?, + odometryUpdateFrequency: Double, + odometryStandardDeviation: Matrix?, + visionStandardDeviation: Matrix?, + vararg modules: SwerveModuleConstants<*, *, *>? + ): super( DeviceConstructor { deviceId: Int, canbus: String? -> TalonFX(deviceId, canbus) }, DeviceConstructor { deviceId: Int, canbus: String? -> TalonFX(deviceId, canbus) }, @@ -302,6 +302,4 @@ object TunerConstants { odometryStandardDeviation, visionStandardDeviation, *modules ) } - - -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Vision.kt b/src/main/java/frc/robot/util/Vision.kt index 859ba18..0e033fe 100644 --- a/src/main/java/frc/robot/util/Vision.kt +++ b/src/main/java/frc/robot/util/Vision.kt @@ -1,15 +1,14 @@ package frc.robot.util +import com.ctre.phoenix6.Utils import edu.wpi.first.math.VecBuilder 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.util.sendable.Sendable import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard -import frc.robot.LimelightHelpers -import frc.robot.RobotConfiguration -import frc.robot.RobotContainer -import frc.robot.RobotType +import frc.robot.* import kotlin.math.max //import frc.robot.constants.FieldConstants @@ -21,36 +20,61 @@ class VisionSystem { val max_distance_m = 6.0 val limelightNames: Array = when (RobotConfiguration.robotType) { - RobotType.Real -> arrayOf("limelight-front") + RobotType.Real -> arrayOf("limelight-tright", "limelight-btfront") +// RobotType.Real -> arrayOf("limelight-tleft", "limelight-tright", "limelight-btfront") + else -> emptyArray() } fun updateOdometryFromDisabled() { var namesToSearch: Array; + var fullReset = SmartDashboard.getBoolean("full reset with vision", false) namesToSearch = limelightNames - for (llName in namesToSearch) { if (DriverStation.getAlliance().isEmpty) { -// println("DS alliance is empty; skipping vision") + println("DS alliance is empty; skipping vision") return } + if (LimelightHelpers.getBotPoseEstimate_wpiBlue(llName) == null) { +// println(llName + " is null") + SmartDashboard.putBoolean("ll " + llName + " is valid", false) + return; + } + + SmartDashboard.putBoolean("ll " + llName + " is valid", true) var llMeasure: LimelightHelpers.PoseEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue(llName) +// println("ll measure x: " + llMeasure.pose.x) +// println("ll measure y: " + llMeasure.pose.y) + if (llMeasure.pose.x != 0.0 && llMeasure.pose.y != 0.0) { + val poseDifference = llMeasure.pose.translation.getDistance(RobotContainer.drivetrain.getSwervePose().translation) val distanceToTag = llMeasure.avgTagDist +// SmartDashboard.putNumber("distance to tag", distanceToTag) + + +// SmartDashboard.putNumber("ll update pose x: ", llMeasure.pose.x) +// SmartDashboard.putNumber("ll update pose y: ", llMeasure.pose.y) +// SmartDashboard.putNumber("ll timestampt: ", llMeasure.timestampSeconds) + + if (fullReset) { + RobotContainer.drivetrain.resetPose(llMeasure.pose) + return; + } if (distanceToTag < max_distance_m) { - var xyStds: Double - var degStds: Double +// println("doing the thingy") + var xyStds: Double = 0.01 + var degStds: Double = 0.01 if (llMeasure.tagCount >= 2) { xyStds = 0.1 @@ -73,7 +97,7 @@ class VisionSystem { RobotContainer.drivetrain.addVisionMeasurement( llMeasure.pose, - llMeasure.timestampSeconds + Utils.fpgaToCurrentTime(llMeasure.timestampSeconds) ) } } @@ -84,6 +108,7 @@ class VisionSystem { var namesToSearch: Array; + namesToSearch = limelightNames for (llName in namesToSearch) { @@ -92,9 +117,19 @@ class VisionSystem { return } + if (LimelightHelpers.getBotPoseEstimate_wpiBlue(llName) == null) { +// println(llName + " is null") + SmartDashboard.putBoolean("ll " + llName + " is valid", false) + return; + } + + SmartDashboard.putBoolean("ll " + llName + " is valid", true) + var llMeasure: LimelightHelpers.PoseEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue(llName) + + if (llMeasure.tagCount >= tagCount && llMeasure.pose.x != 0.0 && llMeasure.pose.y != 0.0) { val poseDifference = llMeasure.pose.translation.getDistance(RobotContainer.drivetrain.getSwervePose().translation) @@ -105,21 +140,6 @@ class VisionSystem { var xyStds: Double var degStds: Double - if (llName == "limelight-rightsi") { - if (llMeasure.tagCount >= 2) { - xyStds = 1.0 - degStds = 12.0 - } else if (llMeasure.avgTagArea > 0.8 && poseDifference < 0.5) { - xyStds = 2.0 - degStds = 30.0 - } else if (llMeasure.avgTagArea > 0.1 && poseDifference < 0.3) { - xyStds = 4.0 - degStds = 50.0 - } else { - xyStds = 6.0 - degStds = 80.0 - } - } else { if (llMeasure.tagCount >= 2) { xyStds = 0.5 degStds = 6.0 @@ -133,17 +153,20 @@ class VisionSystem { xyStds = 4.0 degStds = 50.0 } - } + + +// var headingDeg = RobotContainer.drivetrain.getSwervePose().rotation RobotContainer.drivetrain.setVisionMeasurementStdDevs( VecBuilder.fill(xyStds, xyStds, Math.toRadians(degStds)) ) + // println("updating odometry with ll") // println("Updating with LL ${llName}: X = " + llMeasure.pose.x + " Y = " + llMeasure.pose.y) RobotContainer.drivetrain.addVisionMeasurement( llMeasure.pose, - llMeasure.timestampSeconds + Utils.fpgaToCurrentTime(llMeasure.timestampSeconds) ) } } diff --git a/src/main/java/frc/robot/util/input/DriverAction.kt b/src/main/java/frc/robot/util/input/DriverAction.kt index c3b6d44..2f085f9 100644 --- a/src/main/java/frc/robot/util/input/DriverAction.kt +++ b/src/main/java/frc/robot/util/input/DriverAction.kt @@ -2,6 +2,7 @@ package frc.robot.util.input import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands +import frc.robot.commands.TeleopAutoScore import frc.robot.subsystems.superstructure.Superstructure enum class DriverAction(val cmd: Command) { @@ -12,5 +13,5 @@ enum class DriverAction(val cmd: Command) { ScoreL4(Commands.runOnce({ Superstructure.scoreL4() })), RemoveAlgaeLow(Commands.runOnce({ Superstructure.removeAlgaeLow() })), RemoveAlgaeHigh(Commands.runOnce({ Superstructure.removeAlgaeHigh() })), - ScoreBarge(Commands.runOnce({ Superstructure.scoreBarge() })) + ScoreBarge(Commands.runOnce({ Superstructure.scoreBarge() })), } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/input/ManualDriverInput.kt b/src/main/java/frc/robot/util/input/ManualDriverInput.kt new file mode 100644 index 0000000..fecddce --- /dev/null +++ b/src/main/java/frc/robot/util/input/ManualDriverInput.kt @@ -0,0 +1,38 @@ +package frc.robot.util.input + +import edu.wpi.first.wpilibj2.command.Commands +import frc.robot.RobotContainer +import frc.robot.RobotState +import frc.robot.commands.TeleopAutoScore +import java.util.* + +object ManualDriverInput { + + val cancel = 4; + fun configureBindings() { + RobotContainer.rightJoystick.button(2) + .onTrue(Commands.runOnce({ RobotContainer.drivetrain.seedFieldCentric(); println("seeding field relative") })) + + RobotContainer.rightJoystick.button(cancel).onTrue(Commands.runOnce({ + if (!RobotContainer.currentDriveCommand.isEmpty) { + RobotContainer.currentDriveCommand.get().cancel() + RobotContainer.currentDriveCommand = Optional.empty() + } + })) + + RobotContainer.leftJoystick.button(3).onTrue(Commands.runOnce({ + RobotState.actionCancelled = true + println("action cancelled") + })) + + RobotContainer.leftJoystick.button(4).onTrue(Commands.runOnce({ + println("action confirmed") + RobotState.actionConfirmed = true +// if (RobotContainer.currentDriveCommand.isEmpty) { +// TeleopAutoScore.score() +// } else { +// } + })) + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/input/ManualOperatorInput.kt b/src/main/java/frc/robot/util/input/ManualOperatorInput.kt index 6e323a5..a2b21d1 100644 --- a/src/main/java/frc/robot/util/input/ManualOperatorInput.kt +++ b/src/main/java/frc/robot/util/input/ManualOperatorInput.kt @@ -1,13 +1,66 @@ package frc.robot.util.IO +import edu.wpi.first.wpilibj2.command.Commands import frc.robot.RobotContainer +import frc.robot.RobotState import frc.robot.subsystems.superstructure.Superstructure object ManualOperatorInput { fun configureBindings() { -// RobotContainer.xbox.x().onTrue(Superstructure.intakeSystem.coralIntake()) -// RobotContainer.xbox.b().onTrue(Superstructure.intakeSystem.coralScore()) -// RobotContainer.xbox.a().onTrue(Superstructure.intakeSystem.algaeIntake()) -// RobotContainer.xbox.y().onTrue(Superstructure.intakeSystem.algaeScore()) +// RobotContainer.xbox.x().onTrue(Commands.runOnce({ +// Superstructure.request(Superstructure.intakeSystem.algaeIntake()) +// })) +// RobotContainer.xbox.b().onTrue(Commands.runOnce({ +// Superstructure.request(Superstructure.intakeSystem.algaeScore()) +// })) +// Superstructure.request(Superstructure.elevatorSystem.algaeRemoveHighPosition()) +// +// })) +// RobotContainer.xbox.a().onTrue(Commands.runOnce({ +// Superstructure.request( +// Superstructure.elevatorSystem.l3Position() +// ) +// })) +// RobotContainer.xbox.b().onTrue(Commands.runOnce({ +// Superstructure.request( +// Superstructure.elevatorSystem.stowPosition() +// ) +// })) +// RobotContainer.xbox.y().onTrue(Commands.runOnce({ +// Superstructure.scoreL4() +// })) +// RobotContainer.xbox.x().onTrue(Commands.runOnce({ +// Superstructure.intakeFeeder() +// })) +// RobotContainer.xbox.b().onTrue(Commands.runOnce({ +// Superstructure.scoreBarge() +// })) + RobotContainer.xbox.y().onTrue(Commands.runOnce({ + Superstructure.scoreL4() + })) + RobotContainer.xbox.b().onTrue(Commands.runOnce({ + Superstructure.scoreL3() + })) + RobotContainer.xbox.x().onTrue(Commands.runOnce({ + Superstructure.intakeFeeder() + })) + RobotContainer.xbox.leftBumper().onTrue(Commands.runOnce({ + Superstructure.removeAlgaeLow() + })) + RobotContainer.xbox.rightBumper().onTrue(Commands.runOnce({ + Superstructure.removeAlgaeHigh() + })) + RobotContainer.xbox.a().onTrue(Commands.runOnce({ + Superstructure.scoreBarge() + })) + RobotContainer.xbox.povUp().onTrue(Commands.runOnce({ + Superstructure.scoreProcessor() + })) + RobotContainer.xbox.back().onTrue(Commands.runOnce({ + RobotState.actionCancelled = true + })) + RobotContainer.xbox.start().onTrue(Commands.runOnce({ + RobotState.actionConfirmed = true + })) } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/util/input/OperatorControls.kt b/src/main/java/frc/robot/util/input/OperatorControls.kt new file mode 100644 index 0000000..67d8a6b --- /dev/null +++ b/src/main/java/frc/robot/util/input/OperatorControls.kt @@ -0,0 +1,31 @@ +package frc.robot.util.input + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import frc.robot.AutoTargeting + +object OperatorControls { + val actionChooser = SendableChooser() + + val coralSideChooser = SendableChooser() + val coralLevelChooser = SendableChooser() + init { + for (action in DriverAction.entries) { + actionChooser.addOption(action.name, action) + } + + SmartDashboard.putData(actionChooser) + + for (side in AutoTargeting.CoralSide.entries) { + coralSideChooser.addOption(side.name, side) + } + + SmartDashboard.putData(coralSideChooser) + + for (level in AutoTargeting.CoralLevel.entries) { + coralLevelChooser.addOption(level.name, level) + } + + SmartDashboard.putData(coralLevelChooser) + } +} \ No newline at end of file diff --git a/src/test/java/frc/robot/math/MiscCalculationsTest.kt b/src/test/java/frc/robot/math/MiscCalculationsTest.kt index 8af3a44..f9a5b7d 100644 --- a/src/test/java/frc/robot/math/MiscCalculationsTest.kt +++ b/src/test/java/frc/robot/math/MiscCalculationsTest.kt @@ -1,5 +1,6 @@ package frc.robot.math +import cshcyberhawks.lib.math.MiscCalculations import edu.wpi.first.math.geometry.Translation2d import org.junit.jupiter.api.Test