diff --git a/build.gradle b/build.gradle index 1a1866a..1402168 100644 --- a/build.gradle +++ b/build.gradle @@ -135,7 +135,7 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { - options.compilerArgs.add '-XDstringConcat=inline' + options.compilerArgs << '-XDstringConcat=inline' << '-Xlint:unchecked' } // Create version file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3e4e620..bf06caa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -52,10 +52,9 @@ public final class Constants { * Define the various multiple robots that use this same code (e.g., COMPBOT, DEVBOT, SIMBOT, * etc.) and the operating modes of the code (REAL, SIM, or REPLAY) */ - private static RobotType robotType = RobotType.SIMBOT; + private static RobotType robotType = RobotType.COMPBOT; private static SwerveType swerveType = SwerveType.PHOENIX6; - private static AutoType autoType = AutoType.PATHPLANNER; public static boolean disableHAL = false; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fdb1de8..6cd3c0c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -144,8 +144,17 @@ public void disabledPeriodic() { @Override public void autonomousInit() { m_robotContainer.setMotorBrake(true); - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - + switch (Constants.getAutoType()) { + case PATHPLANNER: + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + break; + case CHOREO: + m_autonomousCommand = m_robotContainer.getAutonomousCommandChoreo(); + break; + default: + throw new RuntimeException( + "Incorrect AUTO type selected in Constants: " + Constants.getAutoType()); + } // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 95bfe3d..1fbaeba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -65,8 +65,9 @@ public class RobotContainer { final CommandXboxController operatorXbox = new CommandXboxController(1); final OverrideSwitches overrides = new OverrideSwitches(2); + // Autonomous Things Field2d m_field = new Field2d(); - ChoreoTrajectory traj; + ChoreoTrajectory m_traj; // Declare the robot subsystems here private final Drive m_drivebase; @@ -95,10 +96,6 @@ public static AprilTagLayoutType staticGetAprilTagLayoutType() { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - traj = Choreo.getTrajectory("Trajectory"); - - m_field.getObject("traj").setPoses(traj.getInitialPose(), traj.getFinalPose()); - m_field.getObject("trajPoses").setPoses(traj.getPoses()); // Instantiate Robot Subsystems based on RobotType switch (Constants.getMode()) { @@ -136,14 +133,15 @@ public RobotContainer() { // as that is automatically monitored. m_power = new PowerMonitoring(m_flywheel); + // Set up the SmartDashboard Auto Chooser + autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + // Configure the trigger bindings configureBindings(); // Define Auto commands defineAutoCommands(); // Define SysIs Routines definesysIdRoutines(); - // Set up the SmartDashboard Auto Chooser - autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); } /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */ @@ -250,10 +248,16 @@ public Command getAutonomousCommand() { * @return the command to run in autonomous */ public Command getAutonomousCommandChoreo() { + + m_traj = Choreo.getTrajectory("Trajectory"); + + m_field.getObject("traj").setPoses(m_traj.getInitialPose(), m_traj.getFinalPose()); + m_field.getObject("trajPoses").setPoses(m_traj.getPoses()); + var thetaController = new PIDController(AutoConstants.kPThetaController, 0, 0); thetaController.enableContinuousInput(-Math.PI, Math.PI); - m_drivebase.setPose(traj.getInitialPose()); + m_drivebase.setPose(m_traj.getInitialPose()); boolean isFlipped = DriverStation.getAlliance().isPresent() @@ -262,7 +266,7 @@ public Command getAutonomousCommandChoreo() { Command swerveCommand = Choreo.choreoSwerveCommand( // Choreo trajectory from above - traj, + m_traj, // A function that returns the current field-relative pose of the robot: your wheel or // vision odometry m_drivebase::getPose, @@ -288,7 +292,7 @@ public Command getAutonomousCommandChoreo() { m_drivebase); return Commands.sequence( - Commands.runOnce(() -> m_drivebase.setPose(traj.getInitialPose())), + Commands.runOnce(() -> m_drivebase.setPose(m_traj.getInitialPose())), swerveCommand, m_drivebase.run(() -> m_drivebase.stop())); } diff --git a/src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java b/src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java deleted file mode 100644 index fa2f309..0000000 --- a/src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java +++ /dev/null @@ -1,102 +0,0 @@ -// // Copyright (c) 2024 Az-FIRST -// // http://github.com/AZ-First -// // -// // This program is free software; you can redistribute it and/or -// // modify it under the terms of the GNU General Public License -// // version 3 as published by the Free Software Foundation or -// // available in the root directory of this project. -// // -// // This program is distributed in the hope that it will be useful, -// // but WITHOUT ANY WARRANTY; without even the implied warranty of -// // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// // GNU General Public License for more details. -// // -// // Copyright (c) FIRST and other WPILib contributors. -// // Open Source Software; you can modify and/or share it under the terms of -// // the WPILib BSD license file in the root directory of this project. -// // -// // NOTE: This module from the YAGSL Example Project - -// package frc.robot.commands.swervedrive.auto; - -// import edu.wpi.first.math.MathUtil; -// import edu.wpi.first.math.controller.PIDController; -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; - -// /** -// * Auto Balance command using a simple PID controller. Created by Team 3512 ... -// */ -// public class YAGSL_AutoBalanceCommand extends Command { - -// private final YAGSLSwerve swerveSubsystem; -// private final PIDController controller; - -// public YAGSL_AutoBalanceCommand(YAGSLSwerve swerveSubsystem) { -// this.swerveSubsystem = swerveSubsystem; -// controller = new PIDController(1.0, 0.0, 0.0); -// controller.setTolerance(1); -// controller.setSetpoint(0.0); -// // each subsystem used by the command must be passed into the -// // addRequirements() method (which takes a vararg of Subsystem) -// addRequirements(this.swerveSubsystem); -// } - -// /** The initial subroutine of a command. Called once when the command is initially scheduled. -// */ -// @Override -// public void initialize() {} - -// /** -// * The main body of a command. Called repeatedly while the command is scheduled. (That is, it -// is -// * called repeatedly until {@link #isFinished()}) returns true.) -// */ -// @Override -// public void execute() { -// SmartDashboard.putBoolean("At Tolerance", controller.atSetpoint()); - -// double translationVal = -// MathUtil.clamp( -// controller.calculate(swerveSubsystem.getPitch().getDegrees(), 0.0), -0.5, 0.5); -// swerveSubsystem.drive(new Translation2d(translationVal, 0.0), 0.0, true); -// } - -// /** -// * Returns whether this command has finished. Once a command finishes -- indicated by this -// method -// * returning true -- the scheduler will call its {@link #end(boolean)} method. -// * -// *

