diff --git a/src/main/java/frc/lib/AllianceSelector.java b/src/main/java/frc/lib/AllianceSelector.java new file mode 100644 index 0000000..59c2607 --- /dev/null +++ b/src/main/java/frc/lib/AllianceSelector.java @@ -0,0 +1,30 @@ +package frc.lib; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.AutoConstants.AllianceColor; + +import java.util.function.BooleanSupplier; + +public class AllianceSelector extends SubsystemBase{ + + private DigitalInput m_allianceSelectionSwitch; + + public AllianceSelector(int port) { + this.m_allianceSelectionSwitch = new DigitalInput(port); + } + + public BooleanSupplier fieldRotatedSupplier() { + return () -> m_allianceSelectionSwitch.get(); + } + + public AllianceColor getAllianceColor() { + return m_allianceSelectionSwitch.get() ? AllianceColor.Red : AllianceColor.Blue; + } + + @Override + public void periodic() { + SmartDashboard.putString("Alliance Color", getAllianceColor().name()); + } +} diff --git a/src/main/java/frc/lib/AutoSelector.java b/src/main/java/frc/lib/AutoSelector.java new file mode 100644 index 0000000..28cd0de --- /dev/null +++ b/src/main/java/frc/lib/AutoSelector.java @@ -0,0 +1,73 @@ +package frc.lib; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.AutoConstants.AllianceColor; +import frc.robot.Robot.AutoOption; +import frc.robot.autos.ChoreoAuto; + +import java.util.List; + +public class AutoSelector { + + private DigitalInput[] m_switchPositions; + private AllianceSelector m_allianceSelector; + private List m_autoOptions; + + private ChoreoAuto m_autonomous; + + public AutoSelector(int[] ports, AllianceSelector allianceSelector, List autoOptions) { + + m_switchPositions = new DigitalInput[ports.length]; + for (int i = 0; i < ports.length; i++) { + m_switchPositions[i] = new DigitalInput(ports[i]); + } + + this.m_allianceSelector = allianceSelector; + this.m_autoOptions = autoOptions; + } + + /** + * @return Index in array of Digital Inputs corresponding to selected auto mode + */ + private int getSwitchPosition() { + for (int i = 0; i < m_switchPositions.length; i++) { + if (!m_switchPositions[i].get()) { + return i + 1; + } + } + return 0; // failure of the physical switch + } + + public ChoreoAuto getSelectedAutonomous() { + int switchPosition = getSwitchPosition(); + AllianceColor color = m_allianceSelector.getAllianceColor(); + + for (int i = 0; i < m_autoOptions.size(); i++) { + if (m_autoOptions.get(i).getColor() == color) + if (m_autoOptions.get(i).getOption() == switchPosition) { + return m_autoOptions.get(i).getChoreoAuto(); + } + } + return null; + } + + /** + * @return The Command that runs the selected autonomous mode + */ + public Command getAutonomousCommand() { + getSelectedAutonomous(); + if (m_autonomous != null) return m_autonomous; + else return null; + } + + public void periodic() { + + if (m_autonomous != null) { + SmartDashboard.putString("Auto", m_autonomous.getName()); + } else { + SmartDashboard.putString("Auto", "Null"); + } + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 12071c6..cdc682e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.*; -import com.pathplanner.lib.util.PIDConstants; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.units.measure.AngularVelocity; @@ -74,14 +73,18 @@ public static final class ModuleConstants { public static final int kDriveMotorCurrentLimit = 80; public static final int kTurningMotorCurrentLimit = 80; - public static final double kDriveP = 0.1; // 2023 Competition Robot - public static final double kDriveI = 0.0; // 2023 Competition Robot - public static final double kDriveD = 0.0; // 2023 Competition Robot - public static final double kDriveFF = 0.255; // 2023 Competition Robot + public static final class DriveControllerGains { + public static final double kP = 0.1; // 2023 Competition Robot + public static final double kI = 0.0; // 2023 Competition Robot + public static final double kD = 0.0; // 2023 Competition Robot + public static final double kFF = 0.255; // 2023 Competition Robot + } - public static final double kTurningP = 10.0; // 1.5; - public static final double kTurningI = 0.0; // 2023 Competition Robot - public static final double kTurningD = 0.0; // 2023 Competition Robot + public static final class TurningControllerGains { + public static final double kP = 10.0; // 1.5; + public static final double kI = 0.0; // 2023 Competition Robot + public static final double kD = 0.0; // 2023 Competition Robot + } // Not adjusted // public static final double kMaxModuleAngularSpeedRadiansPerSecond = 0.05 * Math.PI; @@ -160,9 +163,15 @@ public static final class XBox { } public static final class AutoConstants { + + public static enum AllianceColor { + Red, + Blue + } + public static final int kAllianceColorSelectorPort = 10; - // length is 8 + // max length is 8 public static final int[] kAutonomousModeSelectorPorts = {11, 12, 13, 18, 19}; // public static final double kMaxSpeedMetersPerSecond = 3.0; @@ -170,8 +179,17 @@ public static final class AutoConstants { // public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; // public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - public static final PIDConstants kTranslationControllerGains = new PIDConstants(1.0, 0.0, 0.0); - public static final PIDConstants kRotationControllerGains = new PIDConstants(7.0, 0.0, 0.0); + public static final class TranslationControllerGains { + public static final double kP = 1.0; + public static final double kI = 0.0; + public static final double kD = 0.0; + } + + public static final class RotationControllerGains { + public static final double kP = 7.0; + public static final double kI = 0.0; + public static final double kD = 0.0; + } // Constraint for the motion profilied robot angle controller // public static final TrapezoidProfile.Constraints kThetaControllerConstraints = diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8935ad0..103a2b0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,28 +2,65 @@ package frc.robot; -import static frc.robot.RobotContainer.getRobotContainer; +import java.util.List; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.lib.AllianceSelector; +import frc.lib.AutoSelector; import frc.lib.ControllerPatroller; +import frc.lib.SendableZorroController; +import frc.robot.Constants.AutoConstants.AllianceColor; +import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.DriveConstants; import frc.robot.Constants.OIConstants; +import frc.robot.Constants.OIConstants.Zorro; +import frc.robot.autos.ChoreoAuto; +import frc.robot.drivetrain.Drivetrain; +import frc.robot.drivetrain.commands.ZorroDriveCommand; public class Robot extends TimedRobot { private Command m_autonomousCommand; + private List m_autoOptions; + + private final PowerDistribution m_powerDistribution = new PowerDistribution(1, ModuleType.kRev); + private final AllianceSelector m_allianceSelector; + private final AutoSelector m_autoSelector; + private final Drivetrain m_swerve; + + private SendableZorroController m_driver; + private XboxController m_operator; private int m_usb_check_delay = OIConstants.kUSBCheckNumLoops; + public Robot() { + configureButtonBindings(); + configureDefaultCommands(); + configureAutoOptions(); + + m_allianceSelector = new AllianceSelector(AutoConstants.kAllianceColorSelectorPort); + m_autoSelector = new AutoSelector(AutoConstants.kAutonomousModeSelectorPorts, m_allianceSelector, m_autoOptions); + + m_swerve = new Drivetrain(m_allianceSelector.fieldRotatedSupplier()); + + // Create a button on Smart Dashboard to reset the encoders. + SmartDashboard.putData( + "Align Encoders", + new InstantCommand(() -> m_swerve.zeroAbsTurningEncoderOffsets()).ignoringDisable(true)); + } + @Override public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. - getRobotContainer(); - // Starts recording to data log // https://docs.wpilib.org/en/stable/docs/software/telemetry/datalog.html#logging-joystick-data DataLogManager.start(); @@ -33,13 +70,13 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - getRobotContainer().periodic(); + SmartDashboard.putData(m_driver); + SmartDashboard.putData(m_operator); + SmartDashboard.putData(m_powerDistribution); } @Override - public void disabledInit() { - getRobotContainer().disabledInit(); - } + public void disabledInit() {} @Override public void disabledPeriodic() { @@ -49,21 +86,19 @@ public void disabledPeriodic() { * Only check if controllers changed every kUSBCheckNumLoops loops of disablePeriodic(). * This prevents us from hammering on some routines that cause the RIO to lock up. */ - RobotContainer rc = getRobotContainer(); m_usb_check_delay--; if (0 >= m_usb_check_delay) { m_usb_check_delay = OIConstants.kUSBCheckNumLoops; if (ControllerPatroller.getInstance().controllersChanged()) { // Reset the joysticks & button mappings. - rc.configureButtonBindings(); + configureButtonBindings(); } } } @Override public void autonomousInit() { - getRobotContainer().autonomousInit(); - m_autonomousCommand = getRobotContainer().getAutonomousCommand(); + m_autonomousCommand = m_autoSelector.getAutonomousCommand(); if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); @@ -78,8 +113,6 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - - getRobotContainer().teleopInit(); } @Override @@ -92,4 +125,84 @@ public void testInit() { @Override public void testPeriodic() {} + + private void configureDefaultCommands() { + m_swerve.setDefaultCommand( + new ZorroDriveCommand(m_swerve, DriveConstants.kDriveKinematics, m_driver)); + } + + public void configureButtonBindings() { + + // Clear any active buttons. + CommandScheduler.getInstance().getActiveButtonLoop().clear(); + + ControllerPatroller cp = ControllerPatroller.getInstance(); + + // We use two different types of controllers - Joystick & XboxController. + // Create objects of the specific types. + m_driver = new SendableZorroController(cp.findDriverPort()); + m_operator = new XboxController(cp.findOperatorPort()); + + configureDriverButtonBindings(); + configureOperatorButtonBindings(); + } + + // spotless:off + private void configureDriverButtonBindings() { + + // Reset heading + new JoystickButton(m_driver, Zorro.kHIn) + .onTrue(new InstantCommand(() -> m_swerve.resetHeading()) + .ignoringDisable(true)); + + } + // spotless:on + + private void configureOperatorButtonBindings() {} + + private void configureAutoOptions() { + m_autoOptions.add(new AutoOption(AllianceColor.Red, 1)); + m_autoOptions.add(new AutoOption(AllianceColor.Blue, 1)); + } + + public class AutoOption { + private AllianceColor m_color; + private int m_option; + private ChoreoAuto m_auto; + + public AutoOption(AllianceColor color, int option, ChoreoAuto auto) { + this.m_color = color; + this.m_option = option; + this.m_auto = auto; + } + + public AutoOption(AllianceColor color, int option) { + this.m_color = color; + this.m_option = option; + this.m_auto = null; + } + + public AllianceColor getColor() { + return this.m_color; + } + + public int getOption() { + return this.m_option; + } + + public ChoreoAuto getChoreoAuto() { + return this.m_auto; + } + } + + /** + * Gets the current drawn from the Power Distribution Hub by a CAN motor controller, assuming that + * (PDH port number + 10) = CAN ID + * + * @param CANBusPort The CAN ID of the motor controller + * @return Current in Amps on the PDH channel corresponding to the motor channel + */ + public double getPDHCurrent(int CANBusPort) { + return m_powerDistribution.getCurrent(CANBusPort - 10); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java deleted file mode 100644 index d11bde7..0000000 --- a/src/main/java/frc/robot/RobotContainer.java +++ /dev/null @@ -1,220 +0,0 @@ -// Copyright (c) Triple Helix Robotics, FRC 2363. All rights reserved. - -package frc.robot; - -import com.pathplanner.lib.commands.PathPlannerAuto; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.event.EventLoop; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc.lib.ControllerPatroller; -import frc.lib.SendableZorroController; -import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.OIConstants.Zorro; -import frc.robot.drivetrain.Drivetrain; -import frc.robot.drivetrain.commands.ZorroDriveCommand; -import java.util.function.IntSupplier; - -public class RobotContainer { - - private static RobotContainer INSTANCE = null; - - public static RobotContainer getRobotContainer() { - if (INSTANCE == null) { - INSTANCE = new RobotContainer(); - } - return INSTANCE; - } - - private final PowerDistribution m_PowerDistribution = new PowerDistribution(1, ModuleType.kRev); - - private final Drivetrain m_swerve = new Drivetrain(); - - private final EventLoop m_loop = new EventLoop(); - private SendableZorroController m_driver; - private XboxController m_operator; - - // digital inputs for autonomous selection - private final DigitalInput[] autonomousModes = - new DigitalInput[AutoConstants.kAutonomousModeSelectorPorts.length]; - - private Autonomous m_autonomous; - - public RobotContainer() { - - configureButtonBindings(); - - for (int i = 0; i < AutoConstants.kAutonomousModeSelectorPorts.length; i++) { - autonomousModes[i] = new DigitalInput(AutoConstants.kAutonomousModeSelectorPorts[i]); - } - - setDefaultCommands(); - - m_swerve.configurePathPlanner(); - - // Create a button on Smart Dashboard to reset the encoders. - SmartDashboard.putData( - "Align Encoders", - new InstantCommand(() -> m_swerve.zeroAbsTurningEncoderOffsets()).ignoringDisable(true)); - } - - public void configureButtonBindings() { - - // Clear any active buttons. - CommandScheduler.getInstance().getActiveButtonLoop().clear(); - - ControllerPatroller cp = ControllerPatroller.getInstance(); - - // We use two different types of controllers - Joystick & XboxController. - // Create objects of the specific types. - m_driver = new SendableZorroController(cp.findDriverPort()); - m_operator = new XboxController(cp.findOperatorPort()); - - configureDriverButtonBindings(); - configureOperatorButtonBindings(); - } - - /** - * @return The Command that runs the selected autonomous mode - */ - public Command getAutonomousCommand() { - updateSelectedAutonomous(); - if (m_autonomous != null) return m_autonomous.getPathPlannerAuto(); - else return null; - } - - public void teleopInit() {} - - public void autonomousInit() {} - - public void disabledInit() {} - - public void periodic() { - m_loop.poll(); - updateSelectedAutonomous(); - - if (m_autonomous != null) { - SmartDashboard.putString("Auto", m_autonomous.getFilename()); - } else { - SmartDashboard.putString("Auto", "Null"); - } - - SmartDashboard.putData(m_driver); - SmartDashboard.putData(m_operator); - SmartDashboard.putData(m_PowerDistribution); - } - - private class Autonomous { - - private final String filename; - - private Autonomous(String filename) { - this.filename = filename; - } - - private Command getPathPlannerAuto() { - return new PathPlannerAuto(filename); - } - - private String getFilename() { - return filename; - } - } - - // spotless:off - /** Updates the autonomous based on the physical selector switch */ - private void updateSelectedAutonomous() { - switch (getAutonomousModeSwitchIndex()) { - case 1: - m_autonomous = - m_swerve.redAllianceSupplier().getAsBoolean() - ? new Autonomous("R-TheOnePiece") - : new Autonomous("B-TheOnePiece"); - break; - - case 2: - m_autonomous = - m_swerve.redAllianceSupplier().getAsBoolean() - ? new Autonomous("R-TheTwoPieceNear") - : new Autonomous("B-TheTwoPieceNear"); - break; - - case 3: - m_autonomous = - m_swerve.redAllianceSupplier().getAsBoolean() - ? new Autonomous("R-TwoPieceFar1") - : new Autonomous("B-TwoPieceFar1"); - break; - - case 4: - m_autonomous = - m_swerve.redAllianceSupplier().getAsBoolean() - ? new Autonomous("R-ThreePieceAutoTame") - : new Autonomous("B-ThreePieceAutoTame"); - break; - - case 5: - m_autonomous = - m_swerve.redAllianceSupplier().getAsBoolean() - ? null - : null; - break; - - default: - m_autonomous = null; - } - } - // spotless:on - - /** - * @return Index in array of Digital Inputs corresponding to selected auto mode - */ - private int getAutonomousModeSwitchIndex() { - for (int port = 0; port < autonomousModes.length; port++) { - if (!autonomousModes[port].get()) { - return port + 1; - } - } - return 0; // failure of the physical switch - } - - private IntSupplier autonomousModeSelector() { - return () -> getAutonomousModeSwitchIndex(); - } - - /** - * Gets the current drawn from the Power Distribution Hub by a CAN motor controller, assuming that - * (PDH port number + 10) = CAN ID - * - * @param CANBusPort The CAN ID of the motor controller - * @return Current in Amps on the PDH channel corresponding to the motor channel - */ - public double getPDHCurrent(int CANBusPort) { - return m_PowerDistribution.getCurrent(CANBusPort - 10); - } - - private void setDefaultCommands() { - m_swerve.setDefaultCommand( - new ZorroDriveCommand(m_swerve, DriveConstants.kDriveKinematics, m_driver)); - } - - // spotless:off - private void configureDriverButtonBindings() { - - // Reset heading - new JoystickButton(m_driver, Zorro.kHIn) - .onTrue(new InstantCommand(() -> m_swerve.resetHeading()) - .ignoringDisable(true)); - - } - // spotless:on - - private void configureOperatorButtonBindings() {} -} diff --git a/src/main/java/frc/robot/autos/ChoreoAuto.java b/src/main/java/frc/robot/autos/ChoreoAuto.java new file mode 100644 index 0000000..60500bc --- /dev/null +++ b/src/main/java/frc/robot/autos/ChoreoAuto.java @@ -0,0 +1,11 @@ +package frc.robot.autos; + +import edu.wpi.first.wpilibj2.command.Command; + +public abstract class ChoreoAuto extends Command { + + /** + * @return The name of the auto mode + */ + public abstract String getName(); +} diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index e97cc13..cb88049 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -2,10 +2,12 @@ package frc.robot.drivetrain; +import choreo.trajectory.SwerveSample; + +import java.util.function.BooleanSupplier; + import com.kauailabs.navx.frc.AHRS; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -15,18 +17,20 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.AutoConstants.RotationControllerGains; +import frc.robot.Constants.AutoConstants.TranslationControllerGains; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.RobotConstants; -import java.util.function.BooleanSupplier; /** Constructs a swerve drive style drivetrain. */ public class Drivetrain extends SubsystemBase { + + private BooleanSupplier m_fieldRotatedSupplier; + static LinearVelocity kMaxSpeed = Constants.DriveConstants.kMaxTranslationalVelocity; static AngularVelocity kMaxAngularSpeed = Constants.DriveConstants.kMaxRotationalVelocity; @@ -59,6 +63,20 @@ public class Drivetrain extends SubsystemBase { private SwerveModule[] modules = {m_frontLeft, m_frontRight, m_rearLeft, m_rearRight}; + private final PIDController xController = + new PIDController( + TranslationControllerGains.kP, + TranslationControllerGains.kI, + TranslationControllerGains.kD); + private final PIDController yController = + new PIDController( + TranslationControllerGains.kP, + TranslationControllerGains.kI, + TranslationControllerGains.kD); + private final PIDController thetaController = + new PIDController( + RotationControllerGains.kP, RotationControllerGains.kI, RotationControllerGains.kD); + private final AHRS m_gyro = new AHRS(); private final SwerveDriveOdometry m_odometry = @@ -74,10 +92,10 @@ public class Drivetrain extends SubsystemBase { private final Field2d m_field = new Field2d(); - private final DigitalInput allianceSelectionSwitch = - new DigitalInput(AutoConstants.kAllianceColorSelectorPort); + public Drivetrain(BooleanSupplier fieldRotatedSupplier) { + + this.m_fieldRotatedSupplier = fieldRotatedSupplier; - public Drivetrain() { m_gyro.reset(); SmartDashboard.putData("Field", m_field); @@ -113,7 +131,6 @@ public void periodic() { SmartDashboard.putNumber(module.getName() + "OutputCurrent", module.getDriveMotorCurrent()); } - SmartDashboard.putBoolean("isRed", getRedAlliance()); SmartDashboard.putNumber("GyroAngle", m_gyro.getRotation2d().getDegrees()); } @@ -185,7 +202,7 @@ public void setPose(Pose2d pose) { public void resetHeading() { Pose2d pose = - getRedAlliance() + m_fieldRotatedSupplier.getAsBoolean() ? new Pose2d(getPose().getTranslation(), new Rotation2d(Math.PI)) : new Pose2d(getPose().getTranslation(), new Rotation2d()); @@ -230,38 +247,23 @@ public ChassisSpeeds getChassisSpeeds() { m_rearRight.getState()); } - // spotless:off - public void configurePathPlanner() { - AutoBuilder.configureHolonomic( - this::getPose, // Supplier for the current pose - this::setPose, // Consumer for resetting the pose - this::getChassisSpeeds, // Supplier for the current robot-relative chassis speeds - this::setChassisSpeeds, // Consumer for setting the robot-relative chassis speeds - - // Configuring the path following commands - new HolonomicPathFollowerConfig( - AutoConstants.kTranslationControllerGains, // Translation PID constants - AutoConstants.kRotationControllerGains, // Rotation PID constants - DriveConstants.kMaxTranslationalVelocity, // Max module speed, in m/s - DriveConstants.kRadius, // Drive base radius in meters - new ReplanningConfig() // Default path replanning config - ), - - () -> {return false;}, // Never mirror path - this // Requires this subsystem - ); + /** + * @param curPose The robot's current pose + * @param sample A sample of the trajectory being followed + */ + public void choreoController(Pose2d curPose, SwerveSample sample) { + ChassisSpeeds speeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + new ChassisSpeeds( + xController.calculate(curPose.getX(), sample.x) + sample.vx, + yController.calculate(curPose.getY(), sample.y) + sample.vy, + thetaController.calculate(curPose.getRotation().getRadians(), sample.heading) + + sample.omega), + curPose.getRotation()); + this.setChassisSpeeds(speeds); } - // spotless:on public BooleanSupplier fieldRotatedSupplier() { - return () -> allianceSelectionSwitch.get(); - } - - private boolean getRedAlliance() { - return allianceSelectionSwitch.get(); - } - - public BooleanSupplier redAllianceSupplier() { - return () -> allianceSelectionSwitch.get(); + return this.m_fieldRotatedSupplier; } } diff --git a/src/main/java/frc/robot/drivetrain/SwerveModule.java b/src/main/java/frc/robot/drivetrain/SwerveModule.java index 0a348be..458540f 100644 --- a/src/main/java/frc/robot/drivetrain/SwerveModule.java +++ b/src/main/java/frc/robot/drivetrain/SwerveModule.java @@ -19,6 +19,8 @@ import edu.wpi.first.wpilibj.Preferences; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.ModuleConstants; +import frc.robot.Constants.ModuleConstants.DriveControllerGains; +import frc.robot.Constants.ModuleConstants.TurningControllerGains; import frc.robot.Constants.RobotConstants; public class SwerveModule { @@ -71,16 +73,16 @@ public SwerveModule( m_drivePIDController = m_driveMotor.getPIDController(); m_turningPIDController = m_turningMotor.getPIDController(); - m_drivePIDController.setP(ModuleConstants.kDriveP); - m_drivePIDController.setI(ModuleConstants.kDriveI); - m_drivePIDController.setD(ModuleConstants.kDriveD); + m_drivePIDController.setP(DriveControllerGains.kP); + m_drivePIDController.setI(DriveControllerGains.kI); + m_drivePIDController.setD(DriveControllerGains.kD); // m_drivePIDController.setIZone(); - m_drivePIDController.setFF(ModuleConstants.kDriveFF); + m_drivePIDController.setFF(DriveControllerGains.kFF); // m_drivePIDController.setOutputRange(); - m_turningPIDController.setP(ModuleConstants.kTurningP); - m_turningPIDController.setI(ModuleConstants.kTurningI); - m_turningPIDController.setD(ModuleConstants.kTurningD); + m_turningPIDController.setP(TurningControllerGains.kP); + m_turningPIDController.setI(TurningControllerGains.kI); + m_turningPIDController.setD(TurningControllerGains.kD); // m_turningPIDController.setIZone(); // m_turningPIDController.setFF(); // m_turningPIDController.setOutputRange(); diff --git a/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java index 1493304..6268bd1 100644 --- a/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java @@ -44,15 +44,11 @@ public void execute() { SmartDashboard.putBoolean("rotateField", drivetrain.fieldRotatedSupplier().getAsBoolean()); drivetrain.setChassisSpeeds( - fieldRelative() ? getFieldRelativeChassisSpeeds() : getRobotRelativeChassisSpeedsReversed(), + fieldRelative() ? getFieldRelativeChassisSpeeds() : getRobotRelativeChassisSpeeds(), kinematicsType); } - private ChassisSpeeds getRobotRelativeChassisSpeedsForward() { - return new ChassisSpeeds(xDot, yDot, thetaDot); - } - - private ChassisSpeeds getRobotRelativeChassisSpeedsReversed() { + private ChassisSpeeds getRobotRelativeChassisSpeeds() { return new ChassisSpeeds(xDot.unaryMinus(), yDot.unaryMinus(), thetaDot); } diff --git a/vendordeps/ChoreoLib2025Beta.json b/vendordeps/ChoreoLib2025Beta.json new file mode 100644 index 0000000..8a02d49 --- /dev/null +++ b/vendordeps/ChoreoLib2025Beta.json @@ -0,0 +1,44 @@ +{ + "fileName": "ChoreoLib2025Beta.json", + "name": "ChoreoLib", + "version": "2025.0.0-beta-5", + "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", + "frcYear": "2025", + "mavenUrls": [ + "https://SleipnirGroup.github.io/ChoreoLib/dep", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib2025Beta.json", + "javaDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-java", + "version": "2025.0.0-beta-5" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.10.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-cpp", + "version": "2025.0.0-beta-5", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file