diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d1cb2cf..1afa325 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -112,8 +112,6 @@ public static final class PhysicalConstants { public static final Matter kChassis = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), PhysicalConstants.kRobotMass); public static final double kLoopTime = 0.13; // s, 20ms + 110ms sprk max velocity lag - public static final double kMaxSpeed = Units.feetToMeters(14.5); - // Maximum speed of the robot in meters per second, used to limit acceleration. } /** Power Distribution Module Constants ********************************** */ @@ -138,15 +136,69 @@ public static final class PowerConstants { /** Autonomous Action Constants ****************************************** */ public static final class AutonConstants { + + // Translation PID constants public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); + // Rotation PID constants public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); } /** Drive Base Constants ************************************************* */ public static final class DrivebaseConstants { + // Physical size of the drive base + private static final double TRACK_WIDTH_X = Units.inchesToMeters(20.75); + private static final double TRACK_WIDTH_Y = Units.inchesToMeters(20.75); + public static final double DRIVE_BASE_RADIUS = + Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); + + // Maximum chassis speeds desired for robot motion -- metric / radians + public static final double MAX_LINEAR_SPEED = Units.feetToMeters(18); // ft/s + public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + // Maximum chassis accelerations desired for robot motion -- metric / radians + public static final double MAX_LINEAR_ACCEL = 4.0; // m/s/s + public static final double MAX_ANGULAR_ACCEL = Units.degreesToRadians(720); // deg/s/s + + // Wheel radius + public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + + // ** Gear ratios for SDS MK4i L2, adjust as necessary ** + public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + public static final double TURN_GEAR_RATIO = 150.0 / 7.0; + // Hold time on motor brakes when disabled public static final double WHEEL_LOCK_TIME = 10; // seconds + + // SysID characterization constants + public static final double kMaxV = 12.0; // Max volts + public static final double kDelay = 3.0; // seconds + public static final double kQuasiTimeout = 5.0; // seconds + public static final double kDynamicTimeout = 3.0; // seconds + } + + /** Example Flywheel Mechanism Constants ********************************* */ + public static final class FlywheelConstants { + + // Mechanism motor gear ratio + public static final double GEAR_RATIO = 1.5; + + // MODE == REAL / REPLAY + // Feedforward constants + public static final double kStaticGainReal = 0.1; + public static final double kVelocityGainReal = 0.05; + // Feedback (PID) constants + public static final double kPReal = 1.0; + public static final double kIReal = 0.0; + public static final double kDReal = 0.0; + + // MODE == SIM + // Feedforward constants + public static final double kStaticGainSim = 0.0; + public static final double kVelocityGainSim = 0.03; + // Feedback (PID) constants + public static final double kPSim = 1.0; + public static final double kISim = 0.0; + public static final double kDSim = 0.0; } /** Operator Constants *************************************************** */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 933a91f..e932512 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -248,7 +248,8 @@ public static class Ports { /* SUBSYSTEM CAN DEVICE IDS */ // This is where mechanism subsystem devices are defined // Example: - // public static final CanDeviceId ELEVATOR_MAIN = new CanDeviceId(3, ""); + public static final CanDeviceId FLYWHEEL_LEADER = new CanDeviceId(3, ""); + public static final CanDeviceId FLYWHEEL_FOLLOWER = new CanDeviceId(4, ""); /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ // This is where digital I/O feedback devices are defined diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java index 325bc9e..db24d82 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -14,6 +14,7 @@ package frc.robot.subsystems.flywheel_example; import static edu.wpi.first.units.Units.*; +import static frc.robot.Constants.FlywheelConstants.*; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; @@ -39,12 +40,12 @@ public Flywheel(FlywheelIO io) { switch (Constants.getMode()) { case REAL: case REPLAY: - ffModel = new SimpleMotorFeedforward(0.1, 0.05); - io.configurePID(1.0, 0.0, 0.0); + ffModel = new SimpleMotorFeedforward(kStaticGainReal, kVelocityGainReal); + io.configurePID(kPReal, kIReal, kDReal); break; case SIM: - ffModel = new SimpleMotorFeedforward(0.0, 0.03); - io.configurePID(0.5, 0.0, 0.0); + ffModel = new SimpleMotorFeedforward(kStaticGainSim, kVelocityGainSim); + io.configurePID(kPSim, kISim, kDSim); break; default: ffModel = new SimpleMotorFeedforward(0.0, 0.0); @@ -98,7 +99,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { } /** Returns the current velocity in RPM. */ - @AutoLogOutput + @AutoLogOutput(key = "Mechanism/Flywheel") public double getVelocityRPM() { return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); } diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java index f9990aa..5c6c979 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java @@ -13,6 +13,8 @@ package frc.robot.subsystems.flywheel_example; +import static frc.robot.Constants.FlywheelConstants.*; + import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; @@ -20,16 +22,19 @@ import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; import edu.wpi.first.math.util.Units; +import frc.robot.RobotContainer.Ports; /** * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with * "CANSparkFlex". */ public class FlywheelIOSparkMax implements FlywheelIO { - private static final double GEAR_RATIO = 1.5; - private final CANSparkMax leader = new CANSparkMax(0, MotorType.kBrushless); - private final CANSparkMax follower = new CANSparkMax(1, MotorType.kBrushless); + // Define the leader / follower motors from the Ports section of RobotContainer + private final CANSparkMax leader = + new CANSparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); + private final CANSparkMax follower = + new CANSparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); private final RelativeEncoder encoder = leader.getEncoder(); private final SparkPIDController pid = leader.getPIDController(); diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java index 0cd34f4..b5f99f9 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -13,6 +13,8 @@ package frc.robot.subsystems.flywheel_example; +import static frc.robot.Constants.FlywheelConstants.*; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Slot0Configs; @@ -23,12 +25,15 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; +import frc.robot.RobotContainer.Ports; public class FlywheelIOTalonFX implements FlywheelIO { - private static final double GEAR_RATIO = 1.5; - private final TalonFX leader = new TalonFX(0); - private final TalonFX follower = new TalonFX(1); + // Define the leader / follower motors from the Ports section of RobotContainer + private final TalonFX leader = + new TalonFX(Ports.FLYWHEEL_LEADER.getDeviceNumber(), Ports.FLYWHEEL_LEADER.getBus()); + private final TalonFX follower = + new TalonFX(Ports.FLYWHEEL_FOLLOWER.getDeviceNumber(), Ports.FLYWHEEL_FOLLOWER.getBus()); private final StatusSignal leaderPosition = leader.getPosition(); private final StatusSignal leaderVelocity = leader.getVelocity(); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index a213403..1a9331b 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -19,6 +19,8 @@ package frc.robot.subsystems.swervedrive; +import static frc.robot.Constants.DrivebaseConstants.*; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.PathConstraints; @@ -26,25 +28,23 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AutonConstants; -import frc.robot.Constants.PhysicalConstants; import frc.robot.subsystems.vision.Vision; import java.io.File; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; @@ -75,39 +75,44 @@ public class SwerveSubsystem extends SubsystemBase { * @param directory Directory of swerve drive config files. */ public SwerveSubsystem(File directory) { + // Angle conversion factor is 360 / (GEAR RATIO * ENCODER RESOLUTION) - // In this case the gear ratio is 12.8 motor revolutions per wheel rotation. - // The encoder resolution per motor revolution is 1 per motor revolution. - double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(12.8); + double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(TURN_GEAR_RATIO); + // Motor conversion factor is (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO * ENCODER - // RESOLUTION). - // In this case the wheel diameter is 4 inches, which must be converted to meters to get - // meters/second. - // The gear ratio is 6.75 motor revolutions per wheel rotation. - // The encoder resolution per motor revolution is 1 per motor revolution. + // RESOLUTION) double driveConversionFactor = - SwerveMath.calculateMetersPerRotation(Units.inchesToMeters(4), 6.75); + SwerveMath.calculateMetersPerRotation(WHEEL_RADIUS * 2.0, DRIVE_GEAR_RATIO); + + // Output to console and to AdvantageKit System.out.println("\"conversionFactors\": {"); System.out.println("\t\"angle\": {\"factor\": " + angleConversionFactor + " },"); System.out.println("\t\"drive\": {\"factor\": " + driveConversionFactor + " }"); System.out.println("}"); + Logger.recordOutput("SwerveDive/ConversionFactors/Angle", angleConversionFactor); + Logger.recordOutput("SwerveDive/ConversionFactors/Drive", driveConversionFactor); // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being // created. SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { - swerveDrive = new SwerveParser(directory).createSwerveDrive(PhysicalConstants.kMaxSpeed); + swerveDrive = new SwerveParser(directory).createSwerveDrive(MAX_LINEAR_SPEED); // Alternative method if you don't want to supply the conversion factor via JSON files. // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, // angleConversionFactor, driveConversionFactor); } catch (Exception e) { throw new RuntimeException(e); } - swerveDrive.setHeadingCorrection( - false); // Heading correction should only be used while controlling the robot via angle. - swerveDrive.setCosineCompensator( - false); // !SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for - // simulations since it causes discrepancies not seen in real life. + // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setHeadingCorrection(false); + // Disables cosine compensation for simulations since it causes discrepancies not seen in real + // life. + if (SwerveDriveTelemetry.isSimulation) { + swerveDrive.setCosineCompensator(false); + } else { + swerveDrive.setCosineCompensator(true); + } + if (visionDriveTest) { setupPhotonVision(); // Stop the odometry thread if we are using vision that way we can synchronize updates better. @@ -124,7 +129,7 @@ public SwerveSubsystem(File directory) { */ public SwerveSubsystem( SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { - swerveDrive = new SwerveDrive(driveCfg, controllerCfg, PhysicalConstants.kMaxSpeed); + swerveDrive = new SwerveDrive(driveCfg, controllerCfg, MAX_LINEAR_SPEED); } /** Setup the photon vision class. */ @@ -132,40 +137,19 @@ public void setupPhotonVision() { vision = new Vision(swerveDrive::getPose, swerveDrive.field); } - @Override - public void periodic() { - // When vision is enabled we must manually update odometry in SwerveDrive - if (visionDriveTest) { - swerveDrive.updateOdometry(); - vision.updatePoseEstimation(swerveDrive); - } - } - - @Override - public void simulationPeriodic() {} - /** Setup AutoBuilder for PathPlanner. */ public void setupPathPlanner() { AutoBuilder.configureHolonomic( this::getPose, // Robot pose supplier - this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting - // pose) + this::resetOdometry, // Method to reset odometry (called if your auto has a starting pose) this::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE - // ChassisSpeeds - new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in - // your Constants class + this::setChassisSpeeds, // Driver method given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( AutonConstants.TRANSLATION_PID, - // Translation PID constants AutonConstants.ANGLE_PID, - // Rotation PID constants - 4.5, - // Max module speed, in m/s - swerveDrive.swerveDriveConfiguration.getDriveBaseRadiusMeters(), - // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() - // Default path replanning config. See the API for the options here - ), + MAX_LINEAR_SPEED, + DRIVE_BASE_RADIUS, + new ReplanningConfig()), () -> { // Boolean supplier that controls when the path will be mirrored for the red alliance // This will flip the path being followed to the red side of the field. @@ -177,42 +161,90 @@ public void setupPathPlanner() { ); } + /** Periodic function -- update odometry and log everything */ + @Override + public void periodic() { + // When vision is enabled we must manually update odometry in SwerveDrive + if (visionDriveTest) { + swerveDrive.updateOdometry(); + vision.updatePoseEstimation(swerveDrive); + } + + /** Log Telemetry Data to AdvantageKit */ + Logger.recordOutput("SwerveDive/Telemetry/moduleCount", SwerveDriveTelemetry.moduleCount); + Logger.recordOutput("SwerveDive/Telemetry/wheelLocations", SwerveDriveTelemetry.wheelLocations); + Logger.recordOutput("SwerveDive/Telemetry/measuredStates", SwerveDriveTelemetry.measuredStates); + Logger.recordOutput("SwerveDive/Telemetry/desiredStates", SwerveDriveTelemetry.desiredStates); + Logger.recordOutput("SwerveDive/Telemetry/robotRotation", SwerveDriveTelemetry.robotRotation); + Logger.recordOutput("SwerveDive/Telemetry/maxSpeed", SwerveDriveTelemetry.maxSpeed); + Logger.recordOutput("SwerveDive/Telemetry/rotationUnit", SwerveDriveTelemetry.rotationUnit); + Logger.recordOutput("SwerveDive/Telemetry/sizeLeftRight", SwerveDriveTelemetry.sizeLeftRight); + Logger.recordOutput("SwerveDive/Telemetry/sizeFrontBack", SwerveDriveTelemetry.sizeFrontBack); + Logger.recordOutput( + "SwerveDive/Telemetry/forwardDirection", SwerveDriveTelemetry.forwardDirection); + Logger.recordOutput( + "SwerveDive/Telemetry/maxAngularVelocity", SwerveDriveTelemetry.maxAngularVelocity); + Logger.recordOutput( + "SwerveDive/Telemetry/measuredChassisSpeeds", SwerveDriveTelemetry.measuredChassisSpeeds); + Logger.recordOutput( + "SwerveDive/Telemetry/desiredChassisSpeeds", SwerveDriveTelemetry.desiredChassisSpeeds); + + /** Log Swerve Drive States to AdvantageKit */ + getModuleStates(); + getDesiredStates(); + Logger.recordOutput( + "SwerveDive/States/RobotRotation", + SwerveDriveTelemetry.rotationUnit == "degrees" + ? Rotation2d.fromDegrees(SwerveDriveTelemetry.robotRotation) + : Rotation2d.fromRadians(SwerveDriveTelemetry.robotRotation)); + } + + /** Simulation periodic function */ + @Override + public void simulationPeriodic() {} + /** - * Get the path follower with events. + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation + * rate, and calculates and commands module states accordingly. Can use either open-loop or + * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative + * modes, which affect how the translation vector is used. * - * @param pathName PathPlanner path name. - * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in + * meters per second. In robot-relative mode, positive x is torwards the bow (front) and + * positive y is torwards port (left). In field-relative mode, positive x is away from the + * alliance wall (field North) and positive y is torwards the left wall when looking through + * the driver station glass (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by + * field/robot relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. */ - public Command getAutonomousCommand(String pathName) { - // Create a path following command using AutoBuilder. This will also trigger event markers. - return new PathPlannerAuto(pathName); + public void drive(Translation2d translation, double rotation, boolean fieldRelative) { + + // Open loop is disabled since it shouldn't be used most of the time. + swerveDrive.drive(translation, rotation, fieldRelative, false); } /** - * Use PathPlanner Path finding to go to a point on the field. + * Drive the robot given a chassis field oriented velocity. * - * @param pose Target {@link Pose2d} to go to. - * @return PathFinding command + * @param velocity Velocity according to the field. */ - public Command driveToPose(Pose2d pose) { - // Create the constraints to use while pathfinding - PathConstraints constraints = - new PathConstraints( - swerveDrive.getMaximumVelocity(), - 4.0, - swerveDrive.getMaximumAngularVelocity(), - Units.degreesToRadians(720)); + public void driveFieldOriented(ChassisSpeeds velocity) { + swerveDrive.driveFieldOriented(velocity); + } - // Since AutoBuilder is configured, we can use it to build pathfinding commands - return AutoBuilder.pathfindToPose( - pose, - constraints, - 0.0, // Goal end velocity in meters/sec - 0.0 // Rotation delay distance in meters. This is how far the robot should travel before - // attempting to rotate. - ); + /** + * Drive according to the chassis robot oriented velocity. + * + * @param velocity Robot oriented {@link ChassisSpeeds} + */ + public void drive(ChassisSpeeds velocity) { + swerveDrive.drive(velocity); } + /************************************************************************* */ + /* COMMAND SECTION -- Swerve-only Commands */ + /** * Command to drive the robot using translative values and heading as a setpoint. * @@ -247,6 +279,32 @@ public Command driveCommand( }); } + /** + * Command to drive the robot using translative values and heading as angular velocity. NOTE: + * Alternate drive command constructor + * + * @param translationX Translation in the X direction. Cubed for smoother controls. + * @param translationY Translation in the Y direction. Cubed for smoother controls. + * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. + * @return Drive command. + */ + public Command driveCommand( + DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) { + return run( + () -> { + // Make the robot move + swerveDrive.drive( + SwerveMath.scaleTranslation( + new Translation2d( + translationX.getAsDouble() * swerveDrive.getMaximumVelocity(), + translationY.getAsDouble() * swerveDrive.getMaximumVelocity()), + 0.8), + Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumAngularVelocity(), + true, + false); + }); + } + /** * Command to drive the robot using translative values and heading as a setpoint. * @@ -273,96 +331,73 @@ public Command simDriveCommand( } /** - * Command to characterize the robot drive motors using SysId + * Get the path follower with events. * - * @return SysId Drive Command + * @param pathName PathPlanner path name. + * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. */ - public Command sysIdDriveMotorCommand() { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setDriveSysIdRoutine(new Config(), this, swerveDrive, 12), 3.0, 5.0, 3.0); + public Command getAutonomousCommand(String pathName) { + // Create a path following command using AutoBuilder. This will also trigger event markers. + return new PathPlannerAuto(pathName); } /** - * Command to characterize the robot angle motors using SysId + * Use PathPlanner Path finding to go to a point on the field. * - * @return SysId Angle Command + * @param pose Target {@link Pose2d} to go to. + * @return PathFinding command */ - public Command sysIdAngleMotorCommand() { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setAngleSysIdRoutine(new Config(), this, swerveDrive), 3.0, 5.0, 3.0); - } + public Command driveToPose(Pose2d pose) { + // Create the constraints to use while pathfinding + PathConstraints constraints = + new PathConstraints( + MAX_LINEAR_SPEED, MAX_LINEAR_ACCEL, MAX_ANGULAR_SPEED, MAX_ANGULAR_ACCEL); - /** - * Command to drive the robot using translative values and heading as angular velocity. - * - * @param translationX Translation in the X direction. Cubed for smoother controls. - * @param translationY Translation in the Y direction. Cubed for smoother controls. - * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. - * @return Drive command. - */ - public Command driveCommand( - DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) { - return run( - () -> { - // Make the robot move - swerveDrive.drive( - SwerveMath.scaleTranslation( - new Translation2d( - translationX.getAsDouble() * swerveDrive.getMaximumVelocity(), - translationY.getAsDouble() * swerveDrive.getMaximumVelocity()), - 0.8), - Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumAngularVelocity(), - true, - false); - }); + // Since AutoBuilder is configured, we can use it to build pathfinding commands + return AutoBuilder.pathfindToPose( + pose, + constraints, + 0.0, // Goal end velocity in meters/sec + 0.0 // Rotation delay distance in meters. This is how far the robot should travel before + // attempting to rotate. + ); } /** - * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation - * rate, and calculates and commands module states accordingly. Can use either open-loop or - * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative - * modes, which affect how the translation vector is used. + * Command to characterize the robot drive motors using SysId * - * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in - * meters per second. In robot-relative mode, positive x is torwards the bow (front) and - * positive y is torwards port (left). In field-relative mode, positive x is away from the - * alliance wall (field North) and positive y is torwards the left wall when looking through - * the driver station glass (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by - * field/robot relativity. - * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + * @return SysId Drive Command */ - public void drive(Translation2d translation, double rotation, boolean fieldRelative) { - swerveDrive.drive( - translation, - rotation, - fieldRelative, - false); // Open loop is disabled since it shouldn't be used most of the time. + public Command sysIdDriveMotorCommand() { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setDriveSysIdRoutine(new Config(), this, swerveDrive, kMaxV), + kDelay, + kQuasiTimeout, + kDynamicTimeout); } /** - * Drive the robot given a chassis field oriented velocity. + * Command to characterize the robot angle motors using SysId * - * @param velocity Velocity according to the field. + * @return SysId Angle Command */ - public void driveFieldOriented(ChassisSpeeds velocity) { - swerveDrive.driveFieldOriented(velocity); + public Command sysIdAngleMotorCommand() { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setAngleSysIdRoutine(new Config(), this, swerveDrive), + kDelay, + kQuasiTimeout, + kDynamicTimeout); } - /** - * Drive according to the chassis robot oriented velocity. - * - * @param velocity Robot oriented {@link ChassisSpeeds} - */ - public void drive(ChassisSpeeds velocity) { - swerveDrive.drive(velocity); - } + /************************************************************************* */ + /* UTILITY SECTION -- Utility methods */ /** * Get the swerve drive kinematics object. * * @return {@link SwerveDriveKinematics} of the swerve drive. */ + @AutoLogOutput(key = "Odometry/Kinematics") public SwerveDriveKinematics getKinematics() { return swerveDrive.kinematics; } @@ -383,6 +418,7 @@ public void resetOdometry(Pose2d initialHolonomicPose) { * * @return The robot's pose */ + @AutoLogOutput(key = "Odometry/RobotPose") public Pose2d getPose() { return swerveDrive.getPose(); } @@ -453,6 +489,7 @@ public void setMotorBrake(boolean brake) { * * @return The yaw angle */ + @AutoLogOutput(key = "Odometry/Heading") public Rotation2d getHeading() { return getPose().getRotation(); } @@ -476,7 +513,7 @@ public ChassisSpeeds getTargetSpeeds( headingX, headingY, getHeading().getRadians(), - PhysicalConstants.kMaxSpeed); + MAX_LINEAR_SPEED); } /** @@ -496,7 +533,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an scaledInputs.getY(), angle.getRadians(), getHeading().getRadians(), - PhysicalConstants.kMaxSpeed); + MAX_LINEAR_SPEED); } /** @@ -504,6 +541,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an * * @return A ChassisSpeeds object of the current field-relative velocity */ + @AutoLogOutput(key = "Odometry/FieldVelocity") public ChassisSpeeds getFieldVelocity() { return swerveDrive.getFieldVelocity(); } @@ -513,6 +551,7 @@ public ChassisSpeeds getFieldVelocity() { * * @return A {@link ChassisSpeeds} object of the current velocity */ + @AutoLogOutput(key = "Odometry/RobotVelocity") public ChassisSpeeds getRobotVelocity() { return swerveDrive.getRobotVelocity(); } @@ -529,8 +568,9 @@ public SwerveController getSwerveController() { /** * Get the {@link SwerveDriveConfiguration} object. * - * @return The {@link SwerveDriveConfiguration} fpr the current drive. + * @return The {@link SwerveDriveConfiguration} for the current drive. */ + @AutoLogOutput(key = "SwerveDrive/Config") public SwerveDriveConfiguration getSwerveDriveConfiguration() { return swerveDrive.swerveDriveConfiguration; } @@ -545,6 +585,7 @@ public void lock() { * * @return The heading as a {@link Rotation2d} angle */ + @AutoLogOutput(key = "Odometry/Pitch") public Rotation2d getPitch() { return swerveDrive.getPitch(); } @@ -555,36 +596,34 @@ public void addFakeVisionReading() { new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); } - /*****************************************************************/ - /** 2024 SEASON-SPECIFIC FUNCTIONS, INCLUDED AS EXAMPLES */ - /** - * Get the distance to the speaker. - * - * @return Distance to speaker in meters. - */ - public double getDistanceToSpeaker() { - int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; - // Taken from PhotonUtils.getDistanceToPose - Pose3d speakerAprilTagPose = - AprilTagConstants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); - return getPose().getTranslation().getDistance(speakerAprilTagPose.toPose2d().getTranslation()); + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveDive/States/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = + new SwerveModuleState( + SwerveDriveTelemetry.measuredStates[(i * 2) + 1], + Rotation2d.fromDegrees(SwerveDriveTelemetry.measuredStates[i * 2])); + } + return states; + } + + /** Returns the desired states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveDive/States/Desired") + private SwerveModuleState[] getDesiredStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = + new SwerveModuleState( + SwerveDriveTelemetry.desiredStates[(i * 2) + 1], + Rotation2d.fromDegrees(SwerveDriveTelemetry.desiredStates[i * 2])); + } + return states; } - /** - * Get the yaw to aim at the speaker. - * - * @return {@link Rotation2d} of which you need to achieve. - */ - public Rotation2d getSpeakerYaw() { - int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; - // Taken from PhotonUtils.getYawToPose() - Pose3d speakerAprilTagPose = - AprilTagConstants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); - Translation2d relativeTrl = - speakerAprilTagPose.toPose2d().relativeTo(getPose()).getTranslation(); - return new Rotation2d(relativeTrl.getX(), relativeTrl.getY()) - .plus(swerveDrive.getOdometryHeading()); - } + /************************************************************************* */ + /** 2024 SEASON-SPECIFIC FUNCTIONS, INCLUDED AS EXAMPLES */ /** * Aim the robot at the speaker. @@ -600,10 +639,17 @@ public Command aimAtSpeaker(double tolerance) { 0, 0, controller.headingCalculate( - getHeading().getRadians(), getSpeakerYaw().getRadians()), + getHeading().getRadians(), + vision.getSpeakerYaw(swerveDrive.getOdometryHeading()).getRadians()), getHeading())); }) - .until(() -> getSpeakerYaw().minus(getHeading()).getDegrees() < tolerance); + .until( + () -> + vision + .getSpeakerYaw(swerveDrive.getOdometryHeading()) + .minus(getHeading()) + .getDegrees() + < tolerance); } /** diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 2ac10d5..2b922e0 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -27,10 +27,13 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; @@ -485,4 +488,37 @@ public void addToVisionSim(VisionSystemSim systemSim) { } } } + + /*****************************************************************/ + /** 2024 SEASON-SPECIFIC FUNCTIONS, INCLUDED AS EXAMPLES */ + /** + * Get the distance to the speaker. + * + * @return Distance to speaker in meters. + */ + public double getDistanceToSpeaker() { + int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; + // Taken from PhotonUtils.getDistanceToPose + Pose3d speakerAprilTagPose = + AprilTagConstants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); + return currentPose + .get() + .getTranslation() + .getDistance(speakerAprilTagPose.toPose2d().getTranslation()); + } + + /** + * Get the yaw to aim at the speaker. + * + * @return {@link Rotation2d} of which you need to achieve. + */ + public Rotation2d getSpeakerYaw(Rotation2d odometryHeading) { + int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; + // Taken from PhotonUtils.getYawToPose() + Pose3d speakerAprilTagPose = + AprilTagConstants.aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); + Translation2d relativeTrl = + speakerAprilTagPose.toPose2d().relativeTo(currentPose.get()).getTranslation(); + return new Rotation2d(relativeTrl.getX(), relativeTrl.getY()).plus(odometryHeading); + } }