Returning false will result in the command never ending automatically. It may still be -// * cancelled manually or interrupted by another command. Hard coding this command to always -// return -// * true will result in the command executing once and finishing immediately. It is recommended -// to -// * use * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} for such an -// * operation. -// * -// * @return whether this command has finished. -// */ -// @Override -// public boolean isFinished() { -// return controller.atSetpoint(); -// } - -// /** -// * The action to take when the command ends. Called when either the command finishes normally -// -- -// * that is it is called when {@link #isFinished()} returns true -- or when it is -// * interrupted/canceled. This is where you may want to wrap up loose ends, like shutting off a -// * motor that was being used in the command. -// * -// * @param interrupted whether the command was interrupted/canceled -// */ -// @Override -// public void end(boolean interrupted) { -// swerveSubsystem.lock(); -// } -// } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java deleted file mode 100644 index da035d8..0000000 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java +++ /dev/null @@ -1,140 +0,0 @@ -// // Copyright (c) 2024 Az-FIRST -// // http://github.com/AZ-First -// // -// // This program is free software; you can redistribute it and/or -// // modify it under the terms of the GNU General Public License -// // version 3 as published by the Free Software Foundation or -// // available in the root directory of this project. -// // -// // This program is distributed in the hope that it will be useful, -// // but WITHOUT ANY WARRANTY; without even the implied warranty of -// // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// // GNU General Public License for more details. -// // -// // Copyright (c) FIRST and other WPILib contributors. -// // Open Source Software; you can modify and/or share it under the terms of -// // the WPILib BSD license file in the root directory of this project. -// // -// // NOTE: This module from the YAGSL Example Project - -// package frc.robot.commands.swervedrive.drivebase; - -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.Constants.PhysicalConstants; -// import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; -// import java.util.List; -// import java.util.function.DoubleSupplier; -// import swervelib.SwerveController; -// import swervelib.math.SwerveMath; - -// /** An example command that uses an example subsystem. */ -// public class YAGSL_AbsoluteDrive extends Command { - -// private final YAGSLSwerve swerve; -// private final DoubleSupplier vX, vY; -// private final DoubleSupplier headingHorizontal, headingVertical; -// private boolean initRotation = false; - -// /** -// * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation -// inputs, -// * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and -// * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— -// * they will be converted to a polar angle, which the robot will rotate to. -// * -// * @param swerve The swerve drivebase subsystem. -// * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the -// range -// * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. -// * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the -// range -// * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when -// * looking through the driver station glass. -// * @param headingHorizontal DoubleSupplier that supplies the horizontal component of the -// robot's -// * heading angle. In the robot coordinate system, this is along the same axis as vY. Should -// * range from -1 to 1 with no deadband. Positive is towards the left wall when looking -// through -// * the driver station glass. -// * @param headingVertical DoubleSupplier that supplies the vertical component of the robot's -// * heading angle. In the robot coordinate system, this is along the same axis as vX. Should -// * range from -1 to 1 with no deadband. Positive is away from the alliance wall. -// */ -// public YAGSL_AbsoluteDrive( -// YAGSLSwerve swerve, -// DoubleSupplier vX, -// DoubleSupplier vY, -// DoubleSupplier headingHorizontal, -// DoubleSupplier headingVertical) { -// this.swerve = swerve; -// this.vX = vX; -// this.vY = vY; -// this.headingHorizontal = headingHorizontal; -// this.headingVertical = headingVertical; - -// addRequirements(swerve); -// } - -// @Override -// public void initialize() { -// initRotation = true; -// } - -// // Called every time the scheduler runs while the command is scheduled. -// @Override -// public void execute() { - -// // Get the desired chassis speeds based on a 2 joystick module. -// ChassisSpeeds desiredSpeeds = -// swerve.getTargetSpeeds( -// vX.getAsDouble(), -// vY.getAsDouble(), -// headingHorizontal.getAsDouble(), -// headingVertical.getAsDouble()); - -// // Prevent Movement After Auto -// if (initRotation) { -// if (headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0) { -// // Get the curretHeading -// Rotation2d firstLoopHeading = swerve.getHeading(); - -// // Set the Current Heading to the desired Heading -// desiredSpeeds = -// swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); -// } -// // Dont Init Rotation Again -// initRotation = false; -// } - -// // Limit velocity to prevent tippy -// Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); -// translation = -// SwerveMath.limitVelocity( -// translation, -// swerve.getFieldVelocity(), -// swerve.getPose(), -// PhysicalConstants.kLoopTime, -// PhysicalConstants.kRobotMass, -// List.of(PhysicalConstants.kChassis), -// swerve.getSwerveDriveConfiguration()); -// SmartDashboard.putNumber("LimitedTranslation", translation.getX()); -// SmartDashboard.putString("Translation", translation.toString()); - -// // Make the robot move -// swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); -// } - -// // Called once the command ends or is interrupted. -// @Override -// public void end(boolean interrupted) {} - -// // Returns true when the command should end. -// @Override -// public boolean isFinished() { -// return false; -// } -// } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java deleted file mode 100644 index d959948..0000000 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java +++ /dev/null @@ -1,171 +0,0 @@ -// // Copyright (c) 2024 Az-FIRST -// // http://github.com/AZ-First -// // -// // This program is free software; you can redistribute it and/or -// // modify it under the terms of the GNU General Public License -// // version 3 as published by the Free Software Foundation or -// // available in the root directory of this project. -// // -// // This program is distributed in the hope that it will be useful, -// // but WITHOUT ANY WARRANTY; without even the implied warranty of -// // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// // GNU General Public License for more details. -// // -// // Copyright (c) FIRST and other WPILib contributors. -// // Open Source Software; you can modify and/or share it under the terms of -// // the WPILib BSD license file in the root directory of this project. -// // -// // NOTE: This module from the YAGSL Example Project - -// package frc.robot.commands.swervedrive.drivebase; - -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.Constants.OperatorConstants; -// import frc.robot.Constants.PhysicalConstants; -// import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; -// import java.util.List; -// import java.util.function.BooleanSupplier; -// import java.util.function.DoubleSupplier; -// import swervelib.SwerveController; -// import swervelib.math.SwerveMath; - -// /** A more advanced Swerve Control System that has 4 buttons for which direction to face */ -// public class YAGSL_AbsoluteDriveAdv extends Command { - -// private final YAGSLSwerve swerve; -// private final DoubleSupplier vX, vY; -// private final DoubleSupplier headingAdjust; -// private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; -// private boolean resetHeading = false; - -// /** -// * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation -// inputs, -// * where x is torwards/away from alliance wall and y is left/right. Heading Adjust changes the -// * current heading after being multipied by a constant. The look booleans are shortcuts to get -// the -// * robot to face a certian direction. Based off of ideas in -// * https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 -// * -// * @param swerve The swerve drivebase subsystem. -// * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the -// range -// * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. -// * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the -// range -// * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when -// * looking through the driver station glass. -// * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle -// * that should be adjusted. Should range from -1 to 1 with deadband already accounted for. -// * @param lookAway Face the robot towards the opposing alliance's wall in the same direction -// the -// * driver is facing -// * @param lookTowards Face the robot towards the driver -// * @param lookLeft Face the robot left -// * @param lookRight Face the robot right -// */ -// public YAGSL_AbsoluteDriveAdv( -// YAGSLSwerve swerve, -// DoubleSupplier vX, -// DoubleSupplier vY, -// DoubleSupplier headingAdjust, -// BooleanSupplier lookAway, -// BooleanSupplier lookTowards, -// BooleanSupplier lookLeft, -// BooleanSupplier lookRight) { -// this.swerve = swerve; -// this.vX = vX; -// this.vY = vY; -// this.headingAdjust = headingAdjust; -// this.lookAway = lookAway; -// this.lookTowards = lookTowards; -// this.lookLeft = lookLeft; -// this.lookRight = lookRight; - -// addRequirements(swerve); -// } - -// @Override -// public void initialize() { -// resetHeading = true; -// } - -// // Called every time the scheduler runs while the command is scheduled. -// @Override -// public void execute() { -// double headingX = 0; -// double headingY = 0; - -// // These are written to allow combinations for 45 angles -// // Face Away from Drivers -// if (lookAway.getAsBoolean()) { -// headingY = -1; -// } -// // Face Right -// if (lookRight.getAsBoolean()) { -// headingX = 1; -// } -// // Face Left -// if (lookLeft.getAsBoolean()) { -// headingX = -1; -// } -// // Face Towards the Drivers -// if (lookTowards.getAsBoolean()) { -// headingY = 1; -// } - -// // Prevent Movement After Auto -// if (resetHeading) { -// if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) == 0) { -// // Get the curret Heading -// Rotation2d currentHeading = swerve.getHeading(); - -// // Set the Current Heading to the desired Heading -// headingX = currentHeading.getSin(); -// headingY = currentHeading.getCos(); -// } -// // Dont reset Heading Again -// resetHeading = false; -// } - -// ChassisSpeeds desiredSpeeds = -// swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); - -// // Limit velocity to prevent tippy -// Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); -// translation = -// SwerveMath.limitVelocity( -// translation, -// swerve.getFieldVelocity(), -// swerve.getPose(), -// PhysicalConstants.kLoopTime, -// PhysicalConstants.kRobotMass, -// List.of(PhysicalConstants.kChassis), -// swerve.getSwerveDriveConfiguration()); -// SmartDashboard.putNumber("LimitedTranslation", translation.getX()); -// SmartDashboard.putString("Translation", translation.toString()); - -// // Make the robot move -// if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { -// resetHeading = true; -// swerve.drive( -// translation, (OperatorConstants.kTurnConstant * -headingAdjust.getAsDouble()), true); -// } else { -// swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); -// } -// } - -// // Called once the command ends or is interrupted. -// @Override -// public void end(boolean interrupted) {} - -// // Returns true when the command should end. -// @Override -// public boolean isFinished() { -// return false; -// } -// } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java deleted file mode 100644 index 2fff696..0000000 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java +++ /dev/null @@ -1,107 +0,0 @@ -// // Copyright (c) 2024 Az-FIRST -// // http://github.com/AZ-First -// // -// // This program is free software; you can redistribute it and/or -// // modify it under the terms of the GNU General Public License -// // version 3 as published by the Free Software Foundation or -// // available in the root directory of this project. -// // -// // This program is distributed in the hope that it will be useful, -// // but WITHOUT ANY WARRANTY; without even the implied warranty of -// // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// // GNU General Public License for more details. -// // -// // Copyright (c) FIRST and other WPILib contributors. -// // Open Source Software; you can modify and/or share it under the terms of -// // the WPILib BSD license file in the root directory of this project. -// // -// // NOTE: This module from the YAGSL Example Project - -// package frc.robot.commands.swervedrive.drivebase; - -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.Constants.PhysicalConstants; -// import frc.robot.subsystems.swervedrive.yagsl_old.YAGSLSwerve; -// import java.util.List; -// import java.util.function.DoubleSupplier; -// import swervelib.SwerveController; -// import swervelib.math.SwerveMath; - -// /** An example command that uses an example subsystem. */ -// public class YAGSL_AbsoluteFieldDrive extends Command { - -// private final YAGSLSwerve swerve; -// private final DoubleSupplier vX, vY, heading; - -// /** -// * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation -// inputs, -// * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and -// * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— -// * they will be converted to a polar angle, which the robot will rotate to. -// * -// * @param swerve The swerve drivebase subsystem. -// * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the -// range -// * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. -// * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the -// range -// * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when -// * looking through the driver station glass. -// * @param heading DoubleSupplier that supplies the robot's heading angle. -// */ -// public YAGSL_AbsoluteFieldDrive( -// YAGSLSwerve swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier heading) { -// this.swerve = swerve; -// this.vX = vX; -// this.vY = vY; -// this.heading = heading; - -// addRequirements(swerve); -// } - -// @Override -// public void initialize() {} - -// // Called every time the scheduler runs while the command is scheduled. -// @Override -// public void execute() { - -// // Get the desired chassis speeds based on a 2 joystick module. - -// ChassisSpeeds desiredSpeeds = -// swerve.getTargetSpeeds( -// vX.getAsDouble(), vY.getAsDouble(), new Rotation2d(heading.getAsDouble() * Math.PI)); - -// // Limit velocity to prevent tippy -// Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); -// translation = -// SwerveMath.limitVelocity( -// translation, -// swerve.getFieldVelocity(), -// swerve.getPose(), -// PhysicalConstants.kLoopTime, -// PhysicalConstants.kRobotMass, -// List.of(PhysicalConstants.kChassis), -// swerve.getSwerveDriveConfiguration()); -// SmartDashboard.putNumber("LimitedTranslation", translation.getX()); -// SmartDashboard.putString("Translation", translation.toString()); - -// // Make the robot move -// swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); -// } - -// // Called once the command ends or is interrupted. -// @Override -// public void end(boolean interrupted) {} - -// // Returns true when the command should end. -// @Override -// public boolean isFinished() { -// return false; -// } -// } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index bb70d42..69d988c 100755 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -11,7 +11,8 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.math.util.Units; -import frc.robot.subsystems.swervedrive_ignore.SwerveSubsystem; + +// import frc.robot.subsystems.swervedrive_ignore.SwerveSubsystem; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html @@ -189,6 +190,6 @@ public class TunerConstants { kInvertRightSide) .withSteerMotorInverted(kBackRightSteerInvert); - public static final SwerveSubsystem DriveTrain = - new SwerveSubsystem(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + // public static final SwerveSubsystem DriveTrain = + // new SwerveSubsystem(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index bdce538..99ce6ab 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -113,31 +113,38 @@ public Drive() { throw new RuntimeException("Invalid Swerve Drive Type"); } - // Configure AutoBuilder for PathPlanner - AutoBuilder.configureHolonomic( - this::getPose, - this::setPose, - () -> kinematics.toChassisSpeeds(getModuleStates()), - this::runVelocity, - new HolonomicPathFollowerConfig( - DrivebaseConstants.kMaxLinearSpeed, - Units.inchesToMeters(kDriveBaseRadius), - new ReplanningConfig()), - () -> - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red, - this); - Pathfinding.setPathfinder(new LocalADStarAK()); - PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); - PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - }); - + // Configure Autonomous Path Building based on `AutoType` + switch (Constants.getAutoType()) { + case PATHPLANNER: + // Configure AutoBuilder for PathPlanner + AutoBuilder.configureHolonomic( + this::getPose, + this::setPose, + () -> kinematics.toChassisSpeeds(getModuleStates()), + this::runVelocity, + new HolonomicPathFollowerConfig( + DrivebaseConstants.kMaxLinearSpeed, + Units.inchesToMeters(kDriveBaseRadius), + new ReplanningConfig()), + () -> + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red, + this); + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + break; + case CHOREO: + break; + default: + } // Configure SysId sysId = new SysIdRoutine( @@ -335,7 +342,7 @@ public static Translation2d[] getModuleTranslations() { }; } - public T getGyro() { + public Object getGyro() { return gyroIO.getGyro(); } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 6a4f286..8b1b9f0 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -18,7 +18,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; -public interface GyroIO { +public interface GyroIO { @AutoLog public static class GyroIOInputs { public boolean connected = false; @@ -30,5 +30,5 @@ public default void updateInputs(GyroIOInputs inputs) {} public default void zero() {} - public abstract T getGyro(); + public abstract T getGyro(); } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 40b4709..009846a 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -23,7 +23,7 @@ import edu.wpi.first.wpilibj.SPI; /** IO implementation for Pigeon2 */ -public class GyroIONavX implements GyroIO { +public class GyroIONavX implements GyroIO { private final AHRS navx; // Constructor, taking default values @@ -39,7 +39,7 @@ public GyroIONavX() { } // Return the Pigeon2 instance - @SuppressWarnings("unchecked") + @Override public AHRS getGyro() { return navx; } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 28a27a4..7ce51c1 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -28,7 +28,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; /** IO implementation for Pigeon2 */ -public class GyroIOPigeon2 implements GyroIO { +public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon; private final StatusSignal yaw; private final StatusSignal yawVelocity; @@ -47,7 +47,7 @@ public GyroIOPigeon2() { } // Return the Pigeon2 instance - @SuppressWarnings("unchecked") + @Override public Pigeon2 getGyro() { return pigeon; } diff --git a/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java index bcb589d..d2eb761 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java @@ -1,374 +1,382 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -// -// NOTE: This module based on the CTRE Phoenix6 examples -// https://github.com/CrossTheRoadElec/Phoenix6-Examples - -package frc.robot.subsystems.swervedrive_ignore; - -import static edu.wpi.first.units.Units.Volts; -import static frc.robot.Constants.DrivebaseConstants.*; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants.DrivebaseConstants; -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.vision.Vision; -import java.util.function.Supplier; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -/** - * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used - * in command-based projects easily. - */ -public class SwerveSubsystem extends SwerveDrivetrain implements Subsystem { - /** CTRE constants */ - private static final double kSimLoopPeriod = 0.005; // 5 ms - - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - /** PhotonVision class to keep an accurate odometry */ - private Vision vision; - - /** Enable vision odometry updates while driving */ - private final boolean visionDriveTest = false; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean hasAppliedOperatorPerspective = false; - - private final SwerveRequest.ApplyChassisSpeeds AutoRequest = - new SwerveRequest.ApplyChassisSpeeds(); - - private final SwerveRequest.SysIdSwerveTranslation TranslationCharacterization = - new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveRotation RotationCharacterization = - new SwerveRequest.SysIdSwerveRotation(); - private final SwerveRequest.SysIdSwerveSteerGains SteerCharacterization = - new SwerveRequest.SysIdSwerveSteerGains(); - - /* Use one of these sysidroutines for your particular test */ - private SysIdRoutine SysIdRoutineTranslation = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - Volts.of(4), - null, - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> setControl(TranslationCharacterization.withVolts(volts)), null, this)); - - private final SysIdRoutine SysIdRoutineRotation = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - Volts.of(4), - null, - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> setControl(RotationCharacterization.withVolts(volts)), null, this)); - private final SysIdRoutine SysIdRoutineSteer = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - Volts.of(7), - null, - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> setControl(SteerCharacterization.withVolts(volts)), null, this)); - - /* Change this to the sysid routine you want to test */ - private final SysIdRoutine RoutineToApply = SysIdRoutineTranslation; - - public SwerveSubsystem( - SwerveDrivetrainConstants driveTrainConstants, - double OdometryUpdateFrequency, - SwerveModuleConstants... modules) { - super(driveTrainConstants, OdometryUpdateFrequency, modules); - configurePathPlanner(); - if (Utils.isSimulation()) { - startSimThread(); - } - Logger.recordOutput("SwerveDrive/Comment1", "This is a comment!"); - } - - public SwerveSubsystem( - SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { - super(driveTrainConstants, modules); - configurePathPlanner(); - if (Utils.isSimulation()) { - startSimThread(); - } - Logger.recordOutput("SwerveDrive/Comment2", "This is a comment!"); - } - - private void configurePathPlanner() { - double driveBaseRadius = 0; - for (var moduleLocation : m_moduleLocations) { - driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); - } - - AutoBuilder.configureHolonomic( - () -> this.getState().Pose, // Supplier of current robot pose - this::seedFieldRelative, // Consumer for seeding pose against auto - this::getCurrentRobotChassisSpeeds, - (speeds) -> - this.setControl( - AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot - new HolonomicPathFollowerConfig( - new PIDConstants(10, 0, 0), - new PIDConstants(10, 0, 0), - TunerConstants.kSpeedAt12VoltsMps, - driveBaseRadius, - new ReplanningConfig()), - () -> - DriverStation.getAlliance().orElse(Alliance.Blue) - == Alliance - .Red, // Assume the path needs to be flipped for Red vs Blue, this is normally - // the case - this); // Subsystem for requirements - } - - /* Section for defining TeleOp Commands */ - // Field-centric driving in open loop mode - public final SwerveRequest.FieldCentric drive = - new SwerveRequest.FieldCentric() - .withDeadband(DrivebaseConstants.kMaxLinearSpeed * 0.1) - .withRotationalDeadband(DrivebaseConstants.kMaxAngularSpeed * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - public final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - public final SwerveRequest.RobotCentric forwardStraight = - new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - public final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - public Command getAutoPath(String pathName) { - return new PathPlannerAuto(pathName); - } - - /* - * Both the sysid commands are specific to one particular sysid routine, change - * which one you're trying to characterize - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return RoutineToApply.quasistatic(direction); - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return RoutineToApply.dynamic(direction); - } - - public ChassisSpeeds getCurrentRobotChassisSpeeds() { - return m_kinematics.toChassisSpeeds(getState().ModuleStates); - } - - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = - new Notifier( - () -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } - - /** Periodic function -- update odometry and log everything */ - @Override - public void periodic() { - /* Periodically try to apply the operator perspective */ - /* If we haven't applied the operator perspective before, then we should apply it regardless of DS state */ - /* This allows us to correct the perspective in case the robot code restarts mid-match */ - /* Otherwise, only check and apply the operator perspective if the DS is disabled */ - /* This ensures driving behavior doesn't change until an explicit disable event occurs during testing*/ - if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance() - .ifPresent( - (allianceColor) -> { - this.setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? RedAlliancePerspectiveRotation - : BlueAlliancePerspectiveRotation); - hasAppliedOperatorPerspective = true; - }); - } - - // // When vision is enabled we must manually update odometry in SwerveDrive - // if (visionDriveTest) { - // vision.updatePoseEstimation(this); - // } - - /** Log Telemetry Data to AdvantageKit */ - Logger.recordOutput("SwerveDrive/Comment", "This is a comment!"); - // Logger.recordOutput("SwerveDrive/Telemetry/moduleCount", this.ModuleCount); - // Logger.recordOutput("SwerveDrive/Telemetry/wheelLocations", this.m_moduleLocations); - Logger.recordOutput("SwerveDrive/Telemetry/measuredStates", getState().ModuleStates); - // 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)); - } - - /************************************************************************* */ - /* COMMAND SECTION -- Drivebase-only Commands */ - - /** - * Get the path follower with events. - * - * @param pathName PathPlanner path name. - * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. - */ - public Command getAutonomousCommand(String pathName) { - // Create a path following command using AutoBuilder. This will also trigger event markers. - return new PathPlannerAuto(pathName); - } - - /** - * Use PathPlanner Path finding to go to a point on the field. - * - * @param pose Target {@link Pose2d} to go to. - * @return PathFinding command - */ - public Command driveToPose(Pose2d pose) { - // Create the constraints to use while pathfinding - PathConstraints constraints = - new PathConstraints(kMaxLinearSpeed, kMaxLinearAccel, kMaxAngularSpeed, kMaxAngularAccel); - - // 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. - ); - } - - /************************************************************************* */ - /* UTILITY SECTION -- Utility methods */ - - /** - * Gets the current pose (position and rotation) of the robot, as reported by odometry. - * - * @return The robot's pose - */ - @AutoLogOutput(key = "Odometry/RobotPose") - public Pose2d getPose() { - return getState().Pose; - } - - /** - * Sets the drive motors to brake/coast mode. - * - * @param brake True to set motors to brake mode, false for coast. - */ - public void setMotorBrake(boolean brake) { - if (brake) { - this.configNeutralMode(NeutralModeValue.Brake); - } else { - this.configNeutralMode(NeutralModeValue.Coast); - } - } - - // /** 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; - // } -} +// // Copyright (c) 2024 Az-FIRST +// // http://github.com/AZ-First +// // +// // This program is free software; you can redistribute it and/or +// // modify it under the terms of the GNU General Public License +// // version 3 as published by the Free Software Foundation or +// // available in the root directory of this project. +// // +// // This program is distributed in the hope that it will be useful, +// // but WITHOUT ANY WARRANTY; without even the implied warranty of +// // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// // GNU General Public License for more details. +// // +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. +// // +// // NOTE: This module based on the CTRE Phoenix6 examples +// // https://github.com/CrossTheRoadElec/Phoenix6-Examples + +// package frc.robot.subsystems.swervedrive_ignore; + +// import static edu.wpi.first.units.Units.Volts; +// import static frc.robot.Constants.DrivebaseConstants.*; + +// import com.ctre.phoenix6.SignalLogger; +// import com.ctre.phoenix6.Utils; +// import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; +// import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +// import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +// import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +// import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +// import com.ctre.phoenix6.signals.NeutralModeValue; +// import com.pathplanner.lib.auto.AutoBuilder; +// import com.pathplanner.lib.commands.PathPlannerAuto; +// import com.pathplanner.lib.path.PathConstraints; +// import com.pathplanner.lib.path.PathPlannerPath; +// import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +// import com.pathplanner.lib.util.PIDConstants; +// import com.pathplanner.lib.util.ReplanningConfig; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; +// import edu.wpi.first.wpilibj.Notifier; +// import edu.wpi.first.wpilibj.RobotController; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.Subsystem; +// import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +// import frc.robot.Constants.DrivebaseConstants; +// import frc.robot.generated.TunerConstants; +// import frc.robot.subsystems.vision.Vision; +// import java.util.function.Supplier; +// import org.littletonrobotics.junction.AutoLogOutput; +// import org.littletonrobotics.junction.Logger; + +// /** +// * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be +// used +// * in command-based projects easily. +// */ +// public class SwerveSubsystem extends SwerveDrivetrain implements Subsystem { +// /** CTRE constants */ +// private static final double kSimLoopPeriod = 0.005; // 5 ms + +// private Notifier m_simNotifier = null; +// private double m_lastSimTime; + +// /** PhotonVision class to keep an accurate odometry */ +// private Vision vision; + +// /** Enable vision odometry updates while driving */ +// private final boolean visionDriveTest = false; + +// /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ +// private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); +// /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ +// private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); +// /* Keep track if we've ever applied the operator perspective before or not */ +// private boolean hasAppliedOperatorPerspective = false; + +// private final SwerveRequest.ApplyChassisSpeeds AutoRequest = +// new SwerveRequest.ApplyChassisSpeeds(); + +// private final SwerveRequest.SysIdSwerveTranslation TranslationCharacterization = +// new SwerveRequest.SysIdSwerveTranslation(); +// private final SwerveRequest.SysIdSwerveRotation RotationCharacterization = +// new SwerveRequest.SysIdSwerveRotation(); +// private final SwerveRequest.SysIdSwerveSteerGains SteerCharacterization = +// new SwerveRequest.SysIdSwerveSteerGains(); + +// /* Use one of these sysidroutines for your particular test */ +// private SysIdRoutine SysIdRoutineTranslation = +// new SysIdRoutine( +// new SysIdRoutine.Config( +// null, +// Volts.of(4), +// null, +// (state) -> SignalLogger.writeString("state", state.toString())), +// new SysIdRoutine.Mechanism( +// (volts) -> setControl(TranslationCharacterization.withVolts(volts)), null, this)); + +// private final SysIdRoutine SysIdRoutineRotation = +// new SysIdRoutine( +// new SysIdRoutine.Config( +// null, +// Volts.of(4), +// null, +// (state) -> SignalLogger.writeString("state", state.toString())), +// new SysIdRoutine.Mechanism( +// (volts) -> setControl(RotationCharacterization.withVolts(volts)), null, this)); +// private final SysIdRoutine SysIdRoutineSteer = +// new SysIdRoutine( +// new SysIdRoutine.Config( +// null, +// Volts.of(7), +// null, +// (state) -> SignalLogger.writeString("state", state.toString())), +// new SysIdRoutine.Mechanism( +// (volts) -> setControl(SteerCharacterization.withVolts(volts)), null, this)); + +// /* Change this to the sysid routine you want to test */ +// private final SysIdRoutine RoutineToApply = SysIdRoutineTranslation; + +// public SwerveSubsystem( +// SwerveDrivetrainConstants driveTrainConstants, +// double OdometryUpdateFrequency, +// SwerveModuleConstants... modules) { +// super(driveTrainConstants, OdometryUpdateFrequency, modules); +// configurePathPlanner(); +// if (Utils.isSimulation()) { +// startSimThread(); +// } +// Logger.recordOutput("SwerveDrive/Comment1", "This is a comment!"); +// } + +// public SwerveSubsystem( +// SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { +// super(driveTrainConstants, modules); +// configurePathPlanner(); +// if (Utils.isSimulation()) { +// startSimThread(); +// } +// Logger.recordOutput("SwerveDrive/Comment2", "This is a comment!"); +// } + +// private void configurePathPlanner() { +// double driveBaseRadius = 0; +// for (var moduleLocation : m_moduleLocations) { +// driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); +// } + +// AutoBuilder.configureHolonomic( +// () -> this.getState().Pose, // Supplier of current robot pose +// this::seedFieldRelative, // Consumer for seeding pose against auto +// this::getCurrentRobotChassisSpeeds, +// (speeds) -> +// this.setControl( +// AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot +// new HolonomicPathFollowerConfig( +// new PIDConstants(10, 0, 0), +// new PIDConstants(10, 0, 0), +// TunerConstants.kSpeedAt12VoltsMps, +// driveBaseRadius, +// new ReplanningConfig()), +// () -> +// DriverStation.getAlliance().orElse(Alliance.Blue) +// == Alliance +// .Red, // Assume the path needs to be flipped for Red vs Blue, this is +// normally +// // the case +// this); // Subsystem for requirements +// } + +// /* Section for defining TeleOp Commands */ +// // Field-centric driving in open loop mode +// public final SwerveRequest.FieldCentric drive = +// new SwerveRequest.FieldCentric() +// .withDeadband(DrivebaseConstants.kMaxLinearSpeed * 0.1) +// .withRotationalDeadband(DrivebaseConstants.kMaxAngularSpeed * 0.1) // Add a 10% +// deadband +// .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + +// public final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); +// public final SwerveRequest.RobotCentric forwardStraight = +// new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); +// public final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + +// public Command applyRequest(Supplier requestSupplier) { +// return run(() -> this.setControl(requestSupplier.get())); +// } + +// public Command getAutoPath(String pathName) { +// return new PathPlannerAuto(pathName); +// } + +// /* +// * Both the sysid commands are specific to one particular sysid routine, change +// * which one you're trying to characterize +// */ +// public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { +// return RoutineToApply.quasistatic(direction); +// } + +// public Command sysIdDynamic(SysIdRoutine.Direction direction) { +// return RoutineToApply.dynamic(direction); +// } + +// public ChassisSpeeds getCurrentRobotChassisSpeeds() { +// return m_kinematics.toChassisSpeeds(getState().ModuleStates); +// } + +// private void startSimThread() { +// m_lastSimTime = Utils.getCurrentTimeSeconds(); + +// /* Run simulation at a faster rate so PID gains behave more reasonably */ +// m_simNotifier = +// new Notifier( +// () -> { +// final double currentTime = Utils.getCurrentTimeSeconds(); +// double deltaTime = currentTime - m_lastSimTime; +// m_lastSimTime = currentTime; + +// /* use the measured time delta, get battery voltage from WPILib */ +// updateSimState(deltaTime, RobotController.getBatteryVoltage()); +// }); +// m_simNotifier.startPeriodic(kSimLoopPeriod); +// } + +// /** Periodic function -- update odometry and log everything */ +// @Override +// public void periodic() { +// /* Periodically try to apply the operator perspective */ +// /* If we haven't applied the operator perspective before, then we should apply it regardless +// of DS state */ +// /* This allows us to correct the perspective in case the robot code restarts mid-match */ +// /* Otherwise, only check and apply the operator perspective if the DS is disabled */ +// /* This ensures driving behavior doesn't change until an explicit disable event occurs during +// testing*/ +// if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { +// DriverStation.getAlliance() +// .ifPresent( +// (allianceColor) -> { +// this.setOperatorPerspectiveForward( +// allianceColor == Alliance.Red +// ? RedAlliancePerspectiveRotation +// : BlueAlliancePerspectiveRotation); +// hasAppliedOperatorPerspective = true; +// }); +// } + +// // // When vision is enabled we must manually update odometry in SwerveDrive +// // if (visionDriveTest) { +// // vision.updatePoseEstimation(this); +// // } + +// /** Log Telemetry Data to AdvantageKit */ +// Logger.recordOutput("SwerveDrive/Comment", "This is a comment!"); +// // Logger.recordOutput("SwerveDrive/Telemetry/moduleCount", this.ModuleCount); +// // Logger.recordOutput("SwerveDrive/Telemetry/wheelLocations", this.m_moduleLocations); +// Logger.recordOutput("SwerveDrive/Telemetry/measuredStates", getState().ModuleStates); +// // 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)); +// } + +// /************************************************************************* */ +// /* COMMAND SECTION -- Drivebase-only Commands */ + +// /** +// * Get the path follower with events. +// * +// * @param pathName PathPlanner path name. +// * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. +// */ +// public Command getAutonomousCommand(String pathName) { +// // Create a path following command using AutoBuilder. This will also trigger event markers. +// return new PathPlannerAuto(pathName); +// } + +// /** +// * Use PathPlanner Path finding to go to a point on the field. +// * +// * @param pose Target {@link Pose2d} to go to. +// * @return PathFinding command +// */ +// public Command driveToPose(Pose2d pose) { +// // Create the constraints to use while pathfinding +// PathConstraints constraints = +// new PathConstraints(kMaxLinearSpeed, kMaxLinearAccel, kMaxAngularSpeed, +// kMaxAngularAccel); + +// // 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. +// ); +// } + +// /************************************************************************* */ +// /* UTILITY SECTION -- Utility methods */ + +// /** +// * Gets the current pose (position and rotation) of the robot, as reported by odometry. +// * +// * @return The robot's pose +// */ +// @AutoLogOutput(key = "Odometry/RobotPose") +// public Pose2d getPose() { +// return getState().Pose; +// } + +// /** +// * Sets the drive motors to brake/coast mode. +// * +// * @param brake True to set motors to brake mode, false for coast. +// */ +// public void setMotorBrake(boolean brake) { +// if (brake) { +// this.configNeutralMode(NeutralModeValue.Brake); +// } else { +// this.configNeutralMode(NeutralModeValue.Coast); +// } +// } + +// // /** 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; +// // } +// } diff --git a/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java index 817a3f0..e697634 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java +++ b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java @@ -1,144 +1,149 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -// -// NOTE: This module based on the CTRE Phoenix6 examples -// https://github.com/CrossTheRoadElec/Phoenix6-Examples - -package frc.robot.subsystems.swervedrive_ignore; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.networktables.DoubleArrayPublisher; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; - -public class SwerveTelemetry { - private final double MaxSpeed; - - /** - * Construct a telemetry object, with the specified max speed of the robot - * - * @param maxSpeed Maximum speed in meters per second - */ - public SwerveTelemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - SignalLogger.start(); - } - - /* What to publish over networktables for telemetry */ - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - - /* Robot pose for field positioning */ - private final NetworkTable table = inst.getTable("Pose"); - private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); - private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); - - /* Robot speeds for general checking */ - private final NetworkTable driveStats = inst.getTable("Drive"); - private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); - private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); - private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); - private final DoublePublisher odomFreq = - driveStats.getDoubleTopic("Odometry Frequency").publish(); - - /* Keep a reference of the last pose to calculate the speeds */ - private Pose2d m_lastPose = new Pose2d(); - private double lastTime = Utils.getCurrentTimeSeconds(); - - /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = - new Mechanism2d[] { - new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), - }; - /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = - new MechanismLigament2d[] { - m_moduleMechanisms[0] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - }; - /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = - new MechanismLigament2d[] { - m_moduleMechanisms[0] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; - - /* Accept the swerve drive state and telemeterize it to smartdashboard */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the pose */ - Pose2d pose = state.Pose; - fieldTypePub.set("Field2d"); - fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); - - /* Telemeterize the robot's general speeds */ - double currentTime = Utils.getCurrentTimeSeconds(); - double diffTime = currentTime - lastTime; - lastTime = currentTime; - Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation(); - m_lastPose = pose; - - Translation2d velocities = distanceDiff.div(diffTime); - - speed.set(velocities.getNorm()); - velocityX.set(velocities.getX()); - velocityY.set(velocities.getY()); - odomFreq.set(1.0 / state.OdometryPeriod); - - /* Telemeterize the module's states */ - for (int i = 0; i < 4; ++i) { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - - SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - } - - SignalLogger.writeDoubleArray( - "odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); - SignalLogger.writeDouble("odom period", state.OdometryPeriod, "seconds"); - } -} +// // Copyright (c) 2024 Az-FIRST +// // http://github.com/AZ-First +// // +// // This program is free software; you can redistribute it and/or +// // modify it under the terms of the GNU General Public License +// // version 3 as published by the Free Software Foundation or +// // available in the root directory of this project. +// // +// // This program is distributed in the hope that it will be useful, +// // but WITHOUT ANY WARRANTY; without even the implied warranty of +// // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// // GNU General Public License for more details. +// // +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. +// // +// // NOTE: This module based on the CTRE Phoenix6 examples +// // https://github.com/CrossTheRoadElec/Phoenix6-Examples + +// package frc.robot.subsystems.swervedrive_ignore; + +// import com.ctre.phoenix6.SignalLogger; +// import com.ctre.phoenix6.Utils; +// import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.networktables.DoubleArrayPublisher; +// import edu.wpi.first.networktables.DoublePublisher; +// import edu.wpi.first.networktables.NetworkTable; +// import edu.wpi.first.networktables.NetworkTableInstance; +// import edu.wpi.first.networktables.StringPublisher; +// import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +// import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj.util.Color; +// import edu.wpi.first.wpilibj.util.Color8Bit; + +// public class SwerveTelemetry { +// private final double MaxSpeed; + +// /** +// * Construct a telemetry object, with the specified max speed of the robot +// * +// * @param maxSpeed Maximum speed in meters per second +// */ +// public SwerveTelemetry(double maxSpeed) { +// MaxSpeed = maxSpeed; +// SignalLogger.start(); +// } + +// /* What to publish over networktables for telemetry */ +// private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + +// /* Robot pose for field positioning */ +// private final NetworkTable table = inst.getTable("Pose"); +// private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); +// private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + +// /* Robot speeds for general checking */ +// private final NetworkTable driveStats = inst.getTable("Drive"); +// private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); +// private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); +// private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); +// private final DoublePublisher odomFreq = +// driveStats.getDoubleTopic("Odometry Frequency").publish(); + +// /* Keep a reference of the last pose to calculate the speeds */ +// private Pose2d m_lastPose = new Pose2d(); +// private double lastTime = Utils.getCurrentTimeSeconds(); + +// /* Mechanisms to represent the swerve module states */ +// private final Mechanism2d[] m_moduleMechanisms = +// new Mechanism2d[] { +// new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, +// 1), +// }; +// /* A direction and length changing ligament for speed representation */ +// private final MechanismLigament2d[] m_moduleSpeeds = +// new MechanismLigament2d[] { +// m_moduleMechanisms[0] +// .getRoot("RootSpeed", 0.5, 0.5) +// .append(new MechanismLigament2d("Speed", 0.5, 0)), +// m_moduleMechanisms[1] +// .getRoot("RootSpeed", 0.5, 0.5) +// .append(new MechanismLigament2d("Speed", 0.5, 0)), +// m_moduleMechanisms[2] +// .getRoot("RootSpeed", 0.5, 0.5) +// .append(new MechanismLigament2d("Speed", 0.5, 0)), +// m_moduleMechanisms[3] +// .getRoot("RootSpeed", 0.5, 0.5) +// .append(new MechanismLigament2d("Speed", 0.5, 0)), +// }; +// /* A direction changing and length constant ligament for module direction */ +// private final MechanismLigament2d[] m_moduleDirections = +// new MechanismLigament2d[] { +// m_moduleMechanisms[0] +// .getRoot("RootDirection", 0.5, 0.5) +// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new +// Color8Bit(Color.kWhite))), +// m_moduleMechanisms[1] +// .getRoot("RootDirection", 0.5, 0.5) +// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new +// Color8Bit(Color.kWhite))), +// m_moduleMechanisms[2] +// .getRoot("RootDirection", 0.5, 0.5) +// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new +// Color8Bit(Color.kWhite))), +// m_moduleMechanisms[3] +// .getRoot("RootDirection", 0.5, 0.5) +// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new +// Color8Bit(Color.kWhite))), +// }; + +// /* Accept the swerve drive state and telemeterize it to smartdashboard */ +// public void telemeterize(SwerveDriveState state) { +// /* Telemeterize the pose */ +// Pose2d pose = state.Pose; +// fieldTypePub.set("Field2d"); +// fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); + +// /* Telemeterize the robot's general speeds */ +// double currentTime = Utils.getCurrentTimeSeconds(); +// double diffTime = currentTime - lastTime; +// lastTime = currentTime; +// Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation(); +// m_lastPose = pose; + +// Translation2d velocities = distanceDiff.div(diffTime); + +// speed.set(velocities.getNorm()); +// velocityX.set(velocities.getX()); +// velocityY.set(velocities.getY()); +// odomFreq.set(1.0 / state.OdometryPeriod); + +// /* Telemeterize the module's states */ +// for (int i = 0; i < 4; ++i) { +// m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); +// m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); +// m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + +// SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); +// } + +// SignalLogger.writeDoubleArray( +// "odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); +// SignalLogger.writeDouble("odom period", state.OdometryPeriod, "seconds"); +// } +// }