diff --git a/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json b/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json deleted file mode 100644 index 6ae8e1b..0000000 --- a/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "TeamNumber": 157 -} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index d0142f9..3769767 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2025", - "teamNumber": 157 + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2025", + "teamNumber": 157 } \ No newline at end of file diff --git a/8.11/checksums/checksums.lock b/8.11/checksums/checksums.lock deleted file mode 100644 index 89394e4..0000000 Binary files a/8.11/checksums/checksums.lock and /dev/null differ diff --git a/8.11/checksums/md5-checksums.bin b/8.11/checksums/md5-checksums.bin deleted file mode 100644 index 32623f2..0000000 Binary files a/8.11/checksums/md5-checksums.bin and /dev/null differ diff --git a/8.11/checksums/sha1-checksums.bin b/8.11/checksums/sha1-checksums.bin deleted file mode 100644 index 476b9d5..0000000 Binary files a/8.11/checksums/sha1-checksums.bin and /dev/null differ diff --git a/8.11/executionHistory/executionHistory.bin b/8.11/executionHistory/executionHistory.bin deleted file mode 100644 index dcc26f5..0000000 Binary files a/8.11/executionHistory/executionHistory.bin and /dev/null differ diff --git a/8.11/executionHistory/executionHistory.lock b/8.11/executionHistory/executionHistory.lock deleted file mode 100644 index 9c3d979..0000000 Binary files a/8.11/executionHistory/executionHistory.lock and /dev/null differ diff --git a/8.11/fileChanges/last-build.bin b/8.11/fileChanges/last-build.bin deleted file mode 100644 index f76dd23..0000000 Binary files a/8.11/fileChanges/last-build.bin and /dev/null differ diff --git a/8.11/fileHashes/fileHashes.bin b/8.11/fileHashes/fileHashes.bin deleted file mode 100644 index 66416b0..0000000 Binary files a/8.11/fileHashes/fileHashes.bin and /dev/null differ diff --git a/8.11/fileHashes/fileHashes.lock b/8.11/fileHashes/fileHashes.lock deleted file mode 100644 index 3228a62..0000000 Binary files a/8.11/fileHashes/fileHashes.lock and /dev/null differ diff --git a/8.11/fileHashes/resourceHashesCache.bin b/8.11/fileHashes/resourceHashesCache.bin deleted file mode 100644 index 2216592..0000000 Binary files a/8.11/fileHashes/resourceHashesCache.bin and /dev/null differ diff --git a/8.11/gc.properties b/8.11/gc.properties deleted file mode 100644 index e69de29..0000000 diff --git a/Constants.java b/Constants.java deleted file mode 100644 index e452735..0000000 --- a/Constants.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot; - -public class Constants { - public static class ControllerConstants { - - public static final int DRIVER_CONTROLLER_PORT = 0; - public static final int OPERATOR_CONTROLLER_PORT = 1; - - // Joystick Deadband - public static final double LEFT_X_DEADBAND = 0.1; - public static final double LEFT_Y_DEADBAND = 0.1; - public static final double RIGHT_X_DEADBAND = 0.1; - } - - public static class LoggingConstants { - - public static final boolean DEFAULT_LOGGING_STATE = false; - } - - - public static class CosmeticConstants { - - public static final int LIGHT_ID = 9; - public static final double SOLID_YELLOW_VALUE = 0.69; - public static final double SOLID_PURPLE_VALUE = 0.91; - public static final int LIGHT_LENGTH = 76; - - } - -} diff --git a/Inputs.java b/Inputs.java deleted file mode 100644 index 26d5bfa..0000000 --- a/Inputs.java +++ /dev/null @@ -1,98 +0,0 @@ -// 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. - -package frc.robot; - -import org.assabet.aztechs157.input.layouts.DynamicLayout; -import org.assabet.aztechs157.input.layouts.Layout; -import org.assabet.aztechs157.input.layouts.MapLayout; -import java.util.function.DoubleSupplier; -import org.assabet.aztechs157.input.models.XboxOne; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; -import org.assabet.aztechs157.numbers.Deadzone; -import org.assabet.aztechs157.numbers.Range; - -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import frc.robot.Constants.ControllerConstants; - -/** Add your docs here. */ -public class Inputs extends DynamicLayout { - - public static final Axis.Key precisionDrive = new Axis.Key(); - - ////////////////////////////////////////////////////////// - // HERE'S AN EXAMPLE OF USING ADDING NEW BUTTONS/AXIS - ///////////////////////////////////////////////////////// - - // public static final Button.Key resetGyro = new Button.Key(); - - // public static final Button.Key driveToSpeaker = new Button.Key(); - // public static final Button.Key driveToAmp = new Button.Key(); - // public static final Button.Key autoIntake = new Button.Key(); - - // public static final Button.Key intake = new Button.Key(); - // public static final Button.Key loadNote = new Button.Key(); - // public static final Button.Key eject = new Button.Key(); - - // public static final Button.Key highShotSpinUp = new Button.Key(); - // public static final Button.Key lowShotSpinUp = new Button.Key(); - - // public static final Button.Key highShot = new Button.Key(); - // public static final Button.Key lowShot = new Button.Key(); - // public static final Button.Key pass = new Button.Key(); - - // public static final Button.Key liftHanger = new Button.Key(); - // public static final Button.Key retractHanger = new Button.Key(); - // public static final Button.Key retractHangerPin = new Button.Key(); - // public static final Button.Key extendHangerPin = new Button.Key(); - - public static Inputs createFromChooser() { - final SendableChooser chooser = new SendableChooser<>(); - chooser.setDefaultOption("xbox", doubleXBOXLayout()); - Shuffleboard.getTab("Driver").add("Layout Choose", chooser); - - return new Inputs(chooser); - } - - private Inputs(final SendableChooser chooser) { - super(chooser::getSelected); - } - - private static Layout doubleXBOXLayout() { - - final var layout = new MapLayout(); - final var driver = new XboxOne(ControllerConstants.DRIVER_CONTROLLER_PORT); - final var operator = new XboxOne(ControllerConstants.OPERATOR_CONTROLLER_PORT); - - layout.assign(precisionDrive, driver.leftTriggerHeld.map(Deadzone.forAxis(new Range(0.0, 0.05))::apply).scaledBy(0.7)); - - ////////////////////////////////////////////////////////// - // HERE'S AN EXAMPLE OF USING CONTROLER LAYOUT OPTIONS - ///////////////////////////////////////////////////////// - - // layout.assign(driveToSpeaker, operator.a); - // layout.assign(driveToAmp, operator.b); - // layout.assign(resetGyro, driver.start); - // layout.assign(autoIntake, operator.leftBumper); - - // layout.assign(intake, driver.leftBumper); - // layout.assign(loadNote, operator.x); - - // layout.assign(highShotSpinUp, operator.rightBumper); - // layout.assign(lowShotSpinUp, operator.leftBumper); - - // layout.assign(highShot, new Button(() -> driver.rightTriggerHeld.get() > 0.2)); - // layout.assign(lowShot, driver.rightBumper); - // layout.assign(pass, operator.a); - // layout.assign(eject, operator.b); - - // layout.assign(liftHanger, operator.pov.up); - // layout.assign(retractHanger, operator.pov.down); - - return layout; - } - -} diff --git a/LoggingSystem.java b/LoggingSystem.java deleted file mode 100644 index 5c21203..0000000 --- a/LoggingSystem.java +++ /dev/null @@ -1,61 +0,0 @@ -// 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. - -package frc.robot.data; - -import java.util.Dictionary; -import java.util.Map; - -import com.ctre.phoenix6.SignalLogger; - -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.LoggingConstants; -import frc.robot.subsystems.DriveSystem; - -public class LoggingSystem extends SubsystemBase { - - private Map subsystemArray; - private boolean loggingState = LoggingConstants.DEFAULT_LOGGING_STATE; - private GenericEntry loggingChooser; - - - /** Creates a new LoggingSystem. */ - public LoggingSystem(Map subsystemArray) { - loggingChooser = Shuffleboard.getTab("Logging").add("Enable Logging", false).getEntry(); - SmartDashboard.putBoolean("Logging Chooser", false); - this.subsystemArray = subsystemArray; - } - - public boolean getLoggingState() { - return loggingState; - } - - private boolean getLoggingFlag() { - // An example command will be run in autonomous - return loggingChooser.getBoolean(false); // TODO: Actually use this value -} - - @Override - public void periodic() { - // This method will be called once per scheduler run - if (getLoggingFlag() != loggingState) { - loggingState = getLoggingFlag(); - - if (loggingState) { - SignalLogger.start(); - // Log the odometry pose as a double array - } else { - SignalLogger.stop(); - } - } - if (loggingState){ - var pose = ((DriveSystem)subsystemArray.get(new String("Drive"))).getState().Pose; - var status = SignalLogger.writeDoubleArray("odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); - } - } -} diff --git a/Robot.java b/Robot.java deleted file mode 100644 index 0556048..0000000 --- a/Robot.java +++ /dev/null @@ -1,76 +0,0 @@ -// 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. - -package frc.robot; - -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; - -public class Robot extends TimedRobot { - private Command m_autonomousCommand; - - private RobotContainer m_robotContainer; - - @Override - public void robotInit() { - m_robotContainer = new RobotContainer(); - } - - @Override - public void robotPeriodic() { - CommandScheduler.getInstance().run(); - } - - @Override - public void disabledInit() {} - - @Override - public void disabledPeriodic() {} - - @Override - public void disabledExit() {} - - @Override - public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - } - - @Override - public void autonomousPeriodic() {} - - @Override - public void autonomousExit() {} - - @Override - public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } - } - - @Override - public void teleopPeriodic() {} - - @Override - public void teleopExit() {} - - @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); - } - - @Override - public void testPeriodic() {} - - @Override - public void testExit() {} - - @Override - public void simulationPeriodic() {} -} diff --git a/RobotContainer.java b/RobotContainer.java deleted file mode 100644 index 5718dfb..0000000 --- a/RobotContainer.java +++ /dev/null @@ -1,130 +0,0 @@ -// 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. - -package frc.robot; - -import java.util.HashMap; -import java.util.Map; - -import org.assabet.aztechs157.input.layouts.Layout; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.Orchestra; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import com.pathplanner.lib.auto.AutoBuilder; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; - -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.DriveSystem; -import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.data.LoggingSystem; - -public class RobotContainer { - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - private Map SystemMap = new HashMap(); - - private final CommandXboxController joystick = new CommandXboxController(0); // My joystick - public final DriveSystem drivetrain = TunerConstants.createDrivetrain(); // My drivetrain - - {SystemMap.put("Drive",drivetrain);} - private final Layout inputs = Inputs.createFromChooser(); - private final LoggingSystem loggingSystem = new LoggingSystem(SystemMap); - private final SendableChooser autoChooser; - -/* Setting up bindings for necessary control of the swerve drive platform */ -private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() -.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband -.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors -private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); -private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); -private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric() -.withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - private final Telemetry logger = new Telemetry(MaxSpeed); - - // Slew Rate Limiters to limit acceleration of joystick inputs - // private final SlewRateLimiter xLimiter = new SlewRateLimiter(25); - // private final SlewRateLimiter yLimiter = new SlewRateLimiter(25); - // private final SlewRateLimiter rotLimiter = new SlewRateLimiter(1570); - - private Orchestra soundSystem = new Orchestra(); - - public RobotContainer() { - configureBindings(); - - autoChooser = AutoBuilder.buildAutoChooser("NothingAuto"); - SmartDashboard.putData("Auto Chooser", autoChooser); - for(int i = 0; i<4;i++){ - soundSystem.addInstrument(drivetrain.getModule(i).getDriveMotor(), 0); - soundSystem.addInstrument(drivetrain.getModule(i).getSteerMotor(), 1); - } - soundSystem.loadMusic("music/e1m1.chrp"); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // An example command will be run in autonomous - return autoChooser.getSelected(); - } - - public double modifySpeed(final double speed) { - final var modifier = 1 - inputs.axis(Inputs.precisionDrive).get(); - return speed * modifier; -} - - private void configureBindings() { - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain.applyRequest(() -> drive.withVelocityX(-joystick.getLeftY() /* TODO: check inversion */ * modifySpeed(MaxSpeed)) // Drive forward with - // negative Y (forward) - .withVelocityY(-joystick.getLeftX() /* TODO: check inversion */ * modifySpeed(MaxSpeed)) // Drive left with negative X (left) - .withRotationalRate(-joystick.getRightX() /* TODO: check inversion */ * MaxAngularRate) // Drive counterclockwise with negative X (left) - )); - - joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); - joystick.b().whileTrue(drivetrain.applyRequest(() -> - point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())) - )); - - joystick.pov(0).whileTrue(drivetrain.applyRequest(() -> - forwardStraight.withVelocityX(0.5).withVelocityY(0)) - ); - joystick.pov(180).whileTrue(drivetrain.applyRequest(() -> - forwardStraight.withVelocityX(-0.5).withVelocityY(0)) - ); - - // Run SysId routines when holding back/start and X/Y. - // Note that each routine should be run exactly once in a single log. - joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); - joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); - - // reset the field-centric heading on left bumper press - joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - - joystick.start().onTrue(drivetrain.runOnce(()-> {soundSystem.play();})); - joystick.back().onTrue(drivetrain.runOnce(()->{soundSystem.pause();})); - joystick.y().onTrue(drivetrain.runOnce(()->{soundSystem.stop();})); - - drivetrain.registerTelemetry(logger::telemeterize); - } - -} diff --git a/Telemetry.java b/Telemetry.java deleted file mode 100644 index a2d36dd..0000000 --- a/Telemetry.java +++ /dev/null @@ -1,124 +0,0 @@ -package frc.robot; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -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.networktables.StructArrayPublisher; -import edu.wpi.first.networktables.StructPublisher; -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 Telemetry { - 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 Telemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - SignalLogger.start(); - } - - /* What to publish over networktables for telemetry */ - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - - /* Robot swerve drive state */ - private final NetworkTable driveStateTable = inst.getTable("DriveState"); - private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); - private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); - private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); - private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); - private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); - - /* 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(); - - /* 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))), - }; - - private final double[] m_poseArray = new double[3]; - private final double[] m_moduleStatesArray = new double[8]; - private final double[] m_moduleTargetsArray = new double[8]; - - /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the swerve drive state */ - drivePose.set(state.Pose); - driveSpeeds.set(state.Speeds); - driveModuleStates.set(state.ModuleStates); - driveModuleTargets.set(state.ModuleTargets); - driveModulePositions.set(state.ModulePositions); - driveTimestamp.set(state.Timestamp); - driveOdometryFrequency.set(1.0 / state.OdometryPeriod); - - /* Also write to log file */ - m_poseArray[0] = state.Pose.getX(); - m_poseArray[1] = state.Pose.getY(); - m_poseArray[2] = state.Pose.getRotation().getDegrees(); - for (int i = 0; i < 4; ++i) { - m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians(); - m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond; - m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians(); - m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; - } - - SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); - SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); - SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); - SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - - /* Telemeterize the pose to a Field2d */ - fieldTypePub.set("Field2d"); - fieldPub.set(m_poseArray); - - /* Telemeterize the module states to a Mechanism2d */ - 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]); - } - } -} \ No newline at end of file diff --git a/TunerConstants.java b/TunerConstants.java deleted file mode 100644 index cbbbb05..0000000 --- a/TunerConstants.java +++ /dev/null @@ -1,286 +0,0 @@ -package frc.robot.generated; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.hardware.*; -import com.ctre.phoenix6.signals.*; -import com.ctre.phoenix6.swerve.*; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.*; - -import frc.robot.subsystems.DriveSystem; - -// Generated by the Tuner X Swerve Project Generator -// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html -public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) // 100, 0, 0.5 - .withKS(0.1).withKV(1.91).withKA(0) // 0, 1.5, 0 - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) // 3, 0, 0 - .withKS(0).withKV(0.124); // removed KA changed and KV from 0 - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); // 150 - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) - ); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); // Default Name - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); // 5 - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; // 3.5714285714285716 - - private static final double kDriveGearRatio = 7.363636363636365; // 6.122448979591837 - private static final double kSteerGearRatio = 15.42857142857143; // 12.8 - private static final Distance kWheelRadius = Inches.of(2.167); // 1.92 - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 13; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); // 0.00001 - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); // 0.001 - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); // 0.25 - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); // 0.25 - - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - - // Front Left - private static final int kFrontLeftDriveMotorId = 11; - private static final int kFrontLeftSteerMotorId = 12; - private static final int kFrontLeftEncoderId = 10; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); // -0.052734375 - private static final boolean kFrontLeftSteerMotorInverted = true; // false - private static final boolean kFrontLeftEncoderInverted = false; // new - - private static final Distance kFrontLeftXPos = Inches.of(10); // 9.25 - private static final Distance kFrontLeftYPos = Inches.of(10); // 9.25 - - // Front Right - private static final int kFrontRightDriveMotorId = 2; - private static final int kFrontRightSteerMotorId = 3; - private static final int kFrontRightEncoderId = 1; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); // -0.6796875 - private static final boolean kFrontRightSteerMotorInverted = true; // false - private static final boolean kFrontRightEncoderInverted = false; // new - - private static final Distance kFrontRightXPos = Inches.of(10); // 9.25 - private static final Distance kFrontRightYPos = Inches.of(-10); // -9.25 - - // Back Left - private static final int kBackLeftDriveMotorId = 8; - private static final int kBackLeftSteerMotorId = 9; - private static final int kBackLeftEncoderId = 7; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); // -0.304931640625 - private static final boolean kBackLeftSteerMotorInverted = true; // false - private static final boolean kBackLeftEncoderInverted = false; // new - - private static final Distance kBackLeftXPos = Inches.of(-10); // -9.25 - private static final Distance kBackLeftYPos = Inches.of(10); // 9.25 - - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 6; - private static final int kBackRightEncoderId = 4; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); // -0.032470703125 - private static final boolean kBackRightSteerMotorInverted = true; // false - private static final boolean kBackRightEncoderInverted = false; // new - - private static final Distance kBackRightXPos = Inches.of(-10); // -9.25 - private static final Distance kBackRightYPos = Inches.of(-10); // -9.25 - - - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted - ); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted - ); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted - ); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted - ); - - /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. - */ - public static DriveSystem createDrivetrain() { - return new DriveSystem( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ); - } - - - /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. - */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules - ); - } - } -} \ No newline at end of file diff --git a/_reference_docs/FRC2025AprilTagPositions.csv b/_reference_docs/FRC2025AprilTagPositions.csv new file mode 100644 index 0000000..b4e4302 --- /dev/null +++ b/_reference_docs/FRC2025AprilTagPositions.csv @@ -0,0 +1,23 @@ +Element,Side,ID,X,Y,Z,Z-Rotation,X-Rotation +Left Coral Station,Red,1,657.37,25.8,55.25,126,0 +Right Coral Station,Red,2,657.37,291.2,55.25,234,0 +Processor,Red,3,455.15,317.15,47.88,270,0 +Blue Barge,Red,4,365.2,241.64,70.73,0,30 +Red Barge,Red,5,365.2,75.39,70.73,0,30 +Close Left Reef,Red,6,530.49,130.17,8.75,300,0 +Close Center Reef,Red,7,546.87,158.5,8.75,0,0 +Close Right Reef,Red,8,530.49,186.83,8.75,60,0 +Far Right Reef,Red,9,497.77,186.83,8.75,120,0 +Far Center Reef,Red,10,481.39,158.5,8.75,180,0 +Far Left Reef,Red,11,497.77,130.17,8.75,240,0 +Right Coral Station,Blue,12,33.51,25.8,55.25,54,0 +Left Coral Station,Blue,13,33.51,291.2,55.25,306,0 +Blue Barge,Blue,14,325.68,241.64,70.73,180,30 +Red Barge,Blue,15,325.68,75.39,70.73,180,30 +Processor,Blue,16,235.73,-0.15,47.88,90,0 +Close Right Reef,Blue,17,160.39,130.17,8.75,240,0 +Close Center Reef,Blue,18,144,158.5,8.75,180,0 +Close Left Reef,Blue,19,160.39,186.83,8.75,120,0 +Far Left Reef,Blue,20,193.1,186.83,8.75,60,0 +Far Center Reef,Blue,21,209.49,158.5,8.75,0,0 +Far Right Reef,Blue,22,193.1,130.17,8.75,300,0 \ No newline at end of file diff --git a/build.gradle b/build.gradle index 81c0f3b..bb54b03 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.2.1" } java { @@ -33,7 +33,7 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + deleteOldFiles = true // Change to true to delete files on roboRIO that no // longer exist in deploy directory of this project } } @@ -47,7 +47,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. diff --git a/buildOutputCleanup/buildOutputCleanup.lock b/buildOutputCleanup/buildOutputCleanup.lock deleted file mode 100644 index 0e7756e..0000000 Binary files a/buildOutputCleanup/buildOutputCleanup.lock and /dev/null differ diff --git a/buildOutputCleanup/cache.properties b/buildOutputCleanup/cache.properties deleted file mode 100644 index ee61b37..0000000 --- a/buildOutputCleanup/cache.properties +++ /dev/null @@ -1,2 +0,0 @@ -#Tue Jan 07 19:12:58 EST 2025 -gradle.version=8.11 diff --git a/buildOutputCleanup/outputFiles.bin b/buildOutputCleanup/outputFiles.bin deleted file mode 100644 index 9045eb6..0000000 Binary files a/buildOutputCleanup/outputFiles.bin and /dev/null differ diff --git a/cosmetics/BlinkInLEDs.java b/cosmetics/BlinkInLEDs.java deleted file mode 100644 index f2241f0..0000000 --- a/cosmetics/BlinkInLEDs.java +++ /dev/null @@ -1,35 +0,0 @@ -// 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. - -package frc.robot.cosmetics; - -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.CosmeticConstants; - -// We are no longer using this file to run any code, it is merely left as a reference -public class BlinkInLEDs extends SubsystemBase { - public final Spark lightController = new Spark(CosmeticConstants.LIGHT_ID); - // private ShuffleboardTab tab = Shuffleboard.getTab("LED"); - // private GenericEntry lightColor = tab.add("led color", - // CosmeticConstants.SOLID_YELLOW_VALUE).getEntry(); - - /** Creates a new lights. */ - public BlinkInLEDs() { - } - - public void setYellow() { - lightController.set(CosmeticConstants.SOLID_YELLOW_VALUE); - } - - public void setPurple() { - lightController.set(CosmeticConstants.SOLID_PURPLE_VALUE); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - // lightController.set(lightColor.getDouble(CosmeticConstants.SOLID_YELLOW_VALUE)); - } -} diff --git a/cosmetics/PwmLEDs.java b/cosmetics/PwmLEDs.java deleted file mode 100644 index acb2796..0000000 --- a/cosmetics/PwmLEDs.java +++ /dev/null @@ -1,249 +0,0 @@ -// 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. - -package frc.robot.cosmetics; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.CosmeticConstants; - -public class PwmLEDs extends SubsystemBase { - private final AddressableLED lights = new AddressableLED(CosmeticConstants.LIGHT_ID); - private AddressableLEDBuffer buffer = new AddressableLEDBuffer(CosmeticConstants.LIGHT_LENGTH); - private Color color1 = Color.kBlack; - private Color color2 = Color.kBlack; - private double onLength = 0.0; - private double offLength = 0.0; - private int color1Length = 0; - private int color2Length = 0; - private double speed = 0; - private double cycleLength = 0.0; - private double duration = 0.0; - - private double mp1 = 90.0; - private double mp2 = 60.0; - private double mp3 = 30.0; - private double mp4 = 15.0; - private double mp5 = 10.0; - private double mp6 = 5.0; - private double mpTolerance = 0.5; - - private Mode lightMode = Mode.SOLID; - - public void setLightMode(Mode lightMode) { - this.lightMode = lightMode; - } - - public Color getColor1() { - return color1; - } - - public void setColor1(Color color1) { - this.color1 = color1; - } - - public Color getColor2() { - return color2; - } - - public void setColor2(Color color2) { - this.color2 = color2; - } - - public void setColor1Length(int color1Length) { - this.color1Length = color1Length; - } - - public void setColor2Length(int color2Length) { - this.color2Length = color2Length; - } - - public void setSpeed(double speed) { - this.speed = speed; - } - - public static enum Mode { - SOLID, WAVE, CLIMB, STROBE; - } - - /** Creates a new PwmLEDs. */ - public PwmLEDs() { - lights.setLength(CosmeticConstants.LIGHT_LENGTH); - lights.setData(buffer); - lights.start(); - } - - public void solid(Color color) { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setLED(i, color); - } - } - - public void setSolid(Color color) { - this.color1 = color; - this.lightMode = Mode.SOLID; - } - - public void wave(Color color1, Color color2, double cycleLength, double duration) { - double counter = (1 - ((Timer.getFPGATimestamp() % duration) / duration)) * 2.0 * Math.PI; - double counterDiffPerLed = (2.0 * Math.PI) / cycleLength; - for (int i = 0; i < buffer.getLength(); i++) { - counter += counterDiffPerLed; - if (i >= 0) { - double ratio = (Math.pow(Math.sin(counter), 0.4) + 1.0) / 2.0; - if (Double.isNaN(ratio)) { - ratio = (-Math.pow(Math.sin(counter + Math.PI), 0.4) + 1.0) / 2.0; - } - if (Double.isNaN(ratio)) { - ratio = 0.5; - } - double red = (color1.red * (1 - ratio)) + (color2.red * ratio); - double green = (color1.green * (1 - ratio)) + (color2.green * ratio); - double blue = (color1.blue * (1 - ratio)) + (color2.blue * ratio); - buffer.setLED(i, new Color(red, green, blue)); - } - } - } - - public void wave(Color color, double cycleLength, double duration) { - wave(color, Color.kBlack, cycleLength, duration); - } - - public void setWave(Color color1, Color color2, double cycleLength, double duration) { - this.color1 = color1; - this.color2 = color2; - this.cycleLength = cycleLength; - this.duration = duration; - this.lightMode = Mode.WAVE; - } - - public void climb(Color color1, Color color2, int color1Length, int color2Length, double speed) { - int counter = (int) Math.floor(Timer.getFPGATimestamp() * speed); - for (int i = 0; i < buffer.getLength(); i += color2Length + color1Length) { - for (int j = 0; j < color1Length; j++) { - buffer.setLED((i + j + counter) % buffer.getLength(), color1); - } - for (int j = color1Length; j < color1Length + color2Length; j++) { - buffer.setLED((i + j + counter) % buffer.getLength(), color2); - } - } - } - - public void climb(Color color, int colorLength, int offLength, double speed) { - climb(color, Color.kBlack, colorLength, offLength, speed); - } - - public void setClimb(Color color1, Color color2, int color1Length, int color2Length, double speed) { - this.color1 = color1; - this.color2 = color2; - this.color1Length = color1Length; - this.color2Length = color2Length; - this.speed = speed; - this.lightMode = Mode.CLIMB; - } - - public void strobe(Color color1, Color color2, double onLength, double offLength) { - boolean lightsOn = Timer.getFPGATimestamp() % onLength + offLength > onLength; - if (lightsOn) { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setLED(i, color1); - } - } else { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setLED(i, color2); - } - } - } - - public void strobe(Color color, double onLength, double offLength) { - strobe(color, Color.kBlack, onLength, offLength); - } - - public void setStrobe(Color color1, Color color2, double onLength, double offLength) { - this.color1 = color1; - this.color2 = color2; - this.onLength = onLength; - this.offLength = offLength; - this.lightMode = Mode.STROBE; - } - - public void setDefault() { - Color color1 = Color.kBlue; - Color color2 = Color.kGold; - if (DriverStation.isEStopped()) { - color1 = Color.kDarkGreen; - color2 = Color.kPowderBlue; - } - if (!DriverStation.isFMSAttached()) { - color1 = PwmLEDs.dimColor(color1, 0.25); - color2 = PwmLEDs.dimColor(color2, 0.25); - } - - setWave(color1, color2, 10, 3); - } - - public static Color dimColor(Color color, double brightness) { - return new Color(color.red * brightness, color.green * brightness, color.blue * brightness); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - - double time = Timer.getMatchTime(); - if (Math.abs(time - mp1) <= mpTolerance) { - solid(Color.kBlue); - lights.setData(buffer); - return; - } - if (Math.abs(time - mp2) <= mpTolerance) { - solid(Color.kGreen); - lights.setData(buffer); - return; - } - if (Math.abs(time - mp3) <= mpTolerance) { - solid(Color.kFirstRed); - lights.setData(buffer); - return; - } - if (time < mp4 && time > mp5 && time % 1.0 > 0.5) { - solid(dimColor(Color.kDarkGoldenrod, 0.5)); - lights.setData(buffer); - return; - } - if (time < mp5 && time > mp6 && time % 1.0 > 0.5) { - solid(dimColor(Color.kFirstBlue, 0.5)); - lights.setData(buffer); - return; - } - if (time < mp6 && time % 1.0 > 0.5) { - solid(dimColor(Color.kFirstRed, 0.5)); - lights.setData(buffer); - return; - } - - switch (lightMode) { - case SOLID: - solid(color1); - break; - case WAVE: - wave(color1, color2, cycleLength, duration); - break; - case CLIMB: - climb(color1, color2, color1Length, color2Length, speed); - break; - case STROBE: - strobe(color1, color2, onLength, offLength); - break; - default: - solid(Color.kBlack); - } - - lights.setData(buffer); - } -} diff --git a/data/LoggingSystem.java b/data/LoggingSystem.java deleted file mode 100644 index 5c21203..0000000 --- a/data/LoggingSystem.java +++ /dev/null @@ -1,61 +0,0 @@ -// 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. - -package frc.robot.data; - -import java.util.Dictionary; -import java.util.Map; - -import com.ctre.phoenix6.SignalLogger; - -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.LoggingConstants; -import frc.robot.subsystems.DriveSystem; - -public class LoggingSystem extends SubsystemBase { - - private Map subsystemArray; - private boolean loggingState = LoggingConstants.DEFAULT_LOGGING_STATE; - private GenericEntry loggingChooser; - - - /** Creates a new LoggingSystem. */ - public LoggingSystem(Map subsystemArray) { - loggingChooser = Shuffleboard.getTab("Logging").add("Enable Logging", false).getEntry(); - SmartDashboard.putBoolean("Logging Chooser", false); - this.subsystemArray = subsystemArray; - } - - public boolean getLoggingState() { - return loggingState; - } - - private boolean getLoggingFlag() { - // An example command will be run in autonomous - return loggingChooser.getBoolean(false); // TODO: Actually use this value -} - - @Override - public void periodic() { - // This method will be called once per scheduler run - if (getLoggingFlag() != loggingState) { - loggingState = getLoggingFlag(); - - if (loggingState) { - SignalLogger.start(); - // Log the odometry pose as a double array - } else { - SignalLogger.stop(); - } - } - if (loggingState){ - var pose = ((DriveSystem)subsystemArray.get(new String("Drive"))).getState().Pose; - var status = SignalLogger.writeDoubleArray("odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); - } - } -} diff --git a/deploy/music/BuddyHollyRiff.chrp b/deploy/music/BuddyHollyRiff.chrp deleted file mode 100644 index 9489fdd..0000000 Binary files a/deploy/music/BuddyHollyRiff.chrp and /dev/null differ diff --git a/deploy/music/MoreCowbell.chrp b/deploy/music/MoreCowbell.chrp deleted file mode 100644 index e5baa9f..0000000 Binary files a/deploy/music/MoreCowbell.chrp and /dev/null differ diff --git a/deploy/music/TOTTFIY.chrp b/deploy/music/TOTTFIY.chrp deleted file mode 100644 index 52411f1..0000000 Binary files a/deploy/music/TOTTFIY.chrp and /dev/null differ diff --git a/deploy/music/WTTBPchrp.chrp b/deploy/music/WTTBPchrp.chrp deleted file mode 100644 index 0b048cd..0000000 Binary files a/deploy/music/WTTBPchrp.chrp and /dev/null differ diff --git a/deploy/music/YourLoveOF.chrp b/deploy/music/YourLoveOF.chrp deleted file mode 100644 index dda3a25..0000000 Binary files a/deploy/music/YourLoveOF.chrp and /dev/null differ diff --git a/deploy/music/adams.chrp b/deploy/music/adams.chrp deleted file mode 100644 index f5be11b..0000000 Binary files a/deploy/music/adams.chrp and /dev/null differ diff --git a/deploy/music/bluetoothdeviceisreadytopair.chrp b/deploy/music/bluetoothdeviceisreadytopair.chrp deleted file mode 100644 index 6bb6ad6..0000000 Binary files a/deploy/music/bluetoothdeviceisreadytopair.chrp and /dev/null differ diff --git a/deploy/music/e1m1.chrp b/deploy/music/e1m1.chrp deleted file mode 100644 index 08f2fe6..0000000 Binary files a/deploy/music/e1m1.chrp and /dev/null differ diff --git a/deploy/music/entry.chrp b/deploy/music/entry.chrp deleted file mode 100644 index b384863..0000000 Binary files a/deploy/music/entry.chrp and /dev/null differ diff --git a/deploy/music/exorcist.chrp b/deploy/music/exorcist.chrp deleted file mode 100644 index b8d1d6f..0000000 Binary files a/deploy/music/exorcist.chrp and /dev/null differ diff --git a/deploy/music/output.chrp b/deploy/music/output.chrp deleted file mode 100644 index ca4e227..0000000 Binary files a/deploy/music/output.chrp and /dev/null differ diff --git a/deploy/music/sundog.chrp b/deploy/music/sundog.chrp deleted file mode 100644 index 0c76bb1..0000000 Binary files a/deploy/music/sundog.chrp and /dev/null differ diff --git a/deploy/music/super_mario.chrp b/deploy/music/super_mario.chrp deleted file mode 100644 index 16f8649..0000000 Binary files a/deploy/music/super_mario.chrp and /dev/null differ diff --git a/deploy/music/tetris.chrp b/deploy/music/tetris.chrp deleted file mode 100644 index 88363c3..0000000 Binary files a/deploy/music/tetris.chrp and /dev/null differ diff --git a/deploy/pathplanner/autos/1m backward auto.auto b/deploy/pathplanner/autos/1m backward auto.auto deleted file mode 100644 index e988db7..0000000 --- a/deploy/pathplanner/autos/1m backward auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m backward" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/deploy/pathplanner/autos/1m forward & backwards x25.auto b/deploy/pathplanner/autos/1m forward & backwards x25.auto deleted file mode 100644 index e2f253c..0000000 --- a/deploy/pathplanner/autos/1m forward & backwards x25.auto +++ /dev/null @@ -1,169 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 2.0, - "y": 7.0 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/deploy/pathplanner/autos/1m forward & backwards.auto b/deploy/pathplanner/autos/1m forward & backwards.auto deleted file mode 100644 index 79adb64..0000000 --- a/deploy/pathplanner/autos/1m forward & backwards.auto +++ /dev/null @@ -1,302 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/deploy/pathplanner/autos/1m forward auto.auto b/deploy/pathplanner/autos/1m forward auto.auto deleted file mode 100644 index de1be04..0000000 --- a/deploy/pathplanner/autos/1m forward auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m forward" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/deploy/pathplanner/autos/1m right auto.auto b/deploy/pathplanner/autos/1m right auto.auto deleted file mode 100644 index 69e6f18..0000000 --- a/deploy/pathplanner/autos/1m right auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m right" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/deploy/pathplanner/navgrid.json b/deploy/pathplanner/navgrid.json deleted file mode 100644 index bab0da9..0000000 --- a/deploy/pathplanner/navgrid.json +++ /dev/null @@ -1 +0,0 @@ -{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/deploy/pathplanner/paths/1m Back and forth.path b/deploy/pathplanner/paths/1m Back and forth.path deleted file mode 100644 index add093c..0000000 --- a/deploy/pathplanner/paths/1m Back and forth.path +++ /dev/null @@ -1,103 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.1, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0, - "y": 7.0 - }, - "prevControl": { - "x": 2.9, - "y": 7.0 - }, - "nextControl": { - "x": 3.1, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": { - "x": 1.9, - "y": 7.0 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ - { - "name": "Wait", - "waypointRelativePos": 1.0, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - } - ] - } - } - }, - { - "name": "Wait 2", - "waypointRelativePos": 0, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/deploy/pathplanner/paths/1m backward.path b/deploy/pathplanner/paths/1m backward.path deleted file mode 100644 index fa735aa..0000000 --- a/deploy/pathplanner/paths/1m backward.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 5.59 - }, - "prevControl": null, - "nextControl": { - "x": 2.1, - "y": 5.59 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0, - "y": 5.59 - }, - "prevControl": { - "x": 1.1, - "y": 5.59 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/deploy/pathplanner/paths/1m forward.path b/deploy/pathplanner/paths/1m forward.path deleted file mode 100644 index 544f728..0000000 --- a/deploy/pathplanner/paths/1m forward.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.0, - "y": 5.589805823486292 - }, - "prevControl": null, - "nextControl": { - "x": 1.9999999999999967, - "y": 5.589805823486292 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.0, - "y": 5.589805823486292 - }, - "prevControl": { - "x": 1.0, - "y": 5.589805823486292 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/deploy/pathplanner/paths/1m left.path b/deploy/pathplanner/paths/1m left.path deleted file mode 100644 index 1482a76..0000000 --- a/deploy/pathplanner/paths/1m left.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.0, - "y": 5.59 - }, - "prevControl": null, - "nextControl": { - "x": 1.0, - "y": 6.502346398657132 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0, - "y": 6.59 - }, - "prevControl": { - "x": 0.9999999999999999, - "y": 5.59 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/deploy/pathplanner/paths/1m right.path b/deploy/pathplanner/paths/1m right.path deleted file mode 100644 index 88a7b65..0000000 --- a/deploy/pathplanner/paths/1m right.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.0, - "y": 5.59 - }, - "prevControl": null, - "nextControl": { - "x": 1.0, - "y": 5.49 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0, - "y": 4.59 - }, - "prevControl": { - "x": 1.0, - "y": 4.54 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/deploy/pathplanner/paths/Stephen.path b/deploy/pathplanner/paths/Stephen.path deleted file mode 100644 index 9126da3..0000000 --- a/deploy/pathplanner/paths/Stephen.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.439971575099845, - "y": 5.028486410249174 - }, - "prevControl": { - "x": 7.182700177366168, - "y": 6.2563726267053665 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -33.02386755579669, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/deploy/pathplanner/paths/test.path b/deploy/pathplanner/paths/test.path deleted file mode 100644 index 16dc425..0000000 --- a/deploy/pathplanner/paths/test.path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.200391204201211, - "y": 6.572114796651245 - }, - "prevControl": { - "x": 5.200391204201211, - "y": 6.572114796651245 - }, - "nextControl": { - "x": 7.200391204201211, - "y": 6.572114796651245 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.486748192869604, - "y": 4.654273468091096 - }, - "prevControl": { - "x": 7.694394331808605, - "y": 5.256522421876753 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/example.txt b/example.txt deleted file mode 100644 index bb82515..0000000 --- a/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function -to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/file-system.probe b/file-system.probe deleted file mode 100644 index 9d8bc6c..0000000 Binary files a/file-system.probe and /dev/null differ diff --git a/java/frc/robot/Constants.java b/java/frc/robot/Constants.java deleted file mode 100644 index e452735..0000000 --- a/java/frc/robot/Constants.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot; - -public class Constants { - public static class ControllerConstants { - - public static final int DRIVER_CONTROLLER_PORT = 0; - public static final int OPERATOR_CONTROLLER_PORT = 1; - - // Joystick Deadband - public static final double LEFT_X_DEADBAND = 0.1; - public static final double LEFT_Y_DEADBAND = 0.1; - public static final double RIGHT_X_DEADBAND = 0.1; - } - - public static class LoggingConstants { - - public static final boolean DEFAULT_LOGGING_STATE = false; - } - - - public static class CosmeticConstants { - - public static final int LIGHT_ID = 9; - public static final double SOLID_YELLOW_VALUE = 0.69; - public static final double SOLID_PURPLE_VALUE = 0.91; - public static final int LIGHT_LENGTH = 76; - - } - -} diff --git a/java/frc/robot/Inputs.java b/java/frc/robot/Inputs.java deleted file mode 100644 index 26d5bfa..0000000 --- a/java/frc/robot/Inputs.java +++ /dev/null @@ -1,98 +0,0 @@ -// 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. - -package frc.robot; - -import org.assabet.aztechs157.input.layouts.DynamicLayout; -import org.assabet.aztechs157.input.layouts.Layout; -import org.assabet.aztechs157.input.layouts.MapLayout; -import java.util.function.DoubleSupplier; -import org.assabet.aztechs157.input.models.XboxOne; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; -import org.assabet.aztechs157.numbers.Deadzone; -import org.assabet.aztechs157.numbers.Range; - -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import frc.robot.Constants.ControllerConstants; - -/** Add your docs here. */ -public class Inputs extends DynamicLayout { - - public static final Axis.Key precisionDrive = new Axis.Key(); - - ////////////////////////////////////////////////////////// - // HERE'S AN EXAMPLE OF USING ADDING NEW BUTTONS/AXIS - ///////////////////////////////////////////////////////// - - // public static final Button.Key resetGyro = new Button.Key(); - - // public static final Button.Key driveToSpeaker = new Button.Key(); - // public static final Button.Key driveToAmp = new Button.Key(); - // public static final Button.Key autoIntake = new Button.Key(); - - // public static final Button.Key intake = new Button.Key(); - // public static final Button.Key loadNote = new Button.Key(); - // public static final Button.Key eject = new Button.Key(); - - // public static final Button.Key highShotSpinUp = new Button.Key(); - // public static final Button.Key lowShotSpinUp = new Button.Key(); - - // public static final Button.Key highShot = new Button.Key(); - // public static final Button.Key lowShot = new Button.Key(); - // public static final Button.Key pass = new Button.Key(); - - // public static final Button.Key liftHanger = new Button.Key(); - // public static final Button.Key retractHanger = new Button.Key(); - // public static final Button.Key retractHangerPin = new Button.Key(); - // public static final Button.Key extendHangerPin = new Button.Key(); - - public static Inputs createFromChooser() { - final SendableChooser chooser = new SendableChooser<>(); - chooser.setDefaultOption("xbox", doubleXBOXLayout()); - Shuffleboard.getTab("Driver").add("Layout Choose", chooser); - - return new Inputs(chooser); - } - - private Inputs(final SendableChooser chooser) { - super(chooser::getSelected); - } - - private static Layout doubleXBOXLayout() { - - final var layout = new MapLayout(); - final var driver = new XboxOne(ControllerConstants.DRIVER_CONTROLLER_PORT); - final var operator = new XboxOne(ControllerConstants.OPERATOR_CONTROLLER_PORT); - - layout.assign(precisionDrive, driver.leftTriggerHeld.map(Deadzone.forAxis(new Range(0.0, 0.05))::apply).scaledBy(0.7)); - - ////////////////////////////////////////////////////////// - // HERE'S AN EXAMPLE OF USING CONTROLER LAYOUT OPTIONS - ///////////////////////////////////////////////////////// - - // layout.assign(driveToSpeaker, operator.a); - // layout.assign(driveToAmp, operator.b); - // layout.assign(resetGyro, driver.start); - // layout.assign(autoIntake, operator.leftBumper); - - // layout.assign(intake, driver.leftBumper); - // layout.assign(loadNote, operator.x); - - // layout.assign(highShotSpinUp, operator.rightBumper); - // layout.assign(lowShotSpinUp, operator.leftBumper); - - // layout.assign(highShot, new Button(() -> driver.rightTriggerHeld.get() > 0.2)); - // layout.assign(lowShot, driver.rightBumper); - // layout.assign(pass, operator.a); - // layout.assign(eject, operator.b); - - // layout.assign(liftHanger, operator.pov.up); - // layout.assign(retractHanger, operator.pov.down); - - return layout; - } - -} diff --git a/java/frc/robot/Main.java b/java/frc/robot/Main.java deleted file mode 100644 index fe215d7..0000000 --- a/java/frc/robot/Main.java +++ /dev/null @@ -1,15 +0,0 @@ -// 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. - -package frc.robot; - -import edu.wpi.first.wpilibj.RobotBase; - -public final class Main { - private Main() {} - - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} diff --git a/java/frc/robot/RobotContainer.java b/java/frc/robot/RobotContainer.java deleted file mode 100644 index 5718dfb..0000000 --- a/java/frc/robot/RobotContainer.java +++ /dev/null @@ -1,130 +0,0 @@ -// 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. - -package frc.robot; - -import java.util.HashMap; -import java.util.Map; - -import org.assabet.aztechs157.input.layouts.Layout; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.Orchestra; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import com.pathplanner.lib.auto.AutoBuilder; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; - -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.DriveSystem; -import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.data.LoggingSystem; - -public class RobotContainer { - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - private Map SystemMap = new HashMap(); - - private final CommandXboxController joystick = new CommandXboxController(0); // My joystick - public final DriveSystem drivetrain = TunerConstants.createDrivetrain(); // My drivetrain - - {SystemMap.put("Drive",drivetrain);} - private final Layout inputs = Inputs.createFromChooser(); - private final LoggingSystem loggingSystem = new LoggingSystem(SystemMap); - private final SendableChooser autoChooser; - -/* Setting up bindings for necessary control of the swerve drive platform */ -private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() -.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband -.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors -private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); -private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); -private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric() -.withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - private final Telemetry logger = new Telemetry(MaxSpeed); - - // Slew Rate Limiters to limit acceleration of joystick inputs - // private final SlewRateLimiter xLimiter = new SlewRateLimiter(25); - // private final SlewRateLimiter yLimiter = new SlewRateLimiter(25); - // private final SlewRateLimiter rotLimiter = new SlewRateLimiter(1570); - - private Orchestra soundSystem = new Orchestra(); - - public RobotContainer() { - configureBindings(); - - autoChooser = AutoBuilder.buildAutoChooser("NothingAuto"); - SmartDashboard.putData("Auto Chooser", autoChooser); - for(int i = 0; i<4;i++){ - soundSystem.addInstrument(drivetrain.getModule(i).getDriveMotor(), 0); - soundSystem.addInstrument(drivetrain.getModule(i).getSteerMotor(), 1); - } - soundSystem.loadMusic("music/e1m1.chrp"); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // An example command will be run in autonomous - return autoChooser.getSelected(); - } - - public double modifySpeed(final double speed) { - final var modifier = 1 - inputs.axis(Inputs.precisionDrive).get(); - return speed * modifier; -} - - private void configureBindings() { - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain.applyRequest(() -> drive.withVelocityX(-joystick.getLeftY() /* TODO: check inversion */ * modifySpeed(MaxSpeed)) // Drive forward with - // negative Y (forward) - .withVelocityY(-joystick.getLeftX() /* TODO: check inversion */ * modifySpeed(MaxSpeed)) // Drive left with negative X (left) - .withRotationalRate(-joystick.getRightX() /* TODO: check inversion */ * MaxAngularRate) // Drive counterclockwise with negative X (left) - )); - - joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); - joystick.b().whileTrue(drivetrain.applyRequest(() -> - point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())) - )); - - joystick.pov(0).whileTrue(drivetrain.applyRequest(() -> - forwardStraight.withVelocityX(0.5).withVelocityY(0)) - ); - joystick.pov(180).whileTrue(drivetrain.applyRequest(() -> - forwardStraight.withVelocityX(-0.5).withVelocityY(0)) - ); - - // Run SysId routines when holding back/start and X/Y. - // Note that each routine should be run exactly once in a single log. - joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); - joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); - - // reset the field-centric heading on left bumper press - joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - - joystick.start().onTrue(drivetrain.runOnce(()-> {soundSystem.play();})); - joystick.back().onTrue(drivetrain.runOnce(()->{soundSystem.pause();})); - joystick.y().onTrue(drivetrain.runOnce(()->{soundSystem.stop();})); - - drivetrain.registerTelemetry(logger::telemeterize); - } - -} diff --git a/java/frc/robot/Telemetry.java b/java/frc/robot/Telemetry.java deleted file mode 100644 index a2d36dd..0000000 --- a/java/frc/robot/Telemetry.java +++ /dev/null @@ -1,124 +0,0 @@ -package frc.robot; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -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.networktables.StructArrayPublisher; -import edu.wpi.first.networktables.StructPublisher; -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 Telemetry { - 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 Telemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - SignalLogger.start(); - } - - /* What to publish over networktables for telemetry */ - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - - /* Robot swerve drive state */ - private final NetworkTable driveStateTable = inst.getTable("DriveState"); - private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); - private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); - private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); - private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); - private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); - - /* 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(); - - /* 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))), - }; - - private final double[] m_poseArray = new double[3]; - private final double[] m_moduleStatesArray = new double[8]; - private final double[] m_moduleTargetsArray = new double[8]; - - /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the swerve drive state */ - drivePose.set(state.Pose); - driveSpeeds.set(state.Speeds); - driveModuleStates.set(state.ModuleStates); - driveModuleTargets.set(state.ModuleTargets); - driveModulePositions.set(state.ModulePositions); - driveTimestamp.set(state.Timestamp); - driveOdometryFrequency.set(1.0 / state.OdometryPeriod); - - /* Also write to log file */ - m_poseArray[0] = state.Pose.getX(); - m_poseArray[1] = state.Pose.getY(); - m_poseArray[2] = state.Pose.getRotation().getDegrees(); - for (int i = 0; i < 4; ++i) { - m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians(); - m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond; - m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians(); - m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; - } - - SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); - SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); - SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); - SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - - /* Telemeterize the pose to a Field2d */ - fieldTypePub.set("Field2d"); - fieldPub.set(m_poseArray); - - /* Telemeterize the module states to a Mechanism2d */ - 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]); - } - } -} \ No newline at end of file diff --git a/java/frc/robot/cosmetics/BlinkInLEDs.java b/java/frc/robot/cosmetics/BlinkInLEDs.java deleted file mode 100644 index f2241f0..0000000 --- a/java/frc/robot/cosmetics/BlinkInLEDs.java +++ /dev/null @@ -1,35 +0,0 @@ -// 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. - -package frc.robot.cosmetics; - -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.CosmeticConstants; - -// We are no longer using this file to run any code, it is merely left as a reference -public class BlinkInLEDs extends SubsystemBase { - public final Spark lightController = new Spark(CosmeticConstants.LIGHT_ID); - // private ShuffleboardTab tab = Shuffleboard.getTab("LED"); - // private GenericEntry lightColor = tab.add("led color", - // CosmeticConstants.SOLID_YELLOW_VALUE).getEntry(); - - /** Creates a new lights. */ - public BlinkInLEDs() { - } - - public void setYellow() { - lightController.set(CosmeticConstants.SOLID_YELLOW_VALUE); - } - - public void setPurple() { - lightController.set(CosmeticConstants.SOLID_PURPLE_VALUE); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - // lightController.set(lightColor.getDouble(CosmeticConstants.SOLID_YELLOW_VALUE)); - } -} diff --git a/java/frc/robot/cosmetics/PwmLEDs.java b/java/frc/robot/cosmetics/PwmLEDs.java deleted file mode 100644 index acb2796..0000000 --- a/java/frc/robot/cosmetics/PwmLEDs.java +++ /dev/null @@ -1,249 +0,0 @@ -// 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. - -package frc.robot.cosmetics; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.CosmeticConstants; - -public class PwmLEDs extends SubsystemBase { - private final AddressableLED lights = new AddressableLED(CosmeticConstants.LIGHT_ID); - private AddressableLEDBuffer buffer = new AddressableLEDBuffer(CosmeticConstants.LIGHT_LENGTH); - private Color color1 = Color.kBlack; - private Color color2 = Color.kBlack; - private double onLength = 0.0; - private double offLength = 0.0; - private int color1Length = 0; - private int color2Length = 0; - private double speed = 0; - private double cycleLength = 0.0; - private double duration = 0.0; - - private double mp1 = 90.0; - private double mp2 = 60.0; - private double mp3 = 30.0; - private double mp4 = 15.0; - private double mp5 = 10.0; - private double mp6 = 5.0; - private double mpTolerance = 0.5; - - private Mode lightMode = Mode.SOLID; - - public void setLightMode(Mode lightMode) { - this.lightMode = lightMode; - } - - public Color getColor1() { - return color1; - } - - public void setColor1(Color color1) { - this.color1 = color1; - } - - public Color getColor2() { - return color2; - } - - public void setColor2(Color color2) { - this.color2 = color2; - } - - public void setColor1Length(int color1Length) { - this.color1Length = color1Length; - } - - public void setColor2Length(int color2Length) { - this.color2Length = color2Length; - } - - public void setSpeed(double speed) { - this.speed = speed; - } - - public static enum Mode { - SOLID, WAVE, CLIMB, STROBE; - } - - /** Creates a new PwmLEDs. */ - public PwmLEDs() { - lights.setLength(CosmeticConstants.LIGHT_LENGTH); - lights.setData(buffer); - lights.start(); - } - - public void solid(Color color) { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setLED(i, color); - } - } - - public void setSolid(Color color) { - this.color1 = color; - this.lightMode = Mode.SOLID; - } - - public void wave(Color color1, Color color2, double cycleLength, double duration) { - double counter = (1 - ((Timer.getFPGATimestamp() % duration) / duration)) * 2.0 * Math.PI; - double counterDiffPerLed = (2.0 * Math.PI) / cycleLength; - for (int i = 0; i < buffer.getLength(); i++) { - counter += counterDiffPerLed; - if (i >= 0) { - double ratio = (Math.pow(Math.sin(counter), 0.4) + 1.0) / 2.0; - if (Double.isNaN(ratio)) { - ratio = (-Math.pow(Math.sin(counter + Math.PI), 0.4) + 1.0) / 2.0; - } - if (Double.isNaN(ratio)) { - ratio = 0.5; - } - double red = (color1.red * (1 - ratio)) + (color2.red * ratio); - double green = (color1.green * (1 - ratio)) + (color2.green * ratio); - double blue = (color1.blue * (1 - ratio)) + (color2.blue * ratio); - buffer.setLED(i, new Color(red, green, blue)); - } - } - } - - public void wave(Color color, double cycleLength, double duration) { - wave(color, Color.kBlack, cycleLength, duration); - } - - public void setWave(Color color1, Color color2, double cycleLength, double duration) { - this.color1 = color1; - this.color2 = color2; - this.cycleLength = cycleLength; - this.duration = duration; - this.lightMode = Mode.WAVE; - } - - public void climb(Color color1, Color color2, int color1Length, int color2Length, double speed) { - int counter = (int) Math.floor(Timer.getFPGATimestamp() * speed); - for (int i = 0; i < buffer.getLength(); i += color2Length + color1Length) { - for (int j = 0; j < color1Length; j++) { - buffer.setLED((i + j + counter) % buffer.getLength(), color1); - } - for (int j = color1Length; j < color1Length + color2Length; j++) { - buffer.setLED((i + j + counter) % buffer.getLength(), color2); - } - } - } - - public void climb(Color color, int colorLength, int offLength, double speed) { - climb(color, Color.kBlack, colorLength, offLength, speed); - } - - public void setClimb(Color color1, Color color2, int color1Length, int color2Length, double speed) { - this.color1 = color1; - this.color2 = color2; - this.color1Length = color1Length; - this.color2Length = color2Length; - this.speed = speed; - this.lightMode = Mode.CLIMB; - } - - public void strobe(Color color1, Color color2, double onLength, double offLength) { - boolean lightsOn = Timer.getFPGATimestamp() % onLength + offLength > onLength; - if (lightsOn) { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setLED(i, color1); - } - } else { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setLED(i, color2); - } - } - } - - public void strobe(Color color, double onLength, double offLength) { - strobe(color, Color.kBlack, onLength, offLength); - } - - public void setStrobe(Color color1, Color color2, double onLength, double offLength) { - this.color1 = color1; - this.color2 = color2; - this.onLength = onLength; - this.offLength = offLength; - this.lightMode = Mode.STROBE; - } - - public void setDefault() { - Color color1 = Color.kBlue; - Color color2 = Color.kGold; - if (DriverStation.isEStopped()) { - color1 = Color.kDarkGreen; - color2 = Color.kPowderBlue; - } - if (!DriverStation.isFMSAttached()) { - color1 = PwmLEDs.dimColor(color1, 0.25); - color2 = PwmLEDs.dimColor(color2, 0.25); - } - - setWave(color1, color2, 10, 3); - } - - public static Color dimColor(Color color, double brightness) { - return new Color(color.red * brightness, color.green * brightness, color.blue * brightness); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - - double time = Timer.getMatchTime(); - if (Math.abs(time - mp1) <= mpTolerance) { - solid(Color.kBlue); - lights.setData(buffer); - return; - } - if (Math.abs(time - mp2) <= mpTolerance) { - solid(Color.kGreen); - lights.setData(buffer); - return; - } - if (Math.abs(time - mp3) <= mpTolerance) { - solid(Color.kFirstRed); - lights.setData(buffer); - return; - } - if (time < mp4 && time > mp5 && time % 1.0 > 0.5) { - solid(dimColor(Color.kDarkGoldenrod, 0.5)); - lights.setData(buffer); - return; - } - if (time < mp5 && time > mp6 && time % 1.0 > 0.5) { - solid(dimColor(Color.kFirstBlue, 0.5)); - lights.setData(buffer); - return; - } - if (time < mp6 && time % 1.0 > 0.5) { - solid(dimColor(Color.kFirstRed, 0.5)); - lights.setData(buffer); - return; - } - - switch (lightMode) { - case SOLID: - solid(color1); - break; - case WAVE: - wave(color1, color2, cycleLength, duration); - break; - case CLIMB: - climb(color1, color2, color1Length, color2Length, speed); - break; - case STROBE: - strobe(color1, color2, onLength, offLength); - break; - default: - solid(Color.kBlack); - } - - lights.setData(buffer); - } -} diff --git a/java/frc/robot/data/LoggingSystem.java b/java/frc/robot/data/LoggingSystem.java deleted file mode 100644 index 5c21203..0000000 --- a/java/frc/robot/data/LoggingSystem.java +++ /dev/null @@ -1,61 +0,0 @@ -// 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. - -package frc.robot.data; - -import java.util.Dictionary; -import java.util.Map; - -import com.ctre.phoenix6.SignalLogger; - -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.LoggingConstants; -import frc.robot.subsystems.DriveSystem; - -public class LoggingSystem extends SubsystemBase { - - private Map subsystemArray; - private boolean loggingState = LoggingConstants.DEFAULT_LOGGING_STATE; - private GenericEntry loggingChooser; - - - /** Creates a new LoggingSystem. */ - public LoggingSystem(Map subsystemArray) { - loggingChooser = Shuffleboard.getTab("Logging").add("Enable Logging", false).getEntry(); - SmartDashboard.putBoolean("Logging Chooser", false); - this.subsystemArray = subsystemArray; - } - - public boolean getLoggingState() { - return loggingState; - } - - private boolean getLoggingFlag() { - // An example command will be run in autonomous - return loggingChooser.getBoolean(false); // TODO: Actually use this value -} - - @Override - public void periodic() { - // This method will be called once per scheduler run - if (getLoggingFlag() != loggingState) { - loggingState = getLoggingFlag(); - - if (loggingState) { - SignalLogger.start(); - // Log the odometry pose as a double array - } else { - SignalLogger.stop(); - } - } - if (loggingState){ - var pose = ((DriveSystem)subsystemArray.get(new String("Drive"))).getState().Pose; - var status = SignalLogger.writeDoubleArray("odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); - } - } -} diff --git a/java/frc/robot/generated/TunerConstants.java b/java/frc/robot/generated/TunerConstants.java deleted file mode 100644 index cbbbb05..0000000 --- a/java/frc/robot/generated/TunerConstants.java +++ /dev/null @@ -1,286 +0,0 @@ -package frc.robot.generated; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.hardware.*; -import com.ctre.phoenix6.signals.*; -import com.ctre.phoenix6.swerve.*; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.*; - -import frc.robot.subsystems.DriveSystem; - -// Generated by the Tuner X Swerve Project Generator -// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html -public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) // 100, 0, 0.5 - .withKS(0.1).withKV(1.91).withKA(0) // 0, 1.5, 0 - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) // 3, 0, 0 - .withKS(0).withKV(0.124); // removed KA changed and KV from 0 - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); // 150 - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) - ); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); // Default Name - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); // 5 - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; // 3.5714285714285716 - - private static final double kDriveGearRatio = 7.363636363636365; // 6.122448979591837 - private static final double kSteerGearRatio = 15.42857142857143; // 12.8 - private static final Distance kWheelRadius = Inches.of(2.167); // 1.92 - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 13; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); // 0.00001 - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); // 0.001 - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); // 0.25 - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); // 0.25 - - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - - // Front Left - private static final int kFrontLeftDriveMotorId = 11; - private static final int kFrontLeftSteerMotorId = 12; - private static final int kFrontLeftEncoderId = 10; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); // -0.052734375 - private static final boolean kFrontLeftSteerMotorInverted = true; // false - private static final boolean kFrontLeftEncoderInverted = false; // new - - private static final Distance kFrontLeftXPos = Inches.of(10); // 9.25 - private static final Distance kFrontLeftYPos = Inches.of(10); // 9.25 - - // Front Right - private static final int kFrontRightDriveMotorId = 2; - private static final int kFrontRightSteerMotorId = 3; - private static final int kFrontRightEncoderId = 1; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); // -0.6796875 - private static final boolean kFrontRightSteerMotorInverted = true; // false - private static final boolean kFrontRightEncoderInverted = false; // new - - private static final Distance kFrontRightXPos = Inches.of(10); // 9.25 - private static final Distance kFrontRightYPos = Inches.of(-10); // -9.25 - - // Back Left - private static final int kBackLeftDriveMotorId = 8; - private static final int kBackLeftSteerMotorId = 9; - private static final int kBackLeftEncoderId = 7; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); // -0.304931640625 - private static final boolean kBackLeftSteerMotorInverted = true; // false - private static final boolean kBackLeftEncoderInverted = false; // new - - private static final Distance kBackLeftXPos = Inches.of(-10); // -9.25 - private static final Distance kBackLeftYPos = Inches.of(10); // 9.25 - - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 6; - private static final int kBackRightEncoderId = 4; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); // -0.032470703125 - private static final boolean kBackRightSteerMotorInverted = true; // false - private static final boolean kBackRightEncoderInverted = false; // new - - private static final Distance kBackRightXPos = Inches.of(-10); // -9.25 - private static final Distance kBackRightYPos = Inches.of(-10); // -9.25 - - - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted - ); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted - ); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted - ); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted - ); - - /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. - */ - public static DriveSystem createDrivetrain() { - return new DriveSystem( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ); - } - - - /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. - */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules - ); - } - } -} \ No newline at end of file diff --git a/java/frc/robot/subsystems/DriveSystem.java b/java/frc/robot/subsystems/DriveSystem.java deleted file mode 100644 index 3447342..0000000 --- a/java/frc/robot/subsystems/DriveSystem.java +++ /dev/null @@ -1,293 +0,0 @@ -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.*; - -import java.util.function.Supplier; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -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.generated.TunerConstants.TunerSwerveDrivetrain; - -/** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements - * Subsystem so it can easily be used in command-based projects. - */ -public class DriveSystem extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; - - /** Swerve request to apply during robot-centric path following */ - private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this - ) - ); - - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public DriveSystem( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public DriveSystem( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public DriveSystem( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - private void configureAutoBuilder() { - try { - var config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( - () -> getState().Pose, // Supplier of current robot pose - this::resetPose, // Consumer for seeding pose against auto - () -> getState().Speeds, // Supplier of current robot speeds - // Consumer of ChassisSpeeds and feedforwards to drive the robot - (speeds, feedforwards) -> setControl( - m_pathApplyRobotSpeeds.withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) - ), - new PPHolonomicDriveController( - // PID constants for translation - new PIDConstants(10, 0, 0), // 5, 0, 0 - // PID constants for rotation - new PIDConstants(7, 0, 0) // 5, 0, 0 - ), - config, - // Assume the path needs to be flipped for Red vs Blue, this is normally the case - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this // Subsystem for requirements - ); - } catch (Exception ex) { - DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); - } - } - - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - - @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 (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); - m_hasAppliedOperatorPerspective = true; - }); - } - } - - 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); - } -} \ No newline at end of file diff --git a/java/org/assabet/aztechs157/PrintLimiter.java b/java/org/assabet/aztechs157/PrintLimiter.java deleted file mode 100644 index aa6dc7a..0000000 --- a/java/org/assabet/aztechs157/PrintLimiter.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.assabet.aztechs157; - -public class PrintLimiter { - private final int ticksPerPrint; - private int currentTicks = 0; - - public PrintLimiter(final int ticksPerPrint) { - this.ticksPerPrint = ticksPerPrint; - } - - public PrintLimiter tick() { - currentTicks++; - - if (currentTicks > ticksPerPrint) { - currentTicks = 0; - } - - return this; - } - - public PrintLimiter println(final String message) { - if (currentTicks == 0) { - System.out.println(message); - } - - return this; - } - - public PrintLimiter print(final String message) { - if (currentTicks == 0) { - System.out.print(message); - } - - return this; - } - - public PrintLimiter limitBody(final Runnable body) { - if (currentTicks == 0) { - body.run(); - } - - return this; - } -} diff --git a/java/org/assabet/aztechs157/Sanity.java b/java/org/assabet/aztechs157/Sanity.java deleted file mode 100644 index a2ac436..0000000 --- a/java/org/assabet/aztechs157/Sanity.java +++ /dev/null @@ -1,91 +0,0 @@ -package org.assabet.aztechs157; - -import org.assabet.aztechs157.numbers.Range; - -public final class Sanity { - private Sanity() { - throw new UnsupportedOperationException("Expect is a utility class"); - } - - public static class SanityError extends RuntimeException { - public SanityError(final String message) { - super(message); - } - } - - public static NumberSanity check(final double number) { - return new NumberSanity(number); - } - - public static record NumberSanity(double value) { - public NumberSanity containedWithin(final Range range) { - if (range.contains(value)) { - return this; - } else { - throw new SanityError(value + " was not contained within " + range); - } - } - - public NumberSanity equalTo(final double other) { - if (value == other) { - return this; - } else { - throw new SanityError(value + " was not equal to " + other); - } - } - - public NumberSanity notEqualTo(final double other) { - if (value != other) { - return this; - } else { - throw new SanityError(value + " was equal to " + other); - } - } - - public NumberSanity greaterThan(final double other) { - if (value > other) { - return this; - } else { - throw new SanityError(value + " was not greater than " + other); - } - } - - public NumberSanity lessThan(final double other) { - if (value < other) { - return this; - } else { - throw new SanityError(value + " was not less than " + other); - } - } - - public NumberSanity greaterOrEqual(final double other) { - if (value >= other) { - return this; - } else { - throw new SanityError(value + " was not greater or equal to " + other); - } - } - - public NumberSanity lessOrEqual(final double other) { - if (value <= other) { - return this; - } else { - throw new SanityError(value + " was not less or equal to " + other); - } - } - } - - public static BooleanSanity check(final boolean value) { - return new BooleanSanity(value); - } - - public static record BooleanSanity(boolean value) { - public BooleanSanity equal(final boolean other) { - if (value == other) { - return this; - } else { - throw new SanityError(value + " was not equal to " + other); - } - } - } -} diff --git a/java/org/assabet/aztechs157/input/Model.java b/java/org/assabet/aztechs157/input/Model.java deleted file mode 100644 index daa0894..0000000 --- a/java/org/assabet/aztechs157/input/Model.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.assabet.aztechs157.input; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -/** - * Models map physical inputs on a input device to input classes such as - * {@link Button}, {@link Axis}, or {@link Pov}. - */ -public class Model { - - public final int deviceId; - - /** - * Create a Model that models the device specified by `deviceId` - * - * @param deviceId The id of the device - */ - public Model(final int deviceId) { - this.deviceId = deviceId; - } - - /** - * Create a {@link Button} that models a physical button - * - * @param buttonId The button to model - * @return The modeled {@link Button} - */ - public Button button(final int buttonId) { - return Button.fromDriverStation(deviceId, buttonId); - } - - /** - * Create a {@link Axis} that models a physical axis - * - * @param buttonId The axis to model - * @return The modeled {@link Axis} - */ - public Axis axis(final int axisId) { - return Axis.fromDriverStation(deviceId, axisId); - } - - /** - * Create a {@link Pov} that modes a physical pov - * - * @param povId The pov to model - * @return The modeled {@link Pov} - */ - public Pov pov(final int povId) { - return Pov.fromDriverStation(deviceId, povId); - } -} diff --git a/java/org/assabet/aztechs157/input/Pov.java b/java/org/assabet/aztechs157/input/Pov.java deleted file mode 100644 index 7dd2a8f..0000000 --- a/java/org/assabet/aztechs157/input/Pov.java +++ /dev/null @@ -1,67 +0,0 @@ -package org.assabet.aztechs157.input; - -import java.util.function.IntSupplier; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -import edu.wpi.first.wpilibj.DriverStation; - -/** - * Class for getting input from a pov. - */ -public class Pov { - private final IntSupplier degrees; - - public Pov(final IntSupplier degrees) { - this.degrees = degrees; - } - - public static Pov fromDriverStation(final int deviceId, final int povId) { - return new Pov(() -> DriverStation.getStickPOV(deviceId, povId)); - } - - public int get() { - return degrees.getAsInt(); - } - - public final Axis value = new Axis(() -> get()); - - public final Axis y = new Axis(() -> switch (get()) { - case UP_LEFT, UP, UP_RIGHT -> 1; - case LEFT, CENTER, RIGHT -> 0; - case DOWN_LEFT, DOWN, DOWN_RIGHT -> -1; - default -> 0; - }); - - public final Axis x = new Axis(() -> switch (get()) { - case DOWN_RIGHT, RIGHT, UP_RIGHT -> 1; - case DOWN, CENTER, UP -> 0; - case DOWN_LEFT, LEFT, UP_LEFT -> -1; - default -> 0; - }); - - public static final int CENTER = -1; - public static final int UP = 45 * 0; - public static final int UP_RIGHT = 45 * 1; - public static final int RIGHT = 45 * 2; - public static final int DOWN_RIGHT = 45 * 3; - public static final int DOWN = 45 * 4; - public static final int DOWN_LEFT = 45 * 5; - public static final int LEFT = 45 * 6; - public static final int UP_LEFT = 45 * 7; - - private Button buttonForValue(final int degrees, final String name) { - return new Button(() -> get() == degrees); - } - - public final Button center = buttonForValue(CENTER, "Center"); - public final Button up = buttonForValue(UP, "Up"); - public final Button upRight = buttonForValue(UP_RIGHT, "Up Right"); - public final Button right = buttonForValue(RIGHT, "Right"); - public final Button downRight = buttonForValue(DOWN_RIGHT, "Down Right"); - public final Button down = buttonForValue(DOWN, "Down"); - public final Button downLeft = buttonForValue(DOWN_LEFT, "Down Left"); - public final Button left = buttonForValue(LEFT, "Left"); - public final Button upLeft = buttonForValue(UP_LEFT, "Up Left"); -} diff --git a/java/org/assabet/aztechs157/input/layouts/DynamicLayout.java b/java/org/assabet/aztechs157/input/layouts/DynamicLayout.java deleted file mode 100644 index c4cbcf7..0000000 --- a/java/org/assabet/aztechs157/input/layouts/DynamicLayout.java +++ /dev/null @@ -1,43 +0,0 @@ -package org.assabet.aztechs157.input.layouts; - -import java.util.function.Supplier; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -/** - * Object that manages layouts. A layout can be selected from Shuffleboard that - * can then be used by the robot. It maps the inputs of a - * {@link DynamicLayout} to the desired functions of the robot. - */ -public class DynamicLayout implements Layout { - private final Supplier layoutSupplier; - - public DynamicLayout(final Supplier layoutSupplier) { - this.layoutSupplier = layoutSupplier; - } - - public Layout getCurrent() { - return layoutSupplier.get(); - } - - /** - * Get a button from the currently selected layout. - * - * @param key Which button to retrieve - * @return A {@link Button} and {@link Button.Key} representing the input - */ - public Button button(final Button.Key key) { - return new Button(() -> getCurrent().button(key).get()); - } - - /** - * Get a axis from the currently selected layout. - * - * @param key Which axis to retrieve - * @return A {@link Axis} representing the input - */ - public Axis axis(final Axis.Key key) { - return new Axis(() -> getCurrent().axis(key).get()); - } -} diff --git a/java/org/assabet/aztechs157/input/layouts/Layout.java b/java/org/assabet/aztechs157/input/layouts/Layout.java deleted file mode 100644 index 01c914c..0000000 --- a/java/org/assabet/aztechs157/input/layouts/Layout.java +++ /dev/null @@ -1,22 +0,0 @@ -package org.assabet.aztechs157.input.layouts; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public interface Layout { - /** - * Retrieve the {@link Button} associated with a {@link Button.Key} - * - * @param key The key a button was assigned to - * @return The associated button - */ - public Button button(final Button.Key key); - - /** - * Retrieve the {@link Axis} associated with a {@link Axis.KeyBase} - * - * @param key The key an axis was assigned to - * @return The associated axis - */ - public Axis axis(final Axis.Key key); -} diff --git a/java/org/assabet/aztechs157/input/layouts/MapLayout.java b/java/org/assabet/aztechs157/input/layouts/MapLayout.java deleted file mode 100644 index fe4c8b0..0000000 --- a/java/org/assabet/aztechs157/input/layouts/MapLayout.java +++ /dev/null @@ -1,62 +0,0 @@ -package org.assabet.aztechs157.input.layouts; - -import java.util.HashMap; -import java.util.Map; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -/** - * A simple structure that stores the mapping between keys and inputs. These can - * be used with {@link DynamicLayout} to allow hot-swapping of layouts. - */ -public class MapLayout implements Layout { - private final Map buttons = new HashMap<>(); - private final Map axes = new HashMap<>(); - - /** - * For this Layout, assign a {@link Button.Key} to a {@link Button}. - * Calling - * this method multiple times with the same key will override the previous - * assignment. - * - * @param key The key to assign with - * @param button The button being assigned - */ - public void assign(final Button.Key key, final Button button) { - buttons.put(key, button); - } - - /** - * For this Layout, assign a {@link Axis.KeyBase} to a {@link Axis}. Calling - * this - * method multiple times with the same key will override the previous - * assignment. - * - * @param key The key to assign with - * @param axis The axis being assigned - */ - public void assign(final Axis.Key key, final Axis axis) { - axes.put(key, axis); - } - - /** - * Retrieve the {@link Button} associated with a {@link Button.Key} - * - * @param key The key a button was assigned to - * @return The associated button - */ - public Button button(final Button.Key key) { - return buttons.get(key); - } - - /** - * Retrieve the {@link Axis} associated with a {@link Axis.KeyBase} - * - * @param key The key an axis was assigned to - * @return The associated axis - */ - public Axis axis(final Axis.Key key) { - return axes.get(key); - } -} diff --git a/java/org/assabet/aztechs157/input/models/LogitechAttack.java b/java/org/assabet/aztechs157/input/models/LogitechAttack.java deleted file mode 100644 index c097cc6..0000000 --- a/java/org/assabet/aztechs157/input/models/LogitechAttack.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.assabet.aztechs157.input.models; - -import org.assabet.aztechs157.input.Model; -import org.assabet.aztechs157.input.Pov; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public class LogitechAttack extends Model { - - public LogitechAttack(final int joystickId) { - super(joystickId); - } - - public final Button trigger = button(1); - public final Button button2 = button(2); - public final Button button3 = button(3); - public final Button button4 = button(4); - public final Button button5 = button(5); - public final Button button6 = button(6); - public final Button button7 = button(7); - public final Button button8 = button(8); - public final Button button9 = button(9); - public final Button button10 = button(10); - public final Button button11 = button(11); - - public final Axis stickX = axis(0); - public final Axis stickY = axis(1); - public final Axis slider = axis(3); - - public final Pov pov = pov(0); -} diff --git a/java/org/assabet/aztechs157/input/models/LogitechExtreme3D.java b/java/org/assabet/aztechs157/input/models/LogitechExtreme3D.java deleted file mode 100644 index 59a51e3..0000000 --- a/java/org/assabet/aztechs157/input/models/LogitechExtreme3D.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.assabet.aztechs157.input.models; - -import org.assabet.aztechs157.input.Model; -import org.assabet.aztechs157.input.Pov; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public class LogitechExtreme3D extends Model { - public LogitechExtreme3D(final int joystickId) { - super(joystickId); - } - - public final Button trigger = button(1); - public final Button thumb = button(2); - public final Button button3 = button(3); - public final Button button4 = button(4); - public final Button button5 = button(5); - public final Button button6 = button(6); - public final Button button7 = button(7); - public final Button button8 = button(8); - public final Button button9 = button(9); - public final Button button10 = button(10); - public final Button button11 = button(11); - public final Button button12 = button(12); - - public final Axis stickX = axis(0); - public final Axis stickY = axis(1); - public final Axis stickRotate = axis(2); - public final Axis slider = axis(3); - - public final Pov pov = pov(0); -} diff --git a/java/org/assabet/aztechs157/input/models/LogitechGamepadF310.java b/java/org/assabet/aztechs157/input/models/LogitechGamepadF310.java deleted file mode 100644 index 3be4af0..0000000 --- a/java/org/assabet/aztechs157/input/models/LogitechGamepadF310.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.assabet.aztechs157.input.models; - -import org.assabet.aztechs157.input.Model; -import org.assabet.aztechs157.input.Pov; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public class LogitechGamepadF310 extends Model { - - public LogitechGamepadF310(final int joystickId) { - super(joystickId); - } - - public final Button a = button(1); - public final Button b = button(2); - public final Button x = button(3); - public final Button y = button(4); - public final Button leftBumper = button(5); - public final Button rightBumper = button(6); - public final Button back = button(7); - public final Button start = button(8); - public final Button leftStickPress = button(9); - public final Button rightStickPress = button(10); - - public final Axis leftStickX = axis(0); - public final Axis leftStickY = axis(1); - public final Axis rightTriggerHeld = axis(2); - public final Axis leftTriggerHeld = axis(3); - public final Axis rightStickX = axis(4); - public final Axis rightStickY = axis(5); - public final Axis combinedTriggersHeld = rightTriggerHeld.offsetBy(leftTriggerHeld.inverted()::get); - - public final Pov pov = pov(0); -} diff --git a/java/org/assabet/aztechs157/input/models/XboxOne.java b/java/org/assabet/aztechs157/input/models/XboxOne.java deleted file mode 100644 index 0bc79fe..0000000 --- a/java/org/assabet/aztechs157/input/models/XboxOne.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.assabet.aztechs157.input.models; - -import org.assabet.aztechs157.input.Model; -import org.assabet.aztechs157.input.Pov; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public class XboxOne extends Model { - - public XboxOne(final int joystickId) { - super(joystickId); - } - - public final Button a = button(1); - public final Button b = button(2); - public final Button x = button(3); - public final Button y = button(4); - public final Button leftBumper = button(5); - public final Button rightBumper = button(6); - public final Button back = button(7); - public final Button start = button(8); - public final Button leftStickPress = button(9); - public final Button rightStickPress = button(10); - - public final Axis leftStickX = axis(0); - public final Axis leftStickY = axis(1); - public final Axis leftTriggerHeld = axis(2); - public final Axis rightTriggerHeld = axis(3); - public final Axis rightStickX = axis(4); - public final Axis rightStickY = axis(5); - public final Axis combinedTriggersHeld = rightTriggerHeld.offsetBy(leftTriggerHeld.inverted()::get); - - public final Pov pov = pov(0); -} diff --git a/java/org/assabet/aztechs157/input/values/Axis.java b/java/org/assabet/aztechs157/input/values/Axis.java deleted file mode 100644 index 892cbfd..0000000 --- a/java/org/assabet/aztechs157/input/values/Axis.java +++ /dev/null @@ -1,97 +0,0 @@ -package org.assabet.aztechs157.input.values; - -import java.util.function.DoubleConsumer; -import java.util.function.DoubleSupplier; -import java.util.function.DoubleUnaryOperator; - -import org.assabet.aztechs157.numbers.Range; - -import edu.wpi.first.wpilibj.DriverStation; - -/** - * Class for getting input from a axis. This class has methods and static - * methods to modify and compose {@link Axis}s into a new - * {@link Axis}. - */ -public class Axis { - public static class Key { - } - - public static final Range kDeviceDefaultRange = new Range(-1, 1); - - private final DoubleSupplier value; - - public Axis(final DoubleSupplier value) { - this.value = value; - } - - public static Axis fromDriverStation(final int deviceId, final int axisId) { - return new Axis(() -> DriverStation.getStickAxis(deviceId, axisId)); - } - - public static Axis always(final double value) { - return new Axis(() -> value); - } - - public double get() { - return value.getAsDouble(); - } - - public Axis map(final DoubleUnaryOperator body) { - return new Axis(() -> body.applyAsDouble(get())); - } - - public Axis tap(final DoubleConsumer body) { - return map(value -> { - body.accept(value); - return value; - }); - } - - /** - * Inverts the input by negating the number's sign - * - * @return A new inverted input - */ - public Axis inverted() { - return map(value -> -value); - } - - /** - * Scale the input with a scalar value. - * - * @param scale The value to scale by - * @return A new input with the scale applied - */ - public Axis scaledBy(final double scale) { - return map(value -> value * scale); - } - - /** - * Scale the input with another input. - * - * @param scale The input to retrieve the scale from - * @return A new input with the scale applied - */ - public Axis scaledBy(final DoubleSupplier scale) { - return map(value -> value * scale.getAsDouble()); - } - - public Axis offsetBy(final double offset) { - return map(value -> value + offset); - } - - public Axis offsetBy(final DoubleSupplier offset) { - return map(value -> value + offset.getAsDouble()); - } - - /** - * Clamp the input to a number within the provided range. - * - * @param range The range to clamp to - * @return A new input with clamp applied - */ - public Axis clampTo(final Range range) { - return map(value -> range.clamp(value)); - } -} diff --git a/java/org/assabet/aztechs157/input/values/Button.java b/java/org/assabet/aztechs157/input/values/Button.java deleted file mode 100644 index ebd6a2d..0000000 --- a/java/org/assabet/aztechs157/input/values/Button.java +++ /dev/null @@ -1,131 +0,0 @@ -package org.assabet.aztechs157.input.values; - -import java.util.function.BooleanSupplier; -import java.util.function.UnaryOperator; - -import edu.wpi.first.util.function.BooleanConsumer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -/** - * Class for getting input from a button. This class has methods and static - * methods to modify and compose {@link Button}s into a new - * {@link Button}. - */ -public class Button { - public static class Key { - } - - private final BooleanSupplier value; - - public Button(final BooleanSupplier value) { - this.value = value; - } - - public static Button fromDriverStation(final int deviceId, final int buttonId) { - return new Button(() -> DriverStation.getStickButton(deviceId, buttonId)); - } - - public static Button always(final boolean value) { - return new Button(() -> value); - } - - public boolean get() { - return value.getAsBoolean(); - } - - public Button whenPressed(final Command command) { - new Trigger(value).onTrue(command); - return this; - } - - public Button whileHeld(final Command command) { - new Trigger(value).whileTrue(command); - return this; - } - - public Button toggleWhenPressed(final Command command) { - new Trigger(value).toggleOnTrue(command); - return this; - } - - public Button map(final UnaryOperator body) { - return new Button(() -> body.apply(get())); - } - - public Button tap(final BooleanConsumer body) { - return map(value -> { - body.accept(value); - return value; - }); - } - - /** - * Inverts the input; similar to a boolean `!` - * - * @return A new inverted input - */ - public Button inverted() { - return map(value -> !value); - } - - /** - * Checks that all inputs are true; similar to a boolean `&&` - * - * @param first The first input - * @param rest The rest of the inputs - * @return A new input that is only true when all of the passed inputs are true - */ - public static Button all(final Button first, final Button... rest) { - // The first argument is explicit to prevent being given empty arrays - - return new Button(() -> { - // Check each input individually - // As soon as one input is false, return false - - if (first != null && first.get() == false) { - return false; - } - - for (final var input : rest) { - if (input != null && input.get() == false) { - return false; - } - } - - // All inputs are true at this point, so return true - return true; - }); - } - - /** - * Checks that any input is true; similar to a boolean `||` - * - * @param first The first input - * @param rest The rest of the inputs - * @return A new input that is true when any of the passed inputs are true - */ - public static Button any(final Button first, final Button... rest) { - // The first argument is explicit to prevent being given empty arrays - - return new Button(() -> { - // Check each input individually - // As soon as one input is true, return true - - if (first != null && first.get()) { - return true; - } - - for (final var input : rest) { - if (input != null && input.get()) { - return true; - } - } - - // All inputs are false at this point, so return false - return false; - }); - - } -} diff --git a/java/org/assabet/aztechs157/numbers/Deadzone.java b/java/org/assabet/aztechs157/numbers/Deadzone.java deleted file mode 100644 index 62a1578..0000000 --- a/java/org/assabet/aztechs157/numbers/Deadzone.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.assabet.aztechs157.numbers; - -import org.assabet.aztechs157.input.values.Axis; - -public class Deadzone { - public final Range deadzone; - public final Range full; - public final RangeConverter leftConverter; - public final RangeConverter rightConverter; - - public static Deadzone forAxis(final Range deadzone) { - return new Deadzone(deadzone, Axis.kDeviceDefaultRange, 0); - } - - public Deadzone(final Range deadzone, final Range full, final double fullCenter) { - this.deadzone = deadzone; - this.full = full; - - final var leftFull = new Range(full.start(), fullCenter); - final var leftDeadzone = new Range(full.start(), deadzone.start()); - this.leftConverter = new RangeConverter(leftDeadzone, leftFull); - - final var rightFull = new Range(fullCenter, full.end()); - final var rightDeadzone = new Range(deadzone.end(), full.end()); - this.rightConverter = new RangeConverter(rightDeadzone, rightFull); - } - - public double apply(final double input) { - if (deadzone.contains(input)) { - return 0; - } else if (leftConverter.inputRange.contains(input)) { - return leftConverter.convert(input); - } else if (rightConverter.inputRange.contains(input)) { - return rightConverter.convert(input); - } - - throw new Error("Attempted to apply deadzone to input outside of full range " - + full.start() + " to " + full.end()); - } -} diff --git a/java/org/assabet/aztechs157/numbers/Range.java b/java/org/assabet/aztechs157/numbers/Range.java deleted file mode 100644 index a69bb32..0000000 --- a/java/org/assabet/aztechs157/numbers/Range.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.assabet.aztechs157.numbers; - -public record Range(double start, double end) { - - public boolean contains(final double value) { - return start <= value && value <= end; - } - - public double range() { - return end - start; - } - - public double clamp(final double value) { - if (value < start) { - return start; - } else if (value > end) { - return end; - } else { - return value; - } - } - - public double limitMotionWithinRange(final double speed, final double currentPosition) { - if (speed > 0 && currentPosition > end) { - return 0; - } else if (speed < 0 && currentPosition < start) { - return 0; - } else { - return speed; - } - } - - public RangeConverter convertingTo(final Range output) { - return new RangeConverter(this, output); - } -} diff --git a/java/org/assabet/aztechs157/numbers/RangeConverter.java b/java/org/assabet/aztechs157/numbers/RangeConverter.java deleted file mode 100644 index edb424e..0000000 --- a/java/org/assabet/aztechs157/numbers/RangeConverter.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.assabet.aztechs157.numbers; - -public class RangeConverter { - public final Range inputRange; - public final Range outputRange; - public final double scaleFactor; - - public RangeConverter(final Range inputRange, final Range outputRange) { - this.inputRange = inputRange; - this.outputRange = outputRange; - this.scaleFactor = outputRange.range() / inputRange.range(); - } - - public double convert(final double inputValue) { - // Shift to zero based input range - final var basedInput = inputValue - inputRange.start(); - - // Scale the zero based input - final var scaled = basedInput * scaleFactor; - - // Shift from zero based to output range - final var outputValue = scaled + outputRange.start(); - - return outputValue; - } -} diff --git a/main/deploy/example.txt b/main/deploy/example.txt deleted file mode 100644 index bb82515..0000000 --- a/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function -to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/main/deploy/music/BuddyHollyRiff.chrp b/main/deploy/music/BuddyHollyRiff.chrp deleted file mode 100644 index 9489fdd..0000000 Binary files a/main/deploy/music/BuddyHollyRiff.chrp and /dev/null differ diff --git a/main/deploy/music/MoreCowbell.chrp b/main/deploy/music/MoreCowbell.chrp deleted file mode 100644 index e5baa9f..0000000 Binary files a/main/deploy/music/MoreCowbell.chrp and /dev/null differ diff --git a/main/deploy/music/TOTTFIY.chrp b/main/deploy/music/TOTTFIY.chrp deleted file mode 100644 index 52411f1..0000000 Binary files a/main/deploy/music/TOTTFIY.chrp and /dev/null differ diff --git a/main/deploy/music/WTTBPchrp.chrp b/main/deploy/music/WTTBPchrp.chrp deleted file mode 100644 index 0b048cd..0000000 Binary files a/main/deploy/music/WTTBPchrp.chrp and /dev/null differ diff --git a/main/deploy/music/YourLoveOF.chrp b/main/deploy/music/YourLoveOF.chrp deleted file mode 100644 index dda3a25..0000000 Binary files a/main/deploy/music/YourLoveOF.chrp and /dev/null differ diff --git a/main/deploy/music/adams.chrp b/main/deploy/music/adams.chrp deleted file mode 100644 index f5be11b..0000000 Binary files a/main/deploy/music/adams.chrp and /dev/null differ diff --git a/main/deploy/music/bluetoothdeviceisreadytopair.chrp b/main/deploy/music/bluetoothdeviceisreadytopair.chrp deleted file mode 100644 index 6bb6ad6..0000000 Binary files a/main/deploy/music/bluetoothdeviceisreadytopair.chrp and /dev/null differ diff --git a/main/deploy/music/e1m1.chrp b/main/deploy/music/e1m1.chrp deleted file mode 100644 index 08f2fe6..0000000 Binary files a/main/deploy/music/e1m1.chrp and /dev/null differ diff --git a/main/deploy/music/entry.chrp b/main/deploy/music/entry.chrp deleted file mode 100644 index b384863..0000000 Binary files a/main/deploy/music/entry.chrp and /dev/null differ diff --git a/main/deploy/music/exorcist.chrp b/main/deploy/music/exorcist.chrp deleted file mode 100644 index b8d1d6f..0000000 Binary files a/main/deploy/music/exorcist.chrp and /dev/null differ diff --git a/main/deploy/music/output.chrp b/main/deploy/music/output.chrp deleted file mode 100644 index ca4e227..0000000 Binary files a/main/deploy/music/output.chrp and /dev/null differ diff --git a/main/deploy/music/sundog.chrp b/main/deploy/music/sundog.chrp deleted file mode 100644 index 0c76bb1..0000000 Binary files a/main/deploy/music/sundog.chrp and /dev/null differ diff --git a/main/deploy/music/super_mario.chrp b/main/deploy/music/super_mario.chrp deleted file mode 100644 index 16f8649..0000000 Binary files a/main/deploy/music/super_mario.chrp and /dev/null differ diff --git a/main/deploy/music/tetris.chrp b/main/deploy/music/tetris.chrp deleted file mode 100644 index 88363c3..0000000 Binary files a/main/deploy/music/tetris.chrp and /dev/null differ diff --git a/main/deploy/pathplanner/autos/1m backward auto.auto b/main/deploy/pathplanner/autos/1m backward auto.auto deleted file mode 100644 index e988db7..0000000 --- a/main/deploy/pathplanner/autos/1m backward auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m backward" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/main/deploy/pathplanner/autos/1m forward & backwards x25.auto b/main/deploy/pathplanner/autos/1m forward & backwards x25.auto deleted file mode 100644 index e2f253c..0000000 --- a/main/deploy/pathplanner/autos/1m forward & backwards x25.auto +++ /dev/null @@ -1,169 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 2.0, - "y": 7.0 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/main/deploy/pathplanner/autos/1m forward & backwards.auto b/main/deploy/pathplanner/autos/1m forward & backwards.auto deleted file mode 100644 index 79adb64..0000000 --- a/main/deploy/pathplanner/autos/1m forward & backwards.auto +++ /dev/null @@ -1,302 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/main/deploy/pathplanner/autos/1m forward auto.auto b/main/deploy/pathplanner/autos/1m forward auto.auto deleted file mode 100644 index de1be04..0000000 --- a/main/deploy/pathplanner/autos/1m forward auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m forward" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/main/deploy/pathplanner/autos/1m left auto.auto b/main/deploy/pathplanner/autos/1m left auto.auto deleted file mode 100644 index 286b393..0000000 --- a/main/deploy/pathplanner/autos/1m left auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m left" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/main/deploy/pathplanner/autos/1m right auto.auto b/main/deploy/pathplanner/autos/1m right auto.auto deleted file mode 100644 index 69e6f18..0000000 --- a/main/deploy/pathplanner/autos/1m right auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m right" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/main/deploy/pathplanner/navgrid.json b/main/deploy/pathplanner/navgrid.json deleted file mode 100644 index bab0da9..0000000 --- a/main/deploy/pathplanner/navgrid.json +++ /dev/null @@ -1 +0,0 @@ -{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/main/deploy/pathplanner/paths/1m Back and forth.path b/main/deploy/pathplanner/paths/1m Back and forth.path deleted file mode 100644 index add093c..0000000 --- a/main/deploy/pathplanner/paths/1m Back and forth.path +++ /dev/null @@ -1,103 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.1, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0, - "y": 7.0 - }, - "prevControl": { - "x": 2.9, - "y": 7.0 - }, - "nextControl": { - "x": 3.1, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": { - "x": 1.9, - "y": 7.0 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ - { - "name": "Wait", - "waypointRelativePos": 1.0, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - } - ] - } - } - }, - { - "name": "Wait 2", - "waypointRelativePos": 0, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/main/deploy/pathplanner/paths/1m backward.path b/main/deploy/pathplanner/paths/1m backward.path deleted file mode 100644 index fa735aa..0000000 --- a/main/deploy/pathplanner/paths/1m backward.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 5.59 - }, - "prevControl": null, - "nextControl": { - "x": 2.1, - "y": 5.59 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0, - "y": 5.59 - }, - "prevControl": { - "x": 1.1, - "y": 5.59 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/main/deploy/pathplanner/paths/1m forward.path b/main/deploy/pathplanner/paths/1m forward.path deleted file mode 100644 index 544f728..0000000 --- a/main/deploy/pathplanner/paths/1m forward.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.0, - "y": 5.589805823486292 - }, - "prevControl": null, - "nextControl": { - "x": 1.9999999999999967, - "y": 5.589805823486292 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.0, - "y": 5.589805823486292 - }, - "prevControl": { - "x": 1.0, - "y": 5.589805823486292 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/main/deploy/pathplanner/paths/1m left.path b/main/deploy/pathplanner/paths/1m left.path deleted file mode 100644 index 1482a76..0000000 --- a/main/deploy/pathplanner/paths/1m left.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.0, - "y": 5.59 - }, - "prevControl": null, - "nextControl": { - "x": 1.0, - "y": 6.502346398657132 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0, - "y": 6.59 - }, - "prevControl": { - "x": 0.9999999999999999, - "y": 5.59 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/main/deploy/pathplanner/paths/1m right.path b/main/deploy/pathplanner/paths/1m right.path deleted file mode 100644 index 88a7b65..0000000 --- a/main/deploy/pathplanner/paths/1m right.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.0, - "y": 5.59 - }, - "prevControl": null, - "nextControl": { - "x": 1.0, - "y": 5.49 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0, - "y": 4.59 - }, - "prevControl": { - "x": 1.0, - "y": 4.54 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/main/deploy/pathplanner/paths/test.path b/main/deploy/pathplanner/paths/test.path deleted file mode 100644 index 16dc425..0000000 --- a/main/deploy/pathplanner/paths/test.path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.200391204201211, - "y": 6.572114796651245 - }, - "prevControl": { - "x": 5.200391204201211, - "y": 6.572114796651245 - }, - "nextControl": { - "x": 7.200391204201211, - "y": 6.572114796651245 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.486748192869604, - "y": 4.654273468091096 - }, - "prevControl": { - "x": 7.694394331808605, - "y": 5.256522421876753 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/main/java/frc/robot/Constants.java b/main/java/frc/robot/Constants.java deleted file mode 100644 index e452735..0000000 --- a/main/java/frc/robot/Constants.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot; - -public class Constants { - public static class ControllerConstants { - - public static final int DRIVER_CONTROLLER_PORT = 0; - public static final int OPERATOR_CONTROLLER_PORT = 1; - - // Joystick Deadband - public static final double LEFT_X_DEADBAND = 0.1; - public static final double LEFT_Y_DEADBAND = 0.1; - public static final double RIGHT_X_DEADBAND = 0.1; - } - - public static class LoggingConstants { - - public static final boolean DEFAULT_LOGGING_STATE = false; - } - - - public static class CosmeticConstants { - - public static final int LIGHT_ID = 9; - public static final double SOLID_YELLOW_VALUE = 0.69; - public static final double SOLID_PURPLE_VALUE = 0.91; - public static final int LIGHT_LENGTH = 76; - - } - -} diff --git a/main/java/frc/robot/Inputs.java b/main/java/frc/robot/Inputs.java deleted file mode 100644 index 26d5bfa..0000000 --- a/main/java/frc/robot/Inputs.java +++ /dev/null @@ -1,98 +0,0 @@ -// 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. - -package frc.robot; - -import org.assabet.aztechs157.input.layouts.DynamicLayout; -import org.assabet.aztechs157.input.layouts.Layout; -import org.assabet.aztechs157.input.layouts.MapLayout; -import java.util.function.DoubleSupplier; -import org.assabet.aztechs157.input.models.XboxOne; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; -import org.assabet.aztechs157.numbers.Deadzone; -import org.assabet.aztechs157.numbers.Range; - -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import frc.robot.Constants.ControllerConstants; - -/** Add your docs here. */ -public class Inputs extends DynamicLayout { - - public static final Axis.Key precisionDrive = new Axis.Key(); - - ////////////////////////////////////////////////////////// - // HERE'S AN EXAMPLE OF USING ADDING NEW BUTTONS/AXIS - ///////////////////////////////////////////////////////// - - // public static final Button.Key resetGyro = new Button.Key(); - - // public static final Button.Key driveToSpeaker = new Button.Key(); - // public static final Button.Key driveToAmp = new Button.Key(); - // public static final Button.Key autoIntake = new Button.Key(); - - // public static final Button.Key intake = new Button.Key(); - // public static final Button.Key loadNote = new Button.Key(); - // public static final Button.Key eject = new Button.Key(); - - // public static final Button.Key highShotSpinUp = new Button.Key(); - // public static final Button.Key lowShotSpinUp = new Button.Key(); - - // public static final Button.Key highShot = new Button.Key(); - // public static final Button.Key lowShot = new Button.Key(); - // public static final Button.Key pass = new Button.Key(); - - // public static final Button.Key liftHanger = new Button.Key(); - // public static final Button.Key retractHanger = new Button.Key(); - // public static final Button.Key retractHangerPin = new Button.Key(); - // public static final Button.Key extendHangerPin = new Button.Key(); - - public static Inputs createFromChooser() { - final SendableChooser chooser = new SendableChooser<>(); - chooser.setDefaultOption("xbox", doubleXBOXLayout()); - Shuffleboard.getTab("Driver").add("Layout Choose", chooser); - - return new Inputs(chooser); - } - - private Inputs(final SendableChooser chooser) { - super(chooser::getSelected); - } - - private static Layout doubleXBOXLayout() { - - final var layout = new MapLayout(); - final var driver = new XboxOne(ControllerConstants.DRIVER_CONTROLLER_PORT); - final var operator = new XboxOne(ControllerConstants.OPERATOR_CONTROLLER_PORT); - - layout.assign(precisionDrive, driver.leftTriggerHeld.map(Deadzone.forAxis(new Range(0.0, 0.05))::apply).scaledBy(0.7)); - - ////////////////////////////////////////////////////////// - // HERE'S AN EXAMPLE OF USING CONTROLER LAYOUT OPTIONS - ///////////////////////////////////////////////////////// - - // layout.assign(driveToSpeaker, operator.a); - // layout.assign(driveToAmp, operator.b); - // layout.assign(resetGyro, driver.start); - // layout.assign(autoIntake, operator.leftBumper); - - // layout.assign(intake, driver.leftBumper); - // layout.assign(loadNote, operator.x); - - // layout.assign(highShotSpinUp, operator.rightBumper); - // layout.assign(lowShotSpinUp, operator.leftBumper); - - // layout.assign(highShot, new Button(() -> driver.rightTriggerHeld.get() > 0.2)); - // layout.assign(lowShot, driver.rightBumper); - // layout.assign(pass, operator.a); - // layout.assign(eject, operator.b); - - // layout.assign(liftHanger, operator.pov.up); - // layout.assign(retractHanger, operator.pov.down); - - return layout; - } - -} diff --git a/main/java/frc/robot/Main.java b/main/java/frc/robot/Main.java deleted file mode 100644 index fe215d7..0000000 --- a/main/java/frc/robot/Main.java +++ /dev/null @@ -1,15 +0,0 @@ -// 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. - -package frc.robot; - -import edu.wpi.first.wpilibj.RobotBase; - -public final class Main { - private Main() {} - - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} diff --git a/main/java/frc/robot/Robot.java b/main/java/frc/robot/Robot.java deleted file mode 100644 index 0556048..0000000 --- a/main/java/frc/robot/Robot.java +++ /dev/null @@ -1,76 +0,0 @@ -// 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. - -package frc.robot; - -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; - -public class Robot extends TimedRobot { - private Command m_autonomousCommand; - - private RobotContainer m_robotContainer; - - @Override - public void robotInit() { - m_robotContainer = new RobotContainer(); - } - - @Override - public void robotPeriodic() { - CommandScheduler.getInstance().run(); - } - - @Override - public void disabledInit() {} - - @Override - public void disabledPeriodic() {} - - @Override - public void disabledExit() {} - - @Override - public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - } - - @Override - public void autonomousPeriodic() {} - - @Override - public void autonomousExit() {} - - @Override - public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } - } - - @Override - public void teleopPeriodic() {} - - @Override - public void teleopExit() {} - - @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); - } - - @Override - public void testPeriodic() {} - - @Override - public void testExit() {} - - @Override - public void simulationPeriodic() {} -} diff --git a/main/java/frc/robot/RobotContainer.java b/main/java/frc/robot/RobotContainer.java deleted file mode 100644 index 5718dfb..0000000 --- a/main/java/frc/robot/RobotContainer.java +++ /dev/null @@ -1,130 +0,0 @@ -// 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. - -package frc.robot; - -import java.util.HashMap; -import java.util.Map; - -import org.assabet.aztechs157.input.layouts.Layout; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.Orchestra; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import com.pathplanner.lib.auto.AutoBuilder; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; - -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.DriveSystem; -import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.data.LoggingSystem; - -public class RobotContainer { - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - private Map SystemMap = new HashMap(); - - private final CommandXboxController joystick = new CommandXboxController(0); // My joystick - public final DriveSystem drivetrain = TunerConstants.createDrivetrain(); // My drivetrain - - {SystemMap.put("Drive",drivetrain);} - private final Layout inputs = Inputs.createFromChooser(); - private final LoggingSystem loggingSystem = new LoggingSystem(SystemMap); - private final SendableChooser autoChooser; - -/* Setting up bindings for necessary control of the swerve drive platform */ -private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() -.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband -.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors -private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); -private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); -private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric() -.withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - private final Telemetry logger = new Telemetry(MaxSpeed); - - // Slew Rate Limiters to limit acceleration of joystick inputs - // private final SlewRateLimiter xLimiter = new SlewRateLimiter(25); - // private final SlewRateLimiter yLimiter = new SlewRateLimiter(25); - // private final SlewRateLimiter rotLimiter = new SlewRateLimiter(1570); - - private Orchestra soundSystem = new Orchestra(); - - public RobotContainer() { - configureBindings(); - - autoChooser = AutoBuilder.buildAutoChooser("NothingAuto"); - SmartDashboard.putData("Auto Chooser", autoChooser); - for(int i = 0; i<4;i++){ - soundSystem.addInstrument(drivetrain.getModule(i).getDriveMotor(), 0); - soundSystem.addInstrument(drivetrain.getModule(i).getSteerMotor(), 1); - } - soundSystem.loadMusic("music/e1m1.chrp"); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // An example command will be run in autonomous - return autoChooser.getSelected(); - } - - public double modifySpeed(final double speed) { - final var modifier = 1 - inputs.axis(Inputs.precisionDrive).get(); - return speed * modifier; -} - - private void configureBindings() { - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain.applyRequest(() -> drive.withVelocityX(-joystick.getLeftY() /* TODO: check inversion */ * modifySpeed(MaxSpeed)) // Drive forward with - // negative Y (forward) - .withVelocityY(-joystick.getLeftX() /* TODO: check inversion */ * modifySpeed(MaxSpeed)) // Drive left with negative X (left) - .withRotationalRate(-joystick.getRightX() /* TODO: check inversion */ * MaxAngularRate) // Drive counterclockwise with negative X (left) - )); - - joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); - joystick.b().whileTrue(drivetrain.applyRequest(() -> - point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())) - )); - - joystick.pov(0).whileTrue(drivetrain.applyRequest(() -> - forwardStraight.withVelocityX(0.5).withVelocityY(0)) - ); - joystick.pov(180).whileTrue(drivetrain.applyRequest(() -> - forwardStraight.withVelocityX(-0.5).withVelocityY(0)) - ); - - // Run SysId routines when holding back/start and X/Y. - // Note that each routine should be run exactly once in a single log. - joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); - joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); - - // reset the field-centric heading on left bumper press - joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - - joystick.start().onTrue(drivetrain.runOnce(()-> {soundSystem.play();})); - joystick.back().onTrue(drivetrain.runOnce(()->{soundSystem.pause();})); - joystick.y().onTrue(drivetrain.runOnce(()->{soundSystem.stop();})); - - drivetrain.registerTelemetry(logger::telemeterize); - } - -} diff --git a/main/java/frc/robot/cosmetics/BlinkInLEDs.java b/main/java/frc/robot/cosmetics/BlinkInLEDs.java deleted file mode 100644 index f2241f0..0000000 --- a/main/java/frc/robot/cosmetics/BlinkInLEDs.java +++ /dev/null @@ -1,35 +0,0 @@ -// 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. - -package frc.robot.cosmetics; - -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.CosmeticConstants; - -// We are no longer using this file to run any code, it is merely left as a reference -public class BlinkInLEDs extends SubsystemBase { - public final Spark lightController = new Spark(CosmeticConstants.LIGHT_ID); - // private ShuffleboardTab tab = Shuffleboard.getTab("LED"); - // private GenericEntry lightColor = tab.add("led color", - // CosmeticConstants.SOLID_YELLOW_VALUE).getEntry(); - - /** Creates a new lights. */ - public BlinkInLEDs() { - } - - public void setYellow() { - lightController.set(CosmeticConstants.SOLID_YELLOW_VALUE); - } - - public void setPurple() { - lightController.set(CosmeticConstants.SOLID_PURPLE_VALUE); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - // lightController.set(lightColor.getDouble(CosmeticConstants.SOLID_YELLOW_VALUE)); - } -} diff --git a/main/java/frc/robot/cosmetics/PwmLEDs.java b/main/java/frc/robot/cosmetics/PwmLEDs.java deleted file mode 100644 index acb2796..0000000 --- a/main/java/frc/robot/cosmetics/PwmLEDs.java +++ /dev/null @@ -1,249 +0,0 @@ -// 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. - -package frc.robot.cosmetics; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.CosmeticConstants; - -public class PwmLEDs extends SubsystemBase { - private final AddressableLED lights = new AddressableLED(CosmeticConstants.LIGHT_ID); - private AddressableLEDBuffer buffer = new AddressableLEDBuffer(CosmeticConstants.LIGHT_LENGTH); - private Color color1 = Color.kBlack; - private Color color2 = Color.kBlack; - private double onLength = 0.0; - private double offLength = 0.0; - private int color1Length = 0; - private int color2Length = 0; - private double speed = 0; - private double cycleLength = 0.0; - private double duration = 0.0; - - private double mp1 = 90.0; - private double mp2 = 60.0; - private double mp3 = 30.0; - private double mp4 = 15.0; - private double mp5 = 10.0; - private double mp6 = 5.0; - private double mpTolerance = 0.5; - - private Mode lightMode = Mode.SOLID; - - public void setLightMode(Mode lightMode) { - this.lightMode = lightMode; - } - - public Color getColor1() { - return color1; - } - - public void setColor1(Color color1) { - this.color1 = color1; - } - - public Color getColor2() { - return color2; - } - - public void setColor2(Color color2) { - this.color2 = color2; - } - - public void setColor1Length(int color1Length) { - this.color1Length = color1Length; - } - - public void setColor2Length(int color2Length) { - this.color2Length = color2Length; - } - - public void setSpeed(double speed) { - this.speed = speed; - } - - public static enum Mode { - SOLID, WAVE, CLIMB, STROBE; - } - - /** Creates a new PwmLEDs. */ - public PwmLEDs() { - lights.setLength(CosmeticConstants.LIGHT_LENGTH); - lights.setData(buffer); - lights.start(); - } - - public void solid(Color color) { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setLED(i, color); - } - } - - public void setSolid(Color color) { - this.color1 = color; - this.lightMode = Mode.SOLID; - } - - public void wave(Color color1, Color color2, double cycleLength, double duration) { - double counter = (1 - ((Timer.getFPGATimestamp() % duration) / duration)) * 2.0 * Math.PI; - double counterDiffPerLed = (2.0 * Math.PI) / cycleLength; - for (int i = 0; i < buffer.getLength(); i++) { - counter += counterDiffPerLed; - if (i >= 0) { - double ratio = (Math.pow(Math.sin(counter), 0.4) + 1.0) / 2.0; - if (Double.isNaN(ratio)) { - ratio = (-Math.pow(Math.sin(counter + Math.PI), 0.4) + 1.0) / 2.0; - } - if (Double.isNaN(ratio)) { - ratio = 0.5; - } - double red = (color1.red * (1 - ratio)) + (color2.red * ratio); - double green = (color1.green * (1 - ratio)) + (color2.green * ratio); - double blue = (color1.blue * (1 - ratio)) + (color2.blue * ratio); - buffer.setLED(i, new Color(red, green, blue)); - } - } - } - - public void wave(Color color, double cycleLength, double duration) { - wave(color, Color.kBlack, cycleLength, duration); - } - - public void setWave(Color color1, Color color2, double cycleLength, double duration) { - this.color1 = color1; - this.color2 = color2; - this.cycleLength = cycleLength; - this.duration = duration; - this.lightMode = Mode.WAVE; - } - - public void climb(Color color1, Color color2, int color1Length, int color2Length, double speed) { - int counter = (int) Math.floor(Timer.getFPGATimestamp() * speed); - for (int i = 0; i < buffer.getLength(); i += color2Length + color1Length) { - for (int j = 0; j < color1Length; j++) { - buffer.setLED((i + j + counter) % buffer.getLength(), color1); - } - for (int j = color1Length; j < color1Length + color2Length; j++) { - buffer.setLED((i + j + counter) % buffer.getLength(), color2); - } - } - } - - public void climb(Color color, int colorLength, int offLength, double speed) { - climb(color, Color.kBlack, colorLength, offLength, speed); - } - - public void setClimb(Color color1, Color color2, int color1Length, int color2Length, double speed) { - this.color1 = color1; - this.color2 = color2; - this.color1Length = color1Length; - this.color2Length = color2Length; - this.speed = speed; - this.lightMode = Mode.CLIMB; - } - - public void strobe(Color color1, Color color2, double onLength, double offLength) { - boolean lightsOn = Timer.getFPGATimestamp() % onLength + offLength > onLength; - if (lightsOn) { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setLED(i, color1); - } - } else { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setLED(i, color2); - } - } - } - - public void strobe(Color color, double onLength, double offLength) { - strobe(color, Color.kBlack, onLength, offLength); - } - - public void setStrobe(Color color1, Color color2, double onLength, double offLength) { - this.color1 = color1; - this.color2 = color2; - this.onLength = onLength; - this.offLength = offLength; - this.lightMode = Mode.STROBE; - } - - public void setDefault() { - Color color1 = Color.kBlue; - Color color2 = Color.kGold; - if (DriverStation.isEStopped()) { - color1 = Color.kDarkGreen; - color2 = Color.kPowderBlue; - } - if (!DriverStation.isFMSAttached()) { - color1 = PwmLEDs.dimColor(color1, 0.25); - color2 = PwmLEDs.dimColor(color2, 0.25); - } - - setWave(color1, color2, 10, 3); - } - - public static Color dimColor(Color color, double brightness) { - return new Color(color.red * brightness, color.green * brightness, color.blue * brightness); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - - double time = Timer.getMatchTime(); - if (Math.abs(time - mp1) <= mpTolerance) { - solid(Color.kBlue); - lights.setData(buffer); - return; - } - if (Math.abs(time - mp2) <= mpTolerance) { - solid(Color.kGreen); - lights.setData(buffer); - return; - } - if (Math.abs(time - mp3) <= mpTolerance) { - solid(Color.kFirstRed); - lights.setData(buffer); - return; - } - if (time < mp4 && time > mp5 && time % 1.0 > 0.5) { - solid(dimColor(Color.kDarkGoldenrod, 0.5)); - lights.setData(buffer); - return; - } - if (time < mp5 && time > mp6 && time % 1.0 > 0.5) { - solid(dimColor(Color.kFirstBlue, 0.5)); - lights.setData(buffer); - return; - } - if (time < mp6 && time % 1.0 > 0.5) { - solid(dimColor(Color.kFirstRed, 0.5)); - lights.setData(buffer); - return; - } - - switch (lightMode) { - case SOLID: - solid(color1); - break; - case WAVE: - wave(color1, color2, cycleLength, duration); - break; - case CLIMB: - climb(color1, color2, color1Length, color2Length, speed); - break; - case STROBE: - strobe(color1, color2, onLength, offLength); - break; - default: - solid(Color.kBlack); - } - - lights.setData(buffer); - } -} diff --git a/main/java/frc/robot/data/LoggingSystem.java b/main/java/frc/robot/data/LoggingSystem.java deleted file mode 100644 index 5c21203..0000000 --- a/main/java/frc/robot/data/LoggingSystem.java +++ /dev/null @@ -1,61 +0,0 @@ -// 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. - -package frc.robot.data; - -import java.util.Dictionary; -import java.util.Map; - -import com.ctre.phoenix6.SignalLogger; - -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.LoggingConstants; -import frc.robot.subsystems.DriveSystem; - -public class LoggingSystem extends SubsystemBase { - - private Map subsystemArray; - private boolean loggingState = LoggingConstants.DEFAULT_LOGGING_STATE; - private GenericEntry loggingChooser; - - - /** Creates a new LoggingSystem. */ - public LoggingSystem(Map subsystemArray) { - loggingChooser = Shuffleboard.getTab("Logging").add("Enable Logging", false).getEntry(); - SmartDashboard.putBoolean("Logging Chooser", false); - this.subsystemArray = subsystemArray; - } - - public boolean getLoggingState() { - return loggingState; - } - - private boolean getLoggingFlag() { - // An example command will be run in autonomous - return loggingChooser.getBoolean(false); // TODO: Actually use this value -} - - @Override - public void periodic() { - // This method will be called once per scheduler run - if (getLoggingFlag() != loggingState) { - loggingState = getLoggingFlag(); - - if (loggingState) { - SignalLogger.start(); - // Log the odometry pose as a double array - } else { - SignalLogger.stop(); - } - } - if (loggingState){ - var pose = ((DriveSystem)subsystemArray.get(new String("Drive"))).getState().Pose; - var status = SignalLogger.writeDoubleArray("odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); - } - } -} diff --git a/main/java/frc/robot/generated/TunerConstants.java b/main/java/frc/robot/generated/TunerConstants.java deleted file mode 100644 index cbbbb05..0000000 --- a/main/java/frc/robot/generated/TunerConstants.java +++ /dev/null @@ -1,286 +0,0 @@ -package frc.robot.generated; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.hardware.*; -import com.ctre.phoenix6.signals.*; -import com.ctre.phoenix6.swerve.*; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.*; - -import frc.robot.subsystems.DriveSystem; - -// Generated by the Tuner X Swerve Project Generator -// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html -public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) // 100, 0, 0.5 - .withKS(0.1).withKV(1.91).withKA(0) // 0, 1.5, 0 - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) // 3, 0, 0 - .withKS(0).withKV(0.124); // removed KA changed and KV from 0 - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); // 150 - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) - ); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); // Default Name - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); // 5 - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; // 3.5714285714285716 - - private static final double kDriveGearRatio = 7.363636363636365; // 6.122448979591837 - private static final double kSteerGearRatio = 15.42857142857143; // 12.8 - private static final Distance kWheelRadius = Inches.of(2.167); // 1.92 - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 13; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); // 0.00001 - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); // 0.001 - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); // 0.25 - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); // 0.25 - - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - - // Front Left - private static final int kFrontLeftDriveMotorId = 11; - private static final int kFrontLeftSteerMotorId = 12; - private static final int kFrontLeftEncoderId = 10; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); // -0.052734375 - private static final boolean kFrontLeftSteerMotorInverted = true; // false - private static final boolean kFrontLeftEncoderInverted = false; // new - - private static final Distance kFrontLeftXPos = Inches.of(10); // 9.25 - private static final Distance kFrontLeftYPos = Inches.of(10); // 9.25 - - // Front Right - private static final int kFrontRightDriveMotorId = 2; - private static final int kFrontRightSteerMotorId = 3; - private static final int kFrontRightEncoderId = 1; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); // -0.6796875 - private static final boolean kFrontRightSteerMotorInverted = true; // false - private static final boolean kFrontRightEncoderInverted = false; // new - - private static final Distance kFrontRightXPos = Inches.of(10); // 9.25 - private static final Distance kFrontRightYPos = Inches.of(-10); // -9.25 - - // Back Left - private static final int kBackLeftDriveMotorId = 8; - private static final int kBackLeftSteerMotorId = 9; - private static final int kBackLeftEncoderId = 7; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); // -0.304931640625 - private static final boolean kBackLeftSteerMotorInverted = true; // false - private static final boolean kBackLeftEncoderInverted = false; // new - - private static final Distance kBackLeftXPos = Inches.of(-10); // -9.25 - private static final Distance kBackLeftYPos = Inches.of(10); // 9.25 - - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 6; - private static final int kBackRightEncoderId = 4; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); // -0.032470703125 - private static final boolean kBackRightSteerMotorInverted = true; // false - private static final boolean kBackRightEncoderInverted = false; // new - - private static final Distance kBackRightXPos = Inches.of(-10); // -9.25 - private static final Distance kBackRightYPos = Inches.of(-10); // -9.25 - - - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted - ); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted - ); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted - ); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted - ); - - /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. - */ - public static DriveSystem createDrivetrain() { - return new DriveSystem( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ); - } - - - /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. - */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules - ); - } - } -} \ No newline at end of file diff --git a/main/java/frc/robot/subsystems/DriveSystem.java b/main/java/frc/robot/subsystems/DriveSystem.java deleted file mode 100644 index 3447342..0000000 --- a/main/java/frc/robot/subsystems/DriveSystem.java +++ /dev/null @@ -1,293 +0,0 @@ -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.*; - -import java.util.function.Supplier; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -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.generated.TunerConstants.TunerSwerveDrivetrain; - -/** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements - * Subsystem so it can easily be used in command-based projects. - */ -public class DriveSystem extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; - - /** Swerve request to apply during robot-centric path following */ - private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this - ) - ); - - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public DriveSystem( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public DriveSystem( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public DriveSystem( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - private void configureAutoBuilder() { - try { - var config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( - () -> getState().Pose, // Supplier of current robot pose - this::resetPose, // Consumer for seeding pose against auto - () -> getState().Speeds, // Supplier of current robot speeds - // Consumer of ChassisSpeeds and feedforwards to drive the robot - (speeds, feedforwards) -> setControl( - m_pathApplyRobotSpeeds.withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) - ), - new PPHolonomicDriveController( - // PID constants for translation - new PIDConstants(10, 0, 0), // 5, 0, 0 - // PID constants for rotation - new PIDConstants(7, 0, 0) // 5, 0, 0 - ), - config, - // Assume the path needs to be flipped for Red vs Blue, this is normally the case - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this // Subsystem for requirements - ); - } catch (Exception ex) { - DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); - } - } - - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - - @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 (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); - m_hasAppliedOperatorPerspective = true; - }); - } - } - - 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); - } -} \ No newline at end of file diff --git a/main/java/org/assabet/aztechs157/PrintLimiter.java b/main/java/org/assabet/aztechs157/PrintLimiter.java deleted file mode 100644 index aa6dc7a..0000000 --- a/main/java/org/assabet/aztechs157/PrintLimiter.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.assabet.aztechs157; - -public class PrintLimiter { - private final int ticksPerPrint; - private int currentTicks = 0; - - public PrintLimiter(final int ticksPerPrint) { - this.ticksPerPrint = ticksPerPrint; - } - - public PrintLimiter tick() { - currentTicks++; - - if (currentTicks > ticksPerPrint) { - currentTicks = 0; - } - - return this; - } - - public PrintLimiter println(final String message) { - if (currentTicks == 0) { - System.out.println(message); - } - - return this; - } - - public PrintLimiter print(final String message) { - if (currentTicks == 0) { - System.out.print(message); - } - - return this; - } - - public PrintLimiter limitBody(final Runnable body) { - if (currentTicks == 0) { - body.run(); - } - - return this; - } -} diff --git a/main/java/org/assabet/aztechs157/Sanity.java b/main/java/org/assabet/aztechs157/Sanity.java deleted file mode 100644 index a2ac436..0000000 --- a/main/java/org/assabet/aztechs157/Sanity.java +++ /dev/null @@ -1,91 +0,0 @@ -package org.assabet.aztechs157; - -import org.assabet.aztechs157.numbers.Range; - -public final class Sanity { - private Sanity() { - throw new UnsupportedOperationException("Expect is a utility class"); - } - - public static class SanityError extends RuntimeException { - public SanityError(final String message) { - super(message); - } - } - - public static NumberSanity check(final double number) { - return new NumberSanity(number); - } - - public static record NumberSanity(double value) { - public NumberSanity containedWithin(final Range range) { - if (range.contains(value)) { - return this; - } else { - throw new SanityError(value + " was not contained within " + range); - } - } - - public NumberSanity equalTo(final double other) { - if (value == other) { - return this; - } else { - throw new SanityError(value + " was not equal to " + other); - } - } - - public NumberSanity notEqualTo(final double other) { - if (value != other) { - return this; - } else { - throw new SanityError(value + " was equal to " + other); - } - } - - public NumberSanity greaterThan(final double other) { - if (value > other) { - return this; - } else { - throw new SanityError(value + " was not greater than " + other); - } - } - - public NumberSanity lessThan(final double other) { - if (value < other) { - return this; - } else { - throw new SanityError(value + " was not less than " + other); - } - } - - public NumberSanity greaterOrEqual(final double other) { - if (value >= other) { - return this; - } else { - throw new SanityError(value + " was not greater or equal to " + other); - } - } - - public NumberSanity lessOrEqual(final double other) { - if (value <= other) { - return this; - } else { - throw new SanityError(value + " was not less or equal to " + other); - } - } - } - - public static BooleanSanity check(final boolean value) { - return new BooleanSanity(value); - } - - public static record BooleanSanity(boolean value) { - public BooleanSanity equal(final boolean other) { - if (value == other) { - return this; - } else { - throw new SanityError(value + " was not equal to " + other); - } - } - } -} diff --git a/main/java/org/assabet/aztechs157/input/Model.java b/main/java/org/assabet/aztechs157/input/Model.java deleted file mode 100644 index daa0894..0000000 --- a/main/java/org/assabet/aztechs157/input/Model.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.assabet.aztechs157.input; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -/** - * Models map physical inputs on a input device to input classes such as - * {@link Button}, {@link Axis}, or {@link Pov}. - */ -public class Model { - - public final int deviceId; - - /** - * Create a Model that models the device specified by `deviceId` - * - * @param deviceId The id of the device - */ - public Model(final int deviceId) { - this.deviceId = deviceId; - } - - /** - * Create a {@link Button} that models a physical button - * - * @param buttonId The button to model - * @return The modeled {@link Button} - */ - public Button button(final int buttonId) { - return Button.fromDriverStation(deviceId, buttonId); - } - - /** - * Create a {@link Axis} that models a physical axis - * - * @param buttonId The axis to model - * @return The modeled {@link Axis} - */ - public Axis axis(final int axisId) { - return Axis.fromDriverStation(deviceId, axisId); - } - - /** - * Create a {@link Pov} that modes a physical pov - * - * @param povId The pov to model - * @return The modeled {@link Pov} - */ - public Pov pov(final int povId) { - return Pov.fromDriverStation(deviceId, povId); - } -} diff --git a/main/java/org/assabet/aztechs157/input/Pov.java b/main/java/org/assabet/aztechs157/input/Pov.java deleted file mode 100644 index 7dd2a8f..0000000 --- a/main/java/org/assabet/aztechs157/input/Pov.java +++ /dev/null @@ -1,67 +0,0 @@ -package org.assabet.aztechs157.input; - -import java.util.function.IntSupplier; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -import edu.wpi.first.wpilibj.DriverStation; - -/** - * Class for getting input from a pov. - */ -public class Pov { - private final IntSupplier degrees; - - public Pov(final IntSupplier degrees) { - this.degrees = degrees; - } - - public static Pov fromDriverStation(final int deviceId, final int povId) { - return new Pov(() -> DriverStation.getStickPOV(deviceId, povId)); - } - - public int get() { - return degrees.getAsInt(); - } - - public final Axis value = new Axis(() -> get()); - - public final Axis y = new Axis(() -> switch (get()) { - case UP_LEFT, UP, UP_RIGHT -> 1; - case LEFT, CENTER, RIGHT -> 0; - case DOWN_LEFT, DOWN, DOWN_RIGHT -> -1; - default -> 0; - }); - - public final Axis x = new Axis(() -> switch (get()) { - case DOWN_RIGHT, RIGHT, UP_RIGHT -> 1; - case DOWN, CENTER, UP -> 0; - case DOWN_LEFT, LEFT, UP_LEFT -> -1; - default -> 0; - }); - - public static final int CENTER = -1; - public static final int UP = 45 * 0; - public static final int UP_RIGHT = 45 * 1; - public static final int RIGHT = 45 * 2; - public static final int DOWN_RIGHT = 45 * 3; - public static final int DOWN = 45 * 4; - public static final int DOWN_LEFT = 45 * 5; - public static final int LEFT = 45 * 6; - public static final int UP_LEFT = 45 * 7; - - private Button buttonForValue(final int degrees, final String name) { - return new Button(() -> get() == degrees); - } - - public final Button center = buttonForValue(CENTER, "Center"); - public final Button up = buttonForValue(UP, "Up"); - public final Button upRight = buttonForValue(UP_RIGHT, "Up Right"); - public final Button right = buttonForValue(RIGHT, "Right"); - public final Button downRight = buttonForValue(DOWN_RIGHT, "Down Right"); - public final Button down = buttonForValue(DOWN, "Down"); - public final Button downLeft = buttonForValue(DOWN_LEFT, "Down Left"); - public final Button left = buttonForValue(LEFT, "Left"); - public final Button upLeft = buttonForValue(UP_LEFT, "Up Left"); -} diff --git a/main/java/org/assabet/aztechs157/input/layouts/DynamicLayout.java b/main/java/org/assabet/aztechs157/input/layouts/DynamicLayout.java deleted file mode 100644 index c4cbcf7..0000000 --- a/main/java/org/assabet/aztechs157/input/layouts/DynamicLayout.java +++ /dev/null @@ -1,43 +0,0 @@ -package org.assabet.aztechs157.input.layouts; - -import java.util.function.Supplier; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -/** - * Object that manages layouts. A layout can be selected from Shuffleboard that - * can then be used by the robot. It maps the inputs of a - * {@link DynamicLayout} to the desired functions of the robot. - */ -public class DynamicLayout implements Layout { - private final Supplier layoutSupplier; - - public DynamicLayout(final Supplier layoutSupplier) { - this.layoutSupplier = layoutSupplier; - } - - public Layout getCurrent() { - return layoutSupplier.get(); - } - - /** - * Get a button from the currently selected layout. - * - * @param key Which button to retrieve - * @return A {@link Button} and {@link Button.Key} representing the input - */ - public Button button(final Button.Key key) { - return new Button(() -> getCurrent().button(key).get()); - } - - /** - * Get a axis from the currently selected layout. - * - * @param key Which axis to retrieve - * @return A {@link Axis} representing the input - */ - public Axis axis(final Axis.Key key) { - return new Axis(() -> getCurrent().axis(key).get()); - } -} diff --git a/main/java/org/assabet/aztechs157/input/layouts/Layout.java b/main/java/org/assabet/aztechs157/input/layouts/Layout.java deleted file mode 100644 index 01c914c..0000000 --- a/main/java/org/assabet/aztechs157/input/layouts/Layout.java +++ /dev/null @@ -1,22 +0,0 @@ -package org.assabet.aztechs157.input.layouts; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public interface Layout { - /** - * Retrieve the {@link Button} associated with a {@link Button.Key} - * - * @param key The key a button was assigned to - * @return The associated button - */ - public Button button(final Button.Key key); - - /** - * Retrieve the {@link Axis} associated with a {@link Axis.KeyBase} - * - * @param key The key an axis was assigned to - * @return The associated axis - */ - public Axis axis(final Axis.Key key); -} diff --git a/main/java/org/assabet/aztechs157/input/layouts/MapLayout.java b/main/java/org/assabet/aztechs157/input/layouts/MapLayout.java deleted file mode 100644 index fe4c8b0..0000000 --- a/main/java/org/assabet/aztechs157/input/layouts/MapLayout.java +++ /dev/null @@ -1,62 +0,0 @@ -package org.assabet.aztechs157.input.layouts; - -import java.util.HashMap; -import java.util.Map; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -/** - * A simple structure that stores the mapping between keys and inputs. These can - * be used with {@link DynamicLayout} to allow hot-swapping of layouts. - */ -public class MapLayout implements Layout { - private final Map buttons = new HashMap<>(); - private final Map axes = new HashMap<>(); - - /** - * For this Layout, assign a {@link Button.Key} to a {@link Button}. - * Calling - * this method multiple times with the same key will override the previous - * assignment. - * - * @param key The key to assign with - * @param button The button being assigned - */ - public void assign(final Button.Key key, final Button button) { - buttons.put(key, button); - } - - /** - * For this Layout, assign a {@link Axis.KeyBase} to a {@link Axis}. Calling - * this - * method multiple times with the same key will override the previous - * assignment. - * - * @param key The key to assign with - * @param axis The axis being assigned - */ - public void assign(final Axis.Key key, final Axis axis) { - axes.put(key, axis); - } - - /** - * Retrieve the {@link Button} associated with a {@link Button.Key} - * - * @param key The key a button was assigned to - * @return The associated button - */ - public Button button(final Button.Key key) { - return buttons.get(key); - } - - /** - * Retrieve the {@link Axis} associated with a {@link Axis.KeyBase} - * - * @param key The key an axis was assigned to - * @return The associated axis - */ - public Axis axis(final Axis.Key key) { - return axes.get(key); - } -} diff --git a/main/java/org/assabet/aztechs157/input/models/LogitechAttack.java b/main/java/org/assabet/aztechs157/input/models/LogitechAttack.java deleted file mode 100644 index c097cc6..0000000 --- a/main/java/org/assabet/aztechs157/input/models/LogitechAttack.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.assabet.aztechs157.input.models; - -import org.assabet.aztechs157.input.Model; -import org.assabet.aztechs157.input.Pov; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public class LogitechAttack extends Model { - - public LogitechAttack(final int joystickId) { - super(joystickId); - } - - public final Button trigger = button(1); - public final Button button2 = button(2); - public final Button button3 = button(3); - public final Button button4 = button(4); - public final Button button5 = button(5); - public final Button button6 = button(6); - public final Button button7 = button(7); - public final Button button8 = button(8); - public final Button button9 = button(9); - public final Button button10 = button(10); - public final Button button11 = button(11); - - public final Axis stickX = axis(0); - public final Axis stickY = axis(1); - public final Axis slider = axis(3); - - public final Pov pov = pov(0); -} diff --git a/main/java/org/assabet/aztechs157/input/models/LogitechExtreme3D.java b/main/java/org/assabet/aztechs157/input/models/LogitechExtreme3D.java deleted file mode 100644 index 59a51e3..0000000 --- a/main/java/org/assabet/aztechs157/input/models/LogitechExtreme3D.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.assabet.aztechs157.input.models; - -import org.assabet.aztechs157.input.Model; -import org.assabet.aztechs157.input.Pov; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public class LogitechExtreme3D extends Model { - public LogitechExtreme3D(final int joystickId) { - super(joystickId); - } - - public final Button trigger = button(1); - public final Button thumb = button(2); - public final Button button3 = button(3); - public final Button button4 = button(4); - public final Button button5 = button(5); - public final Button button6 = button(6); - public final Button button7 = button(7); - public final Button button8 = button(8); - public final Button button9 = button(9); - public final Button button10 = button(10); - public final Button button11 = button(11); - public final Button button12 = button(12); - - public final Axis stickX = axis(0); - public final Axis stickY = axis(1); - public final Axis stickRotate = axis(2); - public final Axis slider = axis(3); - - public final Pov pov = pov(0); -} diff --git a/main/java/org/assabet/aztechs157/input/models/LogitechGamepadF310.java b/main/java/org/assabet/aztechs157/input/models/LogitechGamepadF310.java deleted file mode 100644 index 3be4af0..0000000 --- a/main/java/org/assabet/aztechs157/input/models/LogitechGamepadF310.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.assabet.aztechs157.input.models; - -import org.assabet.aztechs157.input.Model; -import org.assabet.aztechs157.input.Pov; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public class LogitechGamepadF310 extends Model { - - public LogitechGamepadF310(final int joystickId) { - super(joystickId); - } - - public final Button a = button(1); - public final Button b = button(2); - public final Button x = button(3); - public final Button y = button(4); - public final Button leftBumper = button(5); - public final Button rightBumper = button(6); - public final Button back = button(7); - public final Button start = button(8); - public final Button leftStickPress = button(9); - public final Button rightStickPress = button(10); - - public final Axis leftStickX = axis(0); - public final Axis leftStickY = axis(1); - public final Axis rightTriggerHeld = axis(2); - public final Axis leftTriggerHeld = axis(3); - public final Axis rightStickX = axis(4); - public final Axis rightStickY = axis(5); - public final Axis combinedTriggersHeld = rightTriggerHeld.offsetBy(leftTriggerHeld.inverted()::get); - - public final Pov pov = pov(0); -} diff --git a/main/java/org/assabet/aztechs157/input/models/XboxOne.java b/main/java/org/assabet/aztechs157/input/models/XboxOne.java deleted file mode 100644 index 0bc79fe..0000000 --- a/main/java/org/assabet/aztechs157/input/models/XboxOne.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.assabet.aztechs157.input.models; - -import org.assabet.aztechs157.input.Model; -import org.assabet.aztechs157.input.Pov; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public class XboxOne extends Model { - - public XboxOne(final int joystickId) { - super(joystickId); - } - - public final Button a = button(1); - public final Button b = button(2); - public final Button x = button(3); - public final Button y = button(4); - public final Button leftBumper = button(5); - public final Button rightBumper = button(6); - public final Button back = button(7); - public final Button start = button(8); - public final Button leftStickPress = button(9); - public final Button rightStickPress = button(10); - - public final Axis leftStickX = axis(0); - public final Axis leftStickY = axis(1); - public final Axis leftTriggerHeld = axis(2); - public final Axis rightTriggerHeld = axis(3); - public final Axis rightStickX = axis(4); - public final Axis rightStickY = axis(5); - public final Axis combinedTriggersHeld = rightTriggerHeld.offsetBy(leftTriggerHeld.inverted()::get); - - public final Pov pov = pov(0); -} diff --git a/main/java/org/assabet/aztechs157/input/values/Axis.java b/main/java/org/assabet/aztechs157/input/values/Axis.java deleted file mode 100644 index 892cbfd..0000000 --- a/main/java/org/assabet/aztechs157/input/values/Axis.java +++ /dev/null @@ -1,97 +0,0 @@ -package org.assabet.aztechs157.input.values; - -import java.util.function.DoubleConsumer; -import java.util.function.DoubleSupplier; -import java.util.function.DoubleUnaryOperator; - -import org.assabet.aztechs157.numbers.Range; - -import edu.wpi.first.wpilibj.DriverStation; - -/** - * Class for getting input from a axis. This class has methods and static - * methods to modify and compose {@link Axis}s into a new - * {@link Axis}. - */ -public class Axis { - public static class Key { - } - - public static final Range kDeviceDefaultRange = new Range(-1, 1); - - private final DoubleSupplier value; - - public Axis(final DoubleSupplier value) { - this.value = value; - } - - public static Axis fromDriverStation(final int deviceId, final int axisId) { - return new Axis(() -> DriverStation.getStickAxis(deviceId, axisId)); - } - - public static Axis always(final double value) { - return new Axis(() -> value); - } - - public double get() { - return value.getAsDouble(); - } - - public Axis map(final DoubleUnaryOperator body) { - return new Axis(() -> body.applyAsDouble(get())); - } - - public Axis tap(final DoubleConsumer body) { - return map(value -> { - body.accept(value); - return value; - }); - } - - /** - * Inverts the input by negating the number's sign - * - * @return A new inverted input - */ - public Axis inverted() { - return map(value -> -value); - } - - /** - * Scale the input with a scalar value. - * - * @param scale The value to scale by - * @return A new input with the scale applied - */ - public Axis scaledBy(final double scale) { - return map(value -> value * scale); - } - - /** - * Scale the input with another input. - * - * @param scale The input to retrieve the scale from - * @return A new input with the scale applied - */ - public Axis scaledBy(final DoubleSupplier scale) { - return map(value -> value * scale.getAsDouble()); - } - - public Axis offsetBy(final double offset) { - return map(value -> value + offset); - } - - public Axis offsetBy(final DoubleSupplier offset) { - return map(value -> value + offset.getAsDouble()); - } - - /** - * Clamp the input to a number within the provided range. - * - * @param range The range to clamp to - * @return A new input with clamp applied - */ - public Axis clampTo(final Range range) { - return map(value -> range.clamp(value)); - } -} diff --git a/main/java/org/assabet/aztechs157/input/values/Button.java b/main/java/org/assabet/aztechs157/input/values/Button.java deleted file mode 100644 index ebd6a2d..0000000 --- a/main/java/org/assabet/aztechs157/input/values/Button.java +++ /dev/null @@ -1,131 +0,0 @@ -package org.assabet.aztechs157.input.values; - -import java.util.function.BooleanSupplier; -import java.util.function.UnaryOperator; - -import edu.wpi.first.util.function.BooleanConsumer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -/** - * Class for getting input from a button. This class has methods and static - * methods to modify and compose {@link Button}s into a new - * {@link Button}. - */ -public class Button { - public static class Key { - } - - private final BooleanSupplier value; - - public Button(final BooleanSupplier value) { - this.value = value; - } - - public static Button fromDriverStation(final int deviceId, final int buttonId) { - return new Button(() -> DriverStation.getStickButton(deviceId, buttonId)); - } - - public static Button always(final boolean value) { - return new Button(() -> value); - } - - public boolean get() { - return value.getAsBoolean(); - } - - public Button whenPressed(final Command command) { - new Trigger(value).onTrue(command); - return this; - } - - public Button whileHeld(final Command command) { - new Trigger(value).whileTrue(command); - return this; - } - - public Button toggleWhenPressed(final Command command) { - new Trigger(value).toggleOnTrue(command); - return this; - } - - public Button map(final UnaryOperator body) { - return new Button(() -> body.apply(get())); - } - - public Button tap(final BooleanConsumer body) { - return map(value -> { - body.accept(value); - return value; - }); - } - - /** - * Inverts the input; similar to a boolean `!` - * - * @return A new inverted input - */ - public Button inverted() { - return map(value -> !value); - } - - /** - * Checks that all inputs are true; similar to a boolean `&&` - * - * @param first The first input - * @param rest The rest of the inputs - * @return A new input that is only true when all of the passed inputs are true - */ - public static Button all(final Button first, final Button... rest) { - // The first argument is explicit to prevent being given empty arrays - - return new Button(() -> { - // Check each input individually - // As soon as one input is false, return false - - if (first != null && first.get() == false) { - return false; - } - - for (final var input : rest) { - if (input != null && input.get() == false) { - return false; - } - } - - // All inputs are true at this point, so return true - return true; - }); - } - - /** - * Checks that any input is true; similar to a boolean `||` - * - * @param first The first input - * @param rest The rest of the inputs - * @return A new input that is true when any of the passed inputs are true - */ - public static Button any(final Button first, final Button... rest) { - // The first argument is explicit to prevent being given empty arrays - - return new Button(() -> { - // Check each input individually - // As soon as one input is true, return true - - if (first != null && first.get()) { - return true; - } - - for (final var input : rest) { - if (input != null && input.get()) { - return true; - } - } - - // All inputs are false at this point, so return false - return false; - }); - - } -} diff --git a/main/java/org/assabet/aztechs157/numbers/Deadzone.java b/main/java/org/assabet/aztechs157/numbers/Deadzone.java deleted file mode 100644 index 62a1578..0000000 --- a/main/java/org/assabet/aztechs157/numbers/Deadzone.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.assabet.aztechs157.numbers; - -import org.assabet.aztechs157.input.values.Axis; - -public class Deadzone { - public final Range deadzone; - public final Range full; - public final RangeConverter leftConverter; - public final RangeConverter rightConverter; - - public static Deadzone forAxis(final Range deadzone) { - return new Deadzone(deadzone, Axis.kDeviceDefaultRange, 0); - } - - public Deadzone(final Range deadzone, final Range full, final double fullCenter) { - this.deadzone = deadzone; - this.full = full; - - final var leftFull = new Range(full.start(), fullCenter); - final var leftDeadzone = new Range(full.start(), deadzone.start()); - this.leftConverter = new RangeConverter(leftDeadzone, leftFull); - - final var rightFull = new Range(fullCenter, full.end()); - final var rightDeadzone = new Range(deadzone.end(), full.end()); - this.rightConverter = new RangeConverter(rightDeadzone, rightFull); - } - - public double apply(final double input) { - if (deadzone.contains(input)) { - return 0; - } else if (leftConverter.inputRange.contains(input)) { - return leftConverter.convert(input); - } else if (rightConverter.inputRange.contains(input)) { - return rightConverter.convert(input); - } - - throw new Error("Attempted to apply deadzone to input outside of full range " - + full.start() + " to " + full.end()); - } -} diff --git a/main/java/org/assabet/aztechs157/numbers/Range.java b/main/java/org/assabet/aztechs157/numbers/Range.java deleted file mode 100644 index a69bb32..0000000 --- a/main/java/org/assabet/aztechs157/numbers/Range.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.assabet.aztechs157.numbers; - -public record Range(double start, double end) { - - public boolean contains(final double value) { - return start <= value && value <= end; - } - - public double range() { - return end - start; - } - - public double clamp(final double value) { - if (value < start) { - return start; - } else if (value > end) { - return end; - } else { - return value; - } - } - - public double limitMotionWithinRange(final double speed, final double currentPosition) { - if (speed > 0 && currentPosition > end) { - return 0; - } else if (speed < 0 && currentPosition < start) { - return 0; - } else { - return speed; - } - } - - public RangeConverter convertingTo(final Range output) { - return new RangeConverter(this, output); - } -} diff --git a/main/java/org/assabet/aztechs157/numbers/RangeConverter.java b/main/java/org/assabet/aztechs157/numbers/RangeConverter.java deleted file mode 100644 index edb424e..0000000 --- a/main/java/org/assabet/aztechs157/numbers/RangeConverter.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.assabet.aztechs157.numbers; - -public class RangeConverter { - public final Range inputRange; - public final Range outputRange; - public final double scaleFactor; - - public RangeConverter(final Range inputRange, final Range outputRange) { - this.inputRange = inputRange; - this.outputRange = outputRange; - this.scaleFactor = outputRange.range() / inputRange.range(); - } - - public double convert(final double inputValue) { - // Shift to zero based input range - final var basedInput = inputValue - inputRange.start(); - - // Scale the zero based input - final var scaled = basedInput * scaleFactor; - - // Shift from zero based to output range - final var outputValue = scaled + outputRange.start(); - - return outputValue; - } -} diff --git a/music/BuddyHollyRiff.chrp b/music/BuddyHollyRiff.chrp deleted file mode 100644 index 9489fdd..0000000 Binary files a/music/BuddyHollyRiff.chrp and /dev/null differ diff --git a/music/MoreCowbell.chrp b/music/MoreCowbell.chrp deleted file mode 100644 index e5baa9f..0000000 Binary files a/music/MoreCowbell.chrp and /dev/null differ diff --git a/music/TOTTFIY.chrp b/music/TOTTFIY.chrp deleted file mode 100644 index 52411f1..0000000 Binary files a/music/TOTTFIY.chrp and /dev/null differ diff --git a/music/WTTBPchrp.chrp b/music/WTTBPchrp.chrp deleted file mode 100644 index 0b048cd..0000000 Binary files a/music/WTTBPchrp.chrp and /dev/null differ diff --git a/music/YourLoveOF.chrp b/music/YourLoveOF.chrp deleted file mode 100644 index dda3a25..0000000 Binary files a/music/YourLoveOF.chrp and /dev/null differ diff --git a/music/adams.chrp b/music/adams.chrp deleted file mode 100644 index f5be11b..0000000 Binary files a/music/adams.chrp and /dev/null differ diff --git a/music/bluetoothdeviceisreadytopair.chrp b/music/bluetoothdeviceisreadytopair.chrp deleted file mode 100644 index 6bb6ad6..0000000 Binary files a/music/bluetoothdeviceisreadytopair.chrp and /dev/null differ diff --git a/music/e1m1.chrp b/music/e1m1.chrp deleted file mode 100644 index 08f2fe6..0000000 Binary files a/music/e1m1.chrp and /dev/null differ diff --git a/music/entry.chrp b/music/entry.chrp deleted file mode 100644 index b384863..0000000 Binary files a/music/entry.chrp and /dev/null differ diff --git a/music/exorcist.chrp b/music/exorcist.chrp deleted file mode 100644 index b8d1d6f..0000000 Binary files a/music/exorcist.chrp and /dev/null differ diff --git a/music/output.chrp b/music/output.chrp deleted file mode 100644 index ca4e227..0000000 Binary files a/music/output.chrp and /dev/null differ diff --git a/music/sundog.chrp b/music/sundog.chrp deleted file mode 100644 index 0c76bb1..0000000 Binary files a/music/sundog.chrp and /dev/null differ diff --git a/music/super_mario.chrp b/music/super_mario.chrp deleted file mode 100644 index 16f8649..0000000 Binary files a/music/super_mario.chrp and /dev/null differ diff --git a/music/tetris.chrp b/music/tetris.chrp deleted file mode 100644 index 88363c3..0000000 Binary files a/music/tetris.chrp and /dev/null differ diff --git a/org/assabet/aztechs157/PrintLimiter.java b/org/assabet/aztechs157/PrintLimiter.java deleted file mode 100644 index aa6dc7a..0000000 --- a/org/assabet/aztechs157/PrintLimiter.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.assabet.aztechs157; - -public class PrintLimiter { - private final int ticksPerPrint; - private int currentTicks = 0; - - public PrintLimiter(final int ticksPerPrint) { - this.ticksPerPrint = ticksPerPrint; - } - - public PrintLimiter tick() { - currentTicks++; - - if (currentTicks > ticksPerPrint) { - currentTicks = 0; - } - - return this; - } - - public PrintLimiter println(final String message) { - if (currentTicks == 0) { - System.out.println(message); - } - - return this; - } - - public PrintLimiter print(final String message) { - if (currentTicks == 0) { - System.out.print(message); - } - - return this; - } - - public PrintLimiter limitBody(final Runnable body) { - if (currentTicks == 0) { - body.run(); - } - - return this; - } -} diff --git a/org/assabet/aztechs157/Sanity.java b/org/assabet/aztechs157/Sanity.java deleted file mode 100644 index a2ac436..0000000 --- a/org/assabet/aztechs157/Sanity.java +++ /dev/null @@ -1,91 +0,0 @@ -package org.assabet.aztechs157; - -import org.assabet.aztechs157.numbers.Range; - -public final class Sanity { - private Sanity() { - throw new UnsupportedOperationException("Expect is a utility class"); - } - - public static class SanityError extends RuntimeException { - public SanityError(final String message) { - super(message); - } - } - - public static NumberSanity check(final double number) { - return new NumberSanity(number); - } - - public static record NumberSanity(double value) { - public NumberSanity containedWithin(final Range range) { - if (range.contains(value)) { - return this; - } else { - throw new SanityError(value + " was not contained within " + range); - } - } - - public NumberSanity equalTo(final double other) { - if (value == other) { - return this; - } else { - throw new SanityError(value + " was not equal to " + other); - } - } - - public NumberSanity notEqualTo(final double other) { - if (value != other) { - return this; - } else { - throw new SanityError(value + " was equal to " + other); - } - } - - public NumberSanity greaterThan(final double other) { - if (value > other) { - return this; - } else { - throw new SanityError(value + " was not greater than " + other); - } - } - - public NumberSanity lessThan(final double other) { - if (value < other) { - return this; - } else { - throw new SanityError(value + " was not less than " + other); - } - } - - public NumberSanity greaterOrEqual(final double other) { - if (value >= other) { - return this; - } else { - throw new SanityError(value + " was not greater or equal to " + other); - } - } - - public NumberSanity lessOrEqual(final double other) { - if (value <= other) { - return this; - } else { - throw new SanityError(value + " was not less or equal to " + other); - } - } - } - - public static BooleanSanity check(final boolean value) { - return new BooleanSanity(value); - } - - public static record BooleanSanity(boolean value) { - public BooleanSanity equal(final boolean other) { - if (value == other) { - return this; - } else { - throw new SanityError(value + " was not equal to " + other); - } - } - } -} diff --git a/org/assabet/aztechs157/input/Model.java b/org/assabet/aztechs157/input/Model.java deleted file mode 100644 index daa0894..0000000 --- a/org/assabet/aztechs157/input/Model.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.assabet.aztechs157.input; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -/** - * Models map physical inputs on a input device to input classes such as - * {@link Button}, {@link Axis}, or {@link Pov}. - */ -public class Model { - - public final int deviceId; - - /** - * Create a Model that models the device specified by `deviceId` - * - * @param deviceId The id of the device - */ - public Model(final int deviceId) { - this.deviceId = deviceId; - } - - /** - * Create a {@link Button} that models a physical button - * - * @param buttonId The button to model - * @return The modeled {@link Button} - */ - public Button button(final int buttonId) { - return Button.fromDriverStation(deviceId, buttonId); - } - - /** - * Create a {@link Axis} that models a physical axis - * - * @param buttonId The axis to model - * @return The modeled {@link Axis} - */ - public Axis axis(final int axisId) { - return Axis.fromDriverStation(deviceId, axisId); - } - - /** - * Create a {@link Pov} that modes a physical pov - * - * @param povId The pov to model - * @return The modeled {@link Pov} - */ - public Pov pov(final int povId) { - return Pov.fromDriverStation(deviceId, povId); - } -} diff --git a/org/assabet/aztechs157/input/Pov.java b/org/assabet/aztechs157/input/Pov.java deleted file mode 100644 index 7dd2a8f..0000000 --- a/org/assabet/aztechs157/input/Pov.java +++ /dev/null @@ -1,67 +0,0 @@ -package org.assabet.aztechs157.input; - -import java.util.function.IntSupplier; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -import edu.wpi.first.wpilibj.DriverStation; - -/** - * Class for getting input from a pov. - */ -public class Pov { - private final IntSupplier degrees; - - public Pov(final IntSupplier degrees) { - this.degrees = degrees; - } - - public static Pov fromDriverStation(final int deviceId, final int povId) { - return new Pov(() -> DriverStation.getStickPOV(deviceId, povId)); - } - - public int get() { - return degrees.getAsInt(); - } - - public final Axis value = new Axis(() -> get()); - - public final Axis y = new Axis(() -> switch (get()) { - case UP_LEFT, UP, UP_RIGHT -> 1; - case LEFT, CENTER, RIGHT -> 0; - case DOWN_LEFT, DOWN, DOWN_RIGHT -> -1; - default -> 0; - }); - - public final Axis x = new Axis(() -> switch (get()) { - case DOWN_RIGHT, RIGHT, UP_RIGHT -> 1; - case DOWN, CENTER, UP -> 0; - case DOWN_LEFT, LEFT, UP_LEFT -> -1; - default -> 0; - }); - - public static final int CENTER = -1; - public static final int UP = 45 * 0; - public static final int UP_RIGHT = 45 * 1; - public static final int RIGHT = 45 * 2; - public static final int DOWN_RIGHT = 45 * 3; - public static final int DOWN = 45 * 4; - public static final int DOWN_LEFT = 45 * 5; - public static final int LEFT = 45 * 6; - public static final int UP_LEFT = 45 * 7; - - private Button buttonForValue(final int degrees, final String name) { - return new Button(() -> get() == degrees); - } - - public final Button center = buttonForValue(CENTER, "Center"); - public final Button up = buttonForValue(UP, "Up"); - public final Button upRight = buttonForValue(UP_RIGHT, "Up Right"); - public final Button right = buttonForValue(RIGHT, "Right"); - public final Button downRight = buttonForValue(DOWN_RIGHT, "Down Right"); - public final Button down = buttonForValue(DOWN, "Down"); - public final Button downLeft = buttonForValue(DOWN_LEFT, "Down Left"); - public final Button left = buttonForValue(LEFT, "Left"); - public final Button upLeft = buttonForValue(UP_LEFT, "Up Left"); -} diff --git a/org/assabet/aztechs157/input/layouts/DynamicLayout.java b/org/assabet/aztechs157/input/layouts/DynamicLayout.java deleted file mode 100644 index c4cbcf7..0000000 --- a/org/assabet/aztechs157/input/layouts/DynamicLayout.java +++ /dev/null @@ -1,43 +0,0 @@ -package org.assabet.aztechs157.input.layouts; - -import java.util.function.Supplier; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -/** - * Object that manages layouts. A layout can be selected from Shuffleboard that - * can then be used by the robot. It maps the inputs of a - * {@link DynamicLayout} to the desired functions of the robot. - */ -public class DynamicLayout implements Layout { - private final Supplier layoutSupplier; - - public DynamicLayout(final Supplier layoutSupplier) { - this.layoutSupplier = layoutSupplier; - } - - public Layout getCurrent() { - return layoutSupplier.get(); - } - - /** - * Get a button from the currently selected layout. - * - * @param key Which button to retrieve - * @return A {@link Button} and {@link Button.Key} representing the input - */ - public Button button(final Button.Key key) { - return new Button(() -> getCurrent().button(key).get()); - } - - /** - * Get a axis from the currently selected layout. - * - * @param key Which axis to retrieve - * @return A {@link Axis} representing the input - */ - public Axis axis(final Axis.Key key) { - return new Axis(() -> getCurrent().axis(key).get()); - } -} diff --git a/org/assabet/aztechs157/input/layouts/Layout.java b/org/assabet/aztechs157/input/layouts/Layout.java deleted file mode 100644 index 01c914c..0000000 --- a/org/assabet/aztechs157/input/layouts/Layout.java +++ /dev/null @@ -1,22 +0,0 @@ -package org.assabet.aztechs157.input.layouts; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public interface Layout { - /** - * Retrieve the {@link Button} associated with a {@link Button.Key} - * - * @param key The key a button was assigned to - * @return The associated button - */ - public Button button(final Button.Key key); - - /** - * Retrieve the {@link Axis} associated with a {@link Axis.KeyBase} - * - * @param key The key an axis was assigned to - * @return The associated axis - */ - public Axis axis(final Axis.Key key); -} diff --git a/org/assabet/aztechs157/input/layouts/MapLayout.java b/org/assabet/aztechs157/input/layouts/MapLayout.java deleted file mode 100644 index fe4c8b0..0000000 --- a/org/assabet/aztechs157/input/layouts/MapLayout.java +++ /dev/null @@ -1,62 +0,0 @@ -package org.assabet.aztechs157.input.layouts; - -import java.util.HashMap; -import java.util.Map; - -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -/** - * A simple structure that stores the mapping between keys and inputs. These can - * be used with {@link DynamicLayout} to allow hot-swapping of layouts. - */ -public class MapLayout implements Layout { - private final Map buttons = new HashMap<>(); - private final Map axes = new HashMap<>(); - - /** - * For this Layout, assign a {@link Button.Key} to a {@link Button}. - * Calling - * this method multiple times with the same key will override the previous - * assignment. - * - * @param key The key to assign with - * @param button The button being assigned - */ - public void assign(final Button.Key key, final Button button) { - buttons.put(key, button); - } - - /** - * For this Layout, assign a {@link Axis.KeyBase} to a {@link Axis}. Calling - * this - * method multiple times with the same key will override the previous - * assignment. - * - * @param key The key to assign with - * @param axis The axis being assigned - */ - public void assign(final Axis.Key key, final Axis axis) { - axes.put(key, axis); - } - - /** - * Retrieve the {@link Button} associated with a {@link Button.Key} - * - * @param key The key a button was assigned to - * @return The associated button - */ - public Button button(final Button.Key key) { - return buttons.get(key); - } - - /** - * Retrieve the {@link Axis} associated with a {@link Axis.KeyBase} - * - * @param key The key an axis was assigned to - * @return The associated axis - */ - public Axis axis(final Axis.Key key) { - return axes.get(key); - } -} diff --git a/org/assabet/aztechs157/input/models/LogitechAttack.java b/org/assabet/aztechs157/input/models/LogitechAttack.java deleted file mode 100644 index c097cc6..0000000 --- a/org/assabet/aztechs157/input/models/LogitechAttack.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.assabet.aztechs157.input.models; - -import org.assabet.aztechs157.input.Model; -import org.assabet.aztechs157.input.Pov; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public class LogitechAttack extends Model { - - public LogitechAttack(final int joystickId) { - super(joystickId); - } - - public final Button trigger = button(1); - public final Button button2 = button(2); - public final Button button3 = button(3); - public final Button button4 = button(4); - public final Button button5 = button(5); - public final Button button6 = button(6); - public final Button button7 = button(7); - public final Button button8 = button(8); - public final Button button9 = button(9); - public final Button button10 = button(10); - public final Button button11 = button(11); - - public final Axis stickX = axis(0); - public final Axis stickY = axis(1); - public final Axis slider = axis(3); - - public final Pov pov = pov(0); -} diff --git a/org/assabet/aztechs157/input/models/LogitechExtreme3D.java b/org/assabet/aztechs157/input/models/LogitechExtreme3D.java deleted file mode 100644 index 59a51e3..0000000 --- a/org/assabet/aztechs157/input/models/LogitechExtreme3D.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.assabet.aztechs157.input.models; - -import org.assabet.aztechs157.input.Model; -import org.assabet.aztechs157.input.Pov; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public class LogitechExtreme3D extends Model { - public LogitechExtreme3D(final int joystickId) { - super(joystickId); - } - - public final Button trigger = button(1); - public final Button thumb = button(2); - public final Button button3 = button(3); - public final Button button4 = button(4); - public final Button button5 = button(5); - public final Button button6 = button(6); - public final Button button7 = button(7); - public final Button button8 = button(8); - public final Button button9 = button(9); - public final Button button10 = button(10); - public final Button button11 = button(11); - public final Button button12 = button(12); - - public final Axis stickX = axis(0); - public final Axis stickY = axis(1); - public final Axis stickRotate = axis(2); - public final Axis slider = axis(3); - - public final Pov pov = pov(0); -} diff --git a/org/assabet/aztechs157/input/models/LogitechGamepadF310.java b/org/assabet/aztechs157/input/models/LogitechGamepadF310.java deleted file mode 100644 index 3be4af0..0000000 --- a/org/assabet/aztechs157/input/models/LogitechGamepadF310.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.assabet.aztechs157.input.models; - -import org.assabet.aztechs157.input.Model; -import org.assabet.aztechs157.input.Pov; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public class LogitechGamepadF310 extends Model { - - public LogitechGamepadF310(final int joystickId) { - super(joystickId); - } - - public final Button a = button(1); - public final Button b = button(2); - public final Button x = button(3); - public final Button y = button(4); - public final Button leftBumper = button(5); - public final Button rightBumper = button(6); - public final Button back = button(7); - public final Button start = button(8); - public final Button leftStickPress = button(9); - public final Button rightStickPress = button(10); - - public final Axis leftStickX = axis(0); - public final Axis leftStickY = axis(1); - public final Axis rightTriggerHeld = axis(2); - public final Axis leftTriggerHeld = axis(3); - public final Axis rightStickX = axis(4); - public final Axis rightStickY = axis(5); - public final Axis combinedTriggersHeld = rightTriggerHeld.offsetBy(leftTriggerHeld.inverted()::get); - - public final Pov pov = pov(0); -} diff --git a/org/assabet/aztechs157/input/models/XboxOne.java b/org/assabet/aztechs157/input/models/XboxOne.java deleted file mode 100644 index 0bc79fe..0000000 --- a/org/assabet/aztechs157/input/models/XboxOne.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.assabet.aztechs157.input.models; - -import org.assabet.aztechs157.input.Model; -import org.assabet.aztechs157.input.Pov; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; - -public class XboxOne extends Model { - - public XboxOne(final int joystickId) { - super(joystickId); - } - - public final Button a = button(1); - public final Button b = button(2); - public final Button x = button(3); - public final Button y = button(4); - public final Button leftBumper = button(5); - public final Button rightBumper = button(6); - public final Button back = button(7); - public final Button start = button(8); - public final Button leftStickPress = button(9); - public final Button rightStickPress = button(10); - - public final Axis leftStickX = axis(0); - public final Axis leftStickY = axis(1); - public final Axis leftTriggerHeld = axis(2); - public final Axis rightTriggerHeld = axis(3); - public final Axis rightStickX = axis(4); - public final Axis rightStickY = axis(5); - public final Axis combinedTriggersHeld = rightTriggerHeld.offsetBy(leftTriggerHeld.inverted()::get); - - public final Pov pov = pov(0); -} diff --git a/org/assabet/aztechs157/input/values/Axis.java b/org/assabet/aztechs157/input/values/Axis.java deleted file mode 100644 index 892cbfd..0000000 --- a/org/assabet/aztechs157/input/values/Axis.java +++ /dev/null @@ -1,97 +0,0 @@ -package org.assabet.aztechs157.input.values; - -import java.util.function.DoubleConsumer; -import java.util.function.DoubleSupplier; -import java.util.function.DoubleUnaryOperator; - -import org.assabet.aztechs157.numbers.Range; - -import edu.wpi.first.wpilibj.DriverStation; - -/** - * Class for getting input from a axis. This class has methods and static - * methods to modify and compose {@link Axis}s into a new - * {@link Axis}. - */ -public class Axis { - public static class Key { - } - - public static final Range kDeviceDefaultRange = new Range(-1, 1); - - private final DoubleSupplier value; - - public Axis(final DoubleSupplier value) { - this.value = value; - } - - public static Axis fromDriverStation(final int deviceId, final int axisId) { - return new Axis(() -> DriverStation.getStickAxis(deviceId, axisId)); - } - - public static Axis always(final double value) { - return new Axis(() -> value); - } - - public double get() { - return value.getAsDouble(); - } - - public Axis map(final DoubleUnaryOperator body) { - return new Axis(() -> body.applyAsDouble(get())); - } - - public Axis tap(final DoubleConsumer body) { - return map(value -> { - body.accept(value); - return value; - }); - } - - /** - * Inverts the input by negating the number's sign - * - * @return A new inverted input - */ - public Axis inverted() { - return map(value -> -value); - } - - /** - * Scale the input with a scalar value. - * - * @param scale The value to scale by - * @return A new input with the scale applied - */ - public Axis scaledBy(final double scale) { - return map(value -> value * scale); - } - - /** - * Scale the input with another input. - * - * @param scale The input to retrieve the scale from - * @return A new input with the scale applied - */ - public Axis scaledBy(final DoubleSupplier scale) { - return map(value -> value * scale.getAsDouble()); - } - - public Axis offsetBy(final double offset) { - return map(value -> value + offset); - } - - public Axis offsetBy(final DoubleSupplier offset) { - return map(value -> value + offset.getAsDouble()); - } - - /** - * Clamp the input to a number within the provided range. - * - * @param range The range to clamp to - * @return A new input with clamp applied - */ - public Axis clampTo(final Range range) { - return map(value -> range.clamp(value)); - } -} diff --git a/org/assabet/aztechs157/input/values/Button.java b/org/assabet/aztechs157/input/values/Button.java deleted file mode 100644 index ebd6a2d..0000000 --- a/org/assabet/aztechs157/input/values/Button.java +++ /dev/null @@ -1,131 +0,0 @@ -package org.assabet.aztechs157.input.values; - -import java.util.function.BooleanSupplier; -import java.util.function.UnaryOperator; - -import edu.wpi.first.util.function.BooleanConsumer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -/** - * Class for getting input from a button. This class has methods and static - * methods to modify and compose {@link Button}s into a new - * {@link Button}. - */ -public class Button { - public static class Key { - } - - private final BooleanSupplier value; - - public Button(final BooleanSupplier value) { - this.value = value; - } - - public static Button fromDriverStation(final int deviceId, final int buttonId) { - return new Button(() -> DriverStation.getStickButton(deviceId, buttonId)); - } - - public static Button always(final boolean value) { - return new Button(() -> value); - } - - public boolean get() { - return value.getAsBoolean(); - } - - public Button whenPressed(final Command command) { - new Trigger(value).onTrue(command); - return this; - } - - public Button whileHeld(final Command command) { - new Trigger(value).whileTrue(command); - return this; - } - - public Button toggleWhenPressed(final Command command) { - new Trigger(value).toggleOnTrue(command); - return this; - } - - public Button map(final UnaryOperator body) { - return new Button(() -> body.apply(get())); - } - - public Button tap(final BooleanConsumer body) { - return map(value -> { - body.accept(value); - return value; - }); - } - - /** - * Inverts the input; similar to a boolean `!` - * - * @return A new inverted input - */ - public Button inverted() { - return map(value -> !value); - } - - /** - * Checks that all inputs are true; similar to a boolean `&&` - * - * @param first The first input - * @param rest The rest of the inputs - * @return A new input that is only true when all of the passed inputs are true - */ - public static Button all(final Button first, final Button... rest) { - // The first argument is explicit to prevent being given empty arrays - - return new Button(() -> { - // Check each input individually - // As soon as one input is false, return false - - if (first != null && first.get() == false) { - return false; - } - - for (final var input : rest) { - if (input != null && input.get() == false) { - return false; - } - } - - // All inputs are true at this point, so return true - return true; - }); - } - - /** - * Checks that any input is true; similar to a boolean `||` - * - * @param first The first input - * @param rest The rest of the inputs - * @return A new input that is true when any of the passed inputs are true - */ - public static Button any(final Button first, final Button... rest) { - // The first argument is explicit to prevent being given empty arrays - - return new Button(() -> { - // Check each input individually - // As soon as one input is true, return true - - if (first != null && first.get()) { - return true; - } - - for (final var input : rest) { - if (input != null && input.get()) { - return true; - } - } - - // All inputs are false at this point, so return false - return false; - }); - - } -} diff --git a/org/assabet/aztechs157/numbers/Deadzone.java b/org/assabet/aztechs157/numbers/Deadzone.java deleted file mode 100644 index 62a1578..0000000 --- a/org/assabet/aztechs157/numbers/Deadzone.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.assabet.aztechs157.numbers; - -import org.assabet.aztechs157.input.values.Axis; - -public class Deadzone { - public final Range deadzone; - public final Range full; - public final RangeConverter leftConverter; - public final RangeConverter rightConverter; - - public static Deadzone forAxis(final Range deadzone) { - return new Deadzone(deadzone, Axis.kDeviceDefaultRange, 0); - } - - public Deadzone(final Range deadzone, final Range full, final double fullCenter) { - this.deadzone = deadzone; - this.full = full; - - final var leftFull = new Range(full.start(), fullCenter); - final var leftDeadzone = new Range(full.start(), deadzone.start()); - this.leftConverter = new RangeConverter(leftDeadzone, leftFull); - - final var rightFull = new Range(fullCenter, full.end()); - final var rightDeadzone = new Range(deadzone.end(), full.end()); - this.rightConverter = new RangeConverter(rightDeadzone, rightFull); - } - - public double apply(final double input) { - if (deadzone.contains(input)) { - return 0; - } else if (leftConverter.inputRange.contains(input)) { - return leftConverter.convert(input); - } else if (rightConverter.inputRange.contains(input)) { - return rightConverter.convert(input); - } - - throw new Error("Attempted to apply deadzone to input outside of full range " - + full.start() + " to " + full.end()); - } -} diff --git a/org/assabet/aztechs157/numbers/Range.java b/org/assabet/aztechs157/numbers/Range.java deleted file mode 100644 index a69bb32..0000000 --- a/org/assabet/aztechs157/numbers/Range.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.assabet.aztechs157.numbers; - -public record Range(double start, double end) { - - public boolean contains(final double value) { - return start <= value && value <= end; - } - - public double range() { - return end - start; - } - - public double clamp(final double value) { - if (value < start) { - return start; - } else if (value > end) { - return end; - } else { - return value; - } - } - - public double limitMotionWithinRange(final double speed, final double currentPosition) { - if (speed > 0 && currentPosition > end) { - return 0; - } else if (speed < 0 && currentPosition < start) { - return 0; - } else { - return speed; - } - } - - public RangeConverter convertingTo(final Range output) { - return new RangeConverter(this, output); - } -} diff --git a/org/assabet/aztechs157/numbers/RangeConverter.java b/org/assabet/aztechs157/numbers/RangeConverter.java deleted file mode 100644 index edb424e..0000000 --- a/org/assabet/aztechs157/numbers/RangeConverter.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.assabet.aztechs157.numbers; - -public class RangeConverter { - public final Range inputRange; - public final Range outputRange; - public final double scaleFactor; - - public RangeConverter(final Range inputRange, final Range outputRange) { - this.inputRange = inputRange; - this.outputRange = outputRange; - this.scaleFactor = outputRange.range() / inputRange.range(); - } - - public double convert(final double inputValue) { - // Shift to zero based input range - final var basedInput = inputValue - inputRange.start(); - - // Scale the zero based input - final var scaled = basedInput * scaleFactor; - - // Shift from zero based to output range - final var outputValue = scaled + outputRange.start(); - - return outputValue; - } -} diff --git a/pathplanner/autos/1m backward auto.auto b/pathplanner/autos/1m backward auto.auto deleted file mode 100644 index e988db7..0000000 --- a/pathplanner/autos/1m backward auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m backward" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/pathplanner/autos/1m forward & backwards x25.auto b/pathplanner/autos/1m forward & backwards x25.auto deleted file mode 100644 index e2f253c..0000000 --- a/pathplanner/autos/1m forward & backwards x25.auto +++ /dev/null @@ -1,169 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 2.0, - "y": 7.0 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - }, - { - "type": "path", - "data": { - "pathName": "1m Back and forth" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/pathplanner/autos/1m forward & backwards.auto b/pathplanner/autos/1m forward & backwards.auto deleted file mode 100644 index 79adb64..0000000 --- a/pathplanner/autos/1m forward & backwards.auto +++ /dev/null @@ -1,302 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - },{ - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m forward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "1m backward" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/pathplanner/autos/1m forward auto.auto b/pathplanner/autos/1m forward auto.auto deleted file mode 100644 index de1be04..0000000 --- a/pathplanner/autos/1m forward auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m forward" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/pathplanner/autos/1m left auto.auto b/pathplanner/autos/1m left auto.auto deleted file mode 100644 index 286b393..0000000 --- a/pathplanner/autos/1m left auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m left" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/pathplanner/autos/1m right auto.auto b/pathplanner/autos/1m right auto.auto deleted file mode 100644 index 69e6f18..0000000 --- a/pathplanner/autos/1m right auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1m right" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/pathplanner/navgrid.json b/pathplanner/navgrid.json deleted file mode 100644 index bab0da9..0000000 --- a/pathplanner/navgrid.json +++ /dev/null @@ -1 +0,0 @@ -{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/pathplanner/paths/1m Back and forth.path b/pathplanner/paths/1m Back and forth.path deleted file mode 100644 index add093c..0000000 --- a/pathplanner/paths/1m Back and forth.path +++ /dev/null @@ -1,103 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.1, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0, - "y": 7.0 - }, - "prevControl": { - "x": 2.9, - "y": 7.0 - }, - "nextControl": { - "x": 3.1, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": { - "x": 1.9, - "y": 7.0 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ - { - "name": "Wait", - "waypointRelativePos": 1.0, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - } - ] - } - } - }, - { - "name": "Wait 2", - "waypointRelativePos": 0, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/pathplanner/paths/1m backward.path b/pathplanner/paths/1m backward.path deleted file mode 100644 index fa735aa..0000000 --- a/pathplanner/paths/1m backward.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 5.59 - }, - "prevControl": null, - "nextControl": { - "x": 2.1, - "y": 5.59 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0, - "y": 5.59 - }, - "prevControl": { - "x": 1.1, - "y": 5.59 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/pathplanner/paths/1m forward.path b/pathplanner/paths/1m forward.path deleted file mode 100644 index 544f728..0000000 --- a/pathplanner/paths/1m forward.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.0, - "y": 5.589805823486292 - }, - "prevControl": null, - "nextControl": { - "x": 1.9999999999999967, - "y": 5.589805823486292 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.0, - "y": 5.589805823486292 - }, - "prevControl": { - "x": 1.0, - "y": 5.589805823486292 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/pathplanner/paths/1m left.path b/pathplanner/paths/1m left.path deleted file mode 100644 index 1482a76..0000000 --- a/pathplanner/paths/1m left.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.0, - "y": 5.59 - }, - "prevControl": null, - "nextControl": { - "x": 1.0, - "y": 6.502346398657132 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0, - "y": 6.59 - }, - "prevControl": { - "x": 0.9999999999999999, - "y": 5.59 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/pathplanner/paths/1m right.path b/pathplanner/paths/1m right.path deleted file mode 100644 index 88a7b65..0000000 --- a/pathplanner/paths/1m right.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.0, - "y": 5.59 - }, - "prevControl": null, - "nextControl": { - "x": 1.0, - "y": 5.49 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0, - "y": 4.59 - }, - "prevControl": { - "x": 1.0, - "y": 4.54 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/pathplanner/paths/Stephen.path b/pathplanner/paths/Stephen.path deleted file mode 100644 index 9126da3..0000000 --- a/pathplanner/paths/Stephen.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.439971575099845, - "y": 5.028486410249174 - }, - "prevControl": { - "x": 7.182700177366168, - "y": 6.2563726267053665 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -33.02386755579669, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/pathplanner/paths/test.path b/pathplanner/paths/test.path deleted file mode 100644 index 16dc425..0000000 --- a/pathplanner/paths/test.path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.200391204201211, - "y": 6.572114796651245 - }, - "prevControl": { - "x": 5.200391204201211, - "y": 6.572114796651245 - }, - "nextControl": { - "x": 7.200391204201211, - "y": 6.572114796651245 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.486748192869604, - "y": 4.654273468091096 - }, - "prevControl": { - "x": 7.694394331808605, - "y": 5.256522421876753 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0292, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/robot/Constants.java b/robot/Constants.java deleted file mode 100644 index e452735..0000000 --- a/robot/Constants.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot; - -public class Constants { - public static class ControllerConstants { - - public static final int DRIVER_CONTROLLER_PORT = 0; - public static final int OPERATOR_CONTROLLER_PORT = 1; - - // Joystick Deadband - public static final double LEFT_X_DEADBAND = 0.1; - public static final double LEFT_Y_DEADBAND = 0.1; - public static final double RIGHT_X_DEADBAND = 0.1; - } - - public static class LoggingConstants { - - public static final boolean DEFAULT_LOGGING_STATE = false; - } - - - public static class CosmeticConstants { - - public static final int LIGHT_ID = 9; - public static final double SOLID_YELLOW_VALUE = 0.69; - public static final double SOLID_PURPLE_VALUE = 0.91; - public static final int LIGHT_LENGTH = 76; - - } - -} diff --git a/robot/Inputs.java b/robot/Inputs.java deleted file mode 100644 index 26d5bfa..0000000 --- a/robot/Inputs.java +++ /dev/null @@ -1,98 +0,0 @@ -// 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. - -package frc.robot; - -import org.assabet.aztechs157.input.layouts.DynamicLayout; -import org.assabet.aztechs157.input.layouts.Layout; -import org.assabet.aztechs157.input.layouts.MapLayout; -import java.util.function.DoubleSupplier; -import org.assabet.aztechs157.input.models.XboxOne; -import org.assabet.aztechs157.input.values.Axis; -import org.assabet.aztechs157.input.values.Button; -import org.assabet.aztechs157.numbers.Deadzone; -import org.assabet.aztechs157.numbers.Range; - -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import frc.robot.Constants.ControllerConstants; - -/** Add your docs here. */ -public class Inputs extends DynamicLayout { - - public static final Axis.Key precisionDrive = new Axis.Key(); - - ////////////////////////////////////////////////////////// - // HERE'S AN EXAMPLE OF USING ADDING NEW BUTTONS/AXIS - ///////////////////////////////////////////////////////// - - // public static final Button.Key resetGyro = new Button.Key(); - - // public static final Button.Key driveToSpeaker = new Button.Key(); - // public static final Button.Key driveToAmp = new Button.Key(); - // public static final Button.Key autoIntake = new Button.Key(); - - // public static final Button.Key intake = new Button.Key(); - // public static final Button.Key loadNote = new Button.Key(); - // public static final Button.Key eject = new Button.Key(); - - // public static final Button.Key highShotSpinUp = new Button.Key(); - // public static final Button.Key lowShotSpinUp = new Button.Key(); - - // public static final Button.Key highShot = new Button.Key(); - // public static final Button.Key lowShot = new Button.Key(); - // public static final Button.Key pass = new Button.Key(); - - // public static final Button.Key liftHanger = new Button.Key(); - // public static final Button.Key retractHanger = new Button.Key(); - // public static final Button.Key retractHangerPin = new Button.Key(); - // public static final Button.Key extendHangerPin = new Button.Key(); - - public static Inputs createFromChooser() { - final SendableChooser chooser = new SendableChooser<>(); - chooser.setDefaultOption("xbox", doubleXBOXLayout()); - Shuffleboard.getTab("Driver").add("Layout Choose", chooser); - - return new Inputs(chooser); - } - - private Inputs(final SendableChooser chooser) { - super(chooser::getSelected); - } - - private static Layout doubleXBOXLayout() { - - final var layout = new MapLayout(); - final var driver = new XboxOne(ControllerConstants.DRIVER_CONTROLLER_PORT); - final var operator = new XboxOne(ControllerConstants.OPERATOR_CONTROLLER_PORT); - - layout.assign(precisionDrive, driver.leftTriggerHeld.map(Deadzone.forAxis(new Range(0.0, 0.05))::apply).scaledBy(0.7)); - - ////////////////////////////////////////////////////////// - // HERE'S AN EXAMPLE OF USING CONTROLER LAYOUT OPTIONS - ///////////////////////////////////////////////////////// - - // layout.assign(driveToSpeaker, operator.a); - // layout.assign(driveToAmp, operator.b); - // layout.assign(resetGyro, driver.start); - // layout.assign(autoIntake, operator.leftBumper); - - // layout.assign(intake, driver.leftBumper); - // layout.assign(loadNote, operator.x); - - // layout.assign(highShotSpinUp, operator.rightBumper); - // layout.assign(lowShotSpinUp, operator.leftBumper); - - // layout.assign(highShot, new Button(() -> driver.rightTriggerHeld.get() > 0.2)); - // layout.assign(lowShot, driver.rightBumper); - // layout.assign(pass, operator.a); - // layout.assign(eject, operator.b); - - // layout.assign(liftHanger, operator.pov.up); - // layout.assign(retractHanger, operator.pov.down); - - return layout; - } - -} diff --git a/robot/Main.java b/robot/Main.java deleted file mode 100644 index fe215d7..0000000 --- a/robot/Main.java +++ /dev/null @@ -1,15 +0,0 @@ -// 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. - -package frc.robot; - -import edu.wpi.first.wpilibj.RobotBase; - -public final class Main { - private Main() {} - - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} diff --git a/robot/Robot.java b/robot/Robot.java deleted file mode 100644 index 0556048..0000000 --- a/robot/Robot.java +++ /dev/null @@ -1,76 +0,0 @@ -// 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. - -package frc.robot; - -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; - -public class Robot extends TimedRobot { - private Command m_autonomousCommand; - - private RobotContainer m_robotContainer; - - @Override - public void robotInit() { - m_robotContainer = new RobotContainer(); - } - - @Override - public void robotPeriodic() { - CommandScheduler.getInstance().run(); - } - - @Override - public void disabledInit() {} - - @Override - public void disabledPeriodic() {} - - @Override - public void disabledExit() {} - - @Override - public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - } - - @Override - public void autonomousPeriodic() {} - - @Override - public void autonomousExit() {} - - @Override - public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } - } - - @Override - public void teleopPeriodic() {} - - @Override - public void teleopExit() {} - - @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); - } - - @Override - public void testPeriodic() {} - - @Override - public void testExit() {} - - @Override - public void simulationPeriodic() {} -} diff --git a/robot/RobotContainer.java b/robot/RobotContainer.java deleted file mode 100644 index 5718dfb..0000000 --- a/robot/RobotContainer.java +++ /dev/null @@ -1,130 +0,0 @@ -// 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. - -package frc.robot; - -import java.util.HashMap; -import java.util.Map; - -import org.assabet.aztechs157.input.layouts.Layout; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.Orchestra; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import com.pathplanner.lib.auto.AutoBuilder; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; - -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.DriveSystem; -import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.data.LoggingSystem; - -public class RobotContainer { - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - private Map SystemMap = new HashMap(); - - private final CommandXboxController joystick = new CommandXboxController(0); // My joystick - public final DriveSystem drivetrain = TunerConstants.createDrivetrain(); // My drivetrain - - {SystemMap.put("Drive",drivetrain);} - private final Layout inputs = Inputs.createFromChooser(); - private final LoggingSystem loggingSystem = new LoggingSystem(SystemMap); - private final SendableChooser autoChooser; - -/* Setting up bindings for necessary control of the swerve drive platform */ -private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() -.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband -.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors -private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); -private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); -private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric() -.withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - private final Telemetry logger = new Telemetry(MaxSpeed); - - // Slew Rate Limiters to limit acceleration of joystick inputs - // private final SlewRateLimiter xLimiter = new SlewRateLimiter(25); - // private final SlewRateLimiter yLimiter = new SlewRateLimiter(25); - // private final SlewRateLimiter rotLimiter = new SlewRateLimiter(1570); - - private Orchestra soundSystem = new Orchestra(); - - public RobotContainer() { - configureBindings(); - - autoChooser = AutoBuilder.buildAutoChooser("NothingAuto"); - SmartDashboard.putData("Auto Chooser", autoChooser); - for(int i = 0; i<4;i++){ - soundSystem.addInstrument(drivetrain.getModule(i).getDriveMotor(), 0); - soundSystem.addInstrument(drivetrain.getModule(i).getSteerMotor(), 1); - } - soundSystem.loadMusic("music/e1m1.chrp"); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // An example command will be run in autonomous - return autoChooser.getSelected(); - } - - public double modifySpeed(final double speed) { - final var modifier = 1 - inputs.axis(Inputs.precisionDrive).get(); - return speed * modifier; -} - - private void configureBindings() { - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain.applyRequest(() -> drive.withVelocityX(-joystick.getLeftY() /* TODO: check inversion */ * modifySpeed(MaxSpeed)) // Drive forward with - // negative Y (forward) - .withVelocityY(-joystick.getLeftX() /* TODO: check inversion */ * modifySpeed(MaxSpeed)) // Drive left with negative X (left) - .withRotationalRate(-joystick.getRightX() /* TODO: check inversion */ * MaxAngularRate) // Drive counterclockwise with negative X (left) - )); - - joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); - joystick.b().whileTrue(drivetrain.applyRequest(() -> - point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())) - )); - - joystick.pov(0).whileTrue(drivetrain.applyRequest(() -> - forwardStraight.withVelocityX(0.5).withVelocityY(0)) - ); - joystick.pov(180).whileTrue(drivetrain.applyRequest(() -> - forwardStraight.withVelocityX(-0.5).withVelocityY(0)) - ); - - // Run SysId routines when holding back/start and X/Y. - // Note that each routine should be run exactly once in a single log. - joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); - joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); - - // reset the field-centric heading on left bumper press - joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - - joystick.start().onTrue(drivetrain.runOnce(()-> {soundSystem.play();})); - joystick.back().onTrue(drivetrain.runOnce(()->{soundSystem.pause();})); - joystick.y().onTrue(drivetrain.runOnce(()->{soundSystem.stop();})); - - drivetrain.registerTelemetry(logger::telemeterize); - } - -} diff --git a/robot/Telemetry.java b/robot/Telemetry.java deleted file mode 100644 index a2d36dd..0000000 --- a/robot/Telemetry.java +++ /dev/null @@ -1,124 +0,0 @@ -package frc.robot; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -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.networktables.StructArrayPublisher; -import edu.wpi.first.networktables.StructPublisher; -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 Telemetry { - 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 Telemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - SignalLogger.start(); - } - - /* What to publish over networktables for telemetry */ - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - - /* Robot swerve drive state */ - private final NetworkTable driveStateTable = inst.getTable("DriveState"); - private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); - private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); - private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); - private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); - private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); - - /* 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(); - - /* 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))), - }; - - private final double[] m_poseArray = new double[3]; - private final double[] m_moduleStatesArray = new double[8]; - private final double[] m_moduleTargetsArray = new double[8]; - - /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the swerve drive state */ - drivePose.set(state.Pose); - driveSpeeds.set(state.Speeds); - driveModuleStates.set(state.ModuleStates); - driveModuleTargets.set(state.ModuleTargets); - driveModulePositions.set(state.ModulePositions); - driveTimestamp.set(state.Timestamp); - driveOdometryFrequency.set(1.0 / state.OdometryPeriod); - - /* Also write to log file */ - m_poseArray[0] = state.Pose.getX(); - m_poseArray[1] = state.Pose.getY(); - m_poseArray[2] = state.Pose.getRotation().getDegrees(); - for (int i = 0; i < 4; ++i) { - m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians(); - m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond; - m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians(); - m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; - } - - SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); - SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); - SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); - SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - - /* Telemeterize the pose to a Field2d */ - fieldTypePub.set("Field2d"); - fieldPub.set(m_poseArray); - - /* Telemeterize the module states to a Mechanism2d */ - 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]); - } - } -} \ No newline at end of file diff --git a/robot/cosmetics/BlinkInLEDs.java b/robot/cosmetics/BlinkInLEDs.java deleted file mode 100644 index f2241f0..0000000 --- a/robot/cosmetics/BlinkInLEDs.java +++ /dev/null @@ -1,35 +0,0 @@ -// 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. - -package frc.robot.cosmetics; - -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.CosmeticConstants; - -// We are no longer using this file to run any code, it is merely left as a reference -public class BlinkInLEDs extends SubsystemBase { - public final Spark lightController = new Spark(CosmeticConstants.LIGHT_ID); - // private ShuffleboardTab tab = Shuffleboard.getTab("LED"); - // private GenericEntry lightColor = tab.add("led color", - // CosmeticConstants.SOLID_YELLOW_VALUE).getEntry(); - - /** Creates a new lights. */ - public BlinkInLEDs() { - } - - public void setYellow() { - lightController.set(CosmeticConstants.SOLID_YELLOW_VALUE); - } - - public void setPurple() { - lightController.set(CosmeticConstants.SOLID_PURPLE_VALUE); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - // lightController.set(lightColor.getDouble(CosmeticConstants.SOLID_YELLOW_VALUE)); - } -} diff --git a/robot/cosmetics/PwmLEDs.java b/robot/cosmetics/PwmLEDs.java deleted file mode 100644 index acb2796..0000000 --- a/robot/cosmetics/PwmLEDs.java +++ /dev/null @@ -1,249 +0,0 @@ -// 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. - -package frc.robot.cosmetics; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.CosmeticConstants; - -public class PwmLEDs extends SubsystemBase { - private final AddressableLED lights = new AddressableLED(CosmeticConstants.LIGHT_ID); - private AddressableLEDBuffer buffer = new AddressableLEDBuffer(CosmeticConstants.LIGHT_LENGTH); - private Color color1 = Color.kBlack; - private Color color2 = Color.kBlack; - private double onLength = 0.0; - private double offLength = 0.0; - private int color1Length = 0; - private int color2Length = 0; - private double speed = 0; - private double cycleLength = 0.0; - private double duration = 0.0; - - private double mp1 = 90.0; - private double mp2 = 60.0; - private double mp3 = 30.0; - private double mp4 = 15.0; - private double mp5 = 10.0; - private double mp6 = 5.0; - private double mpTolerance = 0.5; - - private Mode lightMode = Mode.SOLID; - - public void setLightMode(Mode lightMode) { - this.lightMode = lightMode; - } - - public Color getColor1() { - return color1; - } - - public void setColor1(Color color1) { - this.color1 = color1; - } - - public Color getColor2() { - return color2; - } - - public void setColor2(Color color2) { - this.color2 = color2; - } - - public void setColor1Length(int color1Length) { - this.color1Length = color1Length; - } - - public void setColor2Length(int color2Length) { - this.color2Length = color2Length; - } - - public void setSpeed(double speed) { - this.speed = speed; - } - - public static enum Mode { - SOLID, WAVE, CLIMB, STROBE; - } - - /** Creates a new PwmLEDs. */ - public PwmLEDs() { - lights.setLength(CosmeticConstants.LIGHT_LENGTH); - lights.setData(buffer); - lights.start(); - } - - public void solid(Color color) { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setLED(i, color); - } - } - - public void setSolid(Color color) { - this.color1 = color; - this.lightMode = Mode.SOLID; - } - - public void wave(Color color1, Color color2, double cycleLength, double duration) { - double counter = (1 - ((Timer.getFPGATimestamp() % duration) / duration)) * 2.0 * Math.PI; - double counterDiffPerLed = (2.0 * Math.PI) / cycleLength; - for (int i = 0; i < buffer.getLength(); i++) { - counter += counterDiffPerLed; - if (i >= 0) { - double ratio = (Math.pow(Math.sin(counter), 0.4) + 1.0) / 2.0; - if (Double.isNaN(ratio)) { - ratio = (-Math.pow(Math.sin(counter + Math.PI), 0.4) + 1.0) / 2.0; - } - if (Double.isNaN(ratio)) { - ratio = 0.5; - } - double red = (color1.red * (1 - ratio)) + (color2.red * ratio); - double green = (color1.green * (1 - ratio)) + (color2.green * ratio); - double blue = (color1.blue * (1 - ratio)) + (color2.blue * ratio); - buffer.setLED(i, new Color(red, green, blue)); - } - } - } - - public void wave(Color color, double cycleLength, double duration) { - wave(color, Color.kBlack, cycleLength, duration); - } - - public void setWave(Color color1, Color color2, double cycleLength, double duration) { - this.color1 = color1; - this.color2 = color2; - this.cycleLength = cycleLength; - this.duration = duration; - this.lightMode = Mode.WAVE; - } - - public void climb(Color color1, Color color2, int color1Length, int color2Length, double speed) { - int counter = (int) Math.floor(Timer.getFPGATimestamp() * speed); - for (int i = 0; i < buffer.getLength(); i += color2Length + color1Length) { - for (int j = 0; j < color1Length; j++) { - buffer.setLED((i + j + counter) % buffer.getLength(), color1); - } - for (int j = color1Length; j < color1Length + color2Length; j++) { - buffer.setLED((i + j + counter) % buffer.getLength(), color2); - } - } - } - - public void climb(Color color, int colorLength, int offLength, double speed) { - climb(color, Color.kBlack, colorLength, offLength, speed); - } - - public void setClimb(Color color1, Color color2, int color1Length, int color2Length, double speed) { - this.color1 = color1; - this.color2 = color2; - this.color1Length = color1Length; - this.color2Length = color2Length; - this.speed = speed; - this.lightMode = Mode.CLIMB; - } - - public void strobe(Color color1, Color color2, double onLength, double offLength) { - boolean lightsOn = Timer.getFPGATimestamp() % onLength + offLength > onLength; - if (lightsOn) { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setLED(i, color1); - } - } else { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setLED(i, color2); - } - } - } - - public void strobe(Color color, double onLength, double offLength) { - strobe(color, Color.kBlack, onLength, offLength); - } - - public void setStrobe(Color color1, Color color2, double onLength, double offLength) { - this.color1 = color1; - this.color2 = color2; - this.onLength = onLength; - this.offLength = offLength; - this.lightMode = Mode.STROBE; - } - - public void setDefault() { - Color color1 = Color.kBlue; - Color color2 = Color.kGold; - if (DriverStation.isEStopped()) { - color1 = Color.kDarkGreen; - color2 = Color.kPowderBlue; - } - if (!DriverStation.isFMSAttached()) { - color1 = PwmLEDs.dimColor(color1, 0.25); - color2 = PwmLEDs.dimColor(color2, 0.25); - } - - setWave(color1, color2, 10, 3); - } - - public static Color dimColor(Color color, double brightness) { - return new Color(color.red * brightness, color.green * brightness, color.blue * brightness); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - - double time = Timer.getMatchTime(); - if (Math.abs(time - mp1) <= mpTolerance) { - solid(Color.kBlue); - lights.setData(buffer); - return; - } - if (Math.abs(time - mp2) <= mpTolerance) { - solid(Color.kGreen); - lights.setData(buffer); - return; - } - if (Math.abs(time - mp3) <= mpTolerance) { - solid(Color.kFirstRed); - lights.setData(buffer); - return; - } - if (time < mp4 && time > mp5 && time % 1.0 > 0.5) { - solid(dimColor(Color.kDarkGoldenrod, 0.5)); - lights.setData(buffer); - return; - } - if (time < mp5 && time > mp6 && time % 1.0 > 0.5) { - solid(dimColor(Color.kFirstBlue, 0.5)); - lights.setData(buffer); - return; - } - if (time < mp6 && time % 1.0 > 0.5) { - solid(dimColor(Color.kFirstRed, 0.5)); - lights.setData(buffer); - return; - } - - switch (lightMode) { - case SOLID: - solid(color1); - break; - case WAVE: - wave(color1, color2, cycleLength, duration); - break; - case CLIMB: - climb(color1, color2, color1Length, color2Length, speed); - break; - case STROBE: - strobe(color1, color2, onLength, offLength); - break; - default: - solid(Color.kBlack); - } - - lights.setData(buffer); - } -} diff --git a/robot/data/LoggingSystem.java b/robot/data/LoggingSystem.java deleted file mode 100644 index 5c21203..0000000 --- a/robot/data/LoggingSystem.java +++ /dev/null @@ -1,61 +0,0 @@ -// 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. - -package frc.robot.data; - -import java.util.Dictionary; -import java.util.Map; - -import com.ctre.phoenix6.SignalLogger; - -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.LoggingConstants; -import frc.robot.subsystems.DriveSystem; - -public class LoggingSystem extends SubsystemBase { - - private Map subsystemArray; - private boolean loggingState = LoggingConstants.DEFAULT_LOGGING_STATE; - private GenericEntry loggingChooser; - - - /** Creates a new LoggingSystem. */ - public LoggingSystem(Map subsystemArray) { - loggingChooser = Shuffleboard.getTab("Logging").add("Enable Logging", false).getEntry(); - SmartDashboard.putBoolean("Logging Chooser", false); - this.subsystemArray = subsystemArray; - } - - public boolean getLoggingState() { - return loggingState; - } - - private boolean getLoggingFlag() { - // An example command will be run in autonomous - return loggingChooser.getBoolean(false); // TODO: Actually use this value -} - - @Override - public void periodic() { - // This method will be called once per scheduler run - if (getLoggingFlag() != loggingState) { - loggingState = getLoggingFlag(); - - if (loggingState) { - SignalLogger.start(); - // Log the odometry pose as a double array - } else { - SignalLogger.stop(); - } - } - if (loggingState){ - var pose = ((DriveSystem)subsystemArray.get(new String("Drive"))).getState().Pose; - var status = SignalLogger.writeDoubleArray("odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); - } - } -} diff --git a/robot/generated/TunerConstants.java b/robot/generated/TunerConstants.java deleted file mode 100644 index cbbbb05..0000000 --- a/robot/generated/TunerConstants.java +++ /dev/null @@ -1,286 +0,0 @@ -package frc.robot.generated; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.hardware.*; -import com.ctre.phoenix6.signals.*; -import com.ctre.phoenix6.swerve.*; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.*; - -import frc.robot.subsystems.DriveSystem; - -// Generated by the Tuner X Swerve Project Generator -// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html -public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) // 100, 0, 0.5 - .withKS(0.1).withKV(1.91).withKA(0) // 0, 1.5, 0 - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) // 3, 0, 0 - .withKS(0).withKV(0.124); // removed KA changed and KV from 0 - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); // 150 - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) - ); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); // Default Name - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); // 5 - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; // 3.5714285714285716 - - private static final double kDriveGearRatio = 7.363636363636365; // 6.122448979591837 - private static final double kSteerGearRatio = 15.42857142857143; // 12.8 - private static final Distance kWheelRadius = Inches.of(2.167); // 1.92 - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 13; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); // 0.00001 - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); // 0.001 - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); // 0.25 - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); // 0.25 - - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - - // Front Left - private static final int kFrontLeftDriveMotorId = 11; - private static final int kFrontLeftSteerMotorId = 12; - private static final int kFrontLeftEncoderId = 10; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); // -0.052734375 - private static final boolean kFrontLeftSteerMotorInverted = true; // false - private static final boolean kFrontLeftEncoderInverted = false; // new - - private static final Distance kFrontLeftXPos = Inches.of(10); // 9.25 - private static final Distance kFrontLeftYPos = Inches.of(10); // 9.25 - - // Front Right - private static final int kFrontRightDriveMotorId = 2; - private static final int kFrontRightSteerMotorId = 3; - private static final int kFrontRightEncoderId = 1; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); // -0.6796875 - private static final boolean kFrontRightSteerMotorInverted = true; // false - private static final boolean kFrontRightEncoderInverted = false; // new - - private static final Distance kFrontRightXPos = Inches.of(10); // 9.25 - private static final Distance kFrontRightYPos = Inches.of(-10); // -9.25 - - // Back Left - private static final int kBackLeftDriveMotorId = 8; - private static final int kBackLeftSteerMotorId = 9; - private static final int kBackLeftEncoderId = 7; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); // -0.304931640625 - private static final boolean kBackLeftSteerMotorInverted = true; // false - private static final boolean kBackLeftEncoderInverted = false; // new - - private static final Distance kBackLeftXPos = Inches.of(-10); // -9.25 - private static final Distance kBackLeftYPos = Inches.of(10); // 9.25 - - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 6; - private static final int kBackRightEncoderId = 4; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); // -0.032470703125 - private static final boolean kBackRightSteerMotorInverted = true; // false - private static final boolean kBackRightEncoderInverted = false; // new - - private static final Distance kBackRightXPos = Inches.of(-10); // -9.25 - private static final Distance kBackRightYPos = Inches.of(-10); // -9.25 - - - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted - ); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted - ); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted - ); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted - ); - - /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. - */ - public static DriveSystem createDrivetrain() { - return new DriveSystem( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ); - } - - - /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. - */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules - ); - } - } -} \ No newline at end of file diff --git a/robot/subsystems/DriveSystem.java b/robot/subsystems/DriveSystem.java deleted file mode 100644 index 3447342..0000000 --- a/robot/subsystems/DriveSystem.java +++ /dev/null @@ -1,293 +0,0 @@ -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.*; - -import java.util.function.Supplier; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -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.generated.TunerConstants.TunerSwerveDrivetrain; - -/** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements - * Subsystem so it can easily be used in command-based projects. - */ -public class DriveSystem extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; - - /** Swerve request to apply during robot-centric path following */ - private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this - ) - ); - - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public DriveSystem( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public DriveSystem( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public DriveSystem( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - private void configureAutoBuilder() { - try { - var config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( - () -> getState().Pose, // Supplier of current robot pose - this::resetPose, // Consumer for seeding pose against auto - () -> getState().Speeds, // Supplier of current robot speeds - // Consumer of ChassisSpeeds and feedforwards to drive the robot - (speeds, feedforwards) -> setControl( - m_pathApplyRobotSpeeds.withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) - ), - new PPHolonomicDriveController( - // PID constants for translation - new PIDConstants(10, 0, 0), // 5, 0, 0 - // PID constants for rotation - new PIDConstants(7, 0, 0) // 5, 0, 0 - ), - config, - // Assume the path needs to be flipped for Red vs Blue, this is normally the case - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this // Subsystem for requirements - ); - } catch (Exception ex) { - DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); - } - } - - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - - @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 (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); - m_hasAppliedOperatorPerspective = true; - }); - } - } - - 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); - } -} \ No newline at end of file diff --git a/deploy/example.txt b/src/main/deploy/example.txt similarity index 100% rename from deploy/example.txt rename to src/main/deploy/example.txt diff --git a/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/Backward.auto similarity index 59% rename from deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/Backward.auto index 9e7a7f3..6f21695 100644 --- a/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/Backward.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 2.0, - "y": 7.0 - }, - "rotation": 0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -14,12 +7,13 @@ { "type": "path", "data": { - "pathName": "test" + "pathName": "Backward" } } ] } }, + "resetOdom": true, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Box.auto b/src/main/deploy/pathplanner/autos/Box.auto new file mode 100644 index 0000000..4f78a1b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Box.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Forward" + } + }, + { + "type": "path", + "data": { + "pathName": "Left" + } + }, + { + "type": "path", + "data": { + "pathName": "Backward" + } + }, + { + "type": "path", + "data": { + "pathName": "Right" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/Left.auto similarity index 59% rename from pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/Left.auto index 9e7a7f3..33f3c3c 100644 --- a/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/Left.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 2.0, - "y": 7.0 - }, - "rotation": 0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -14,12 +7,13 @@ { "type": "path", "data": { - "pathName": "test" + "pathName": "Left" } } ] } }, + "resetOdom": true, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/Right.auto similarity index 59% rename from main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/Right.auto index 9e7a7f3..717dba6 100644 --- a/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/Right.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 2.0, - "y": 7.0 - }, - "rotation": 0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -14,12 +7,13 @@ { "type": "path", "data": { - "pathName": "test" + "pathName": "Right" } } ] } }, + "resetOdom": true, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/deploy/pathplanner/autos/1m left auto.auto b/src/main/deploy/pathplanner/autos/Straight.auto similarity index 58% rename from deploy/pathplanner/autos/1m left auto.auto rename to src/main/deploy/pathplanner/autos/Straight.auto index 286b393..3a27fcd 100644 --- a/deploy/pathplanner/autos/1m left auto.auto +++ b/src/main/deploy/pathplanner/autos/Straight.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0, - "y": 5.59 - }, - "rotation": 0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -14,12 +7,13 @@ { "type": "path", "data": { - "pathName": "1m left" + "pathName": "Forward" } } ] } }, + "resetOdom": true, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Backward.path similarity index 66% rename from main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/Backward.path index f745d1b..cc5d65b 100644 --- a/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Backward.path @@ -1,14 +1,14 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { - "x": 2.0, + "x": 5.0, "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 3.0, + "x": 4.0, "y": 7.0 }, "isLocked": false, @@ -17,11 +17,11 @@ { "anchor": { "x": 4.0, - "y": 6.0 + "y": 7.0 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 5.0, + "y": 7.0 }, "nextControl": null, "isLocked": false, @@ -30,23 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0292, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Forward.path similarity index 66% rename from deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/Forward.path index f745d1b..7b84c0a 100644 --- a/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Forward.path @@ -1,26 +1,26 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 4.0, + "y": 6.0 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 5.0, + "y": 6.0 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.0, + "x": 5.0, "y": 6.0 }, "prevControl": { - "x": 3.0, + "x": 4.0, "y": 6.0 }, "nextControl": null, @@ -30,23 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0292, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/main/deploy/pathplanner/paths/Stephen.path b/src/main/deploy/pathplanner/paths/Left.path similarity index 61% rename from main/deploy/pathplanner/paths/Stephen.path rename to src/main/deploy/pathplanner/paths/Left.path index 9126da3..57f588d 100644 --- a/main/deploy/pathplanner/paths/Stephen.path +++ b/src/main/deploy/pathplanner/paths/Left.path @@ -1,27 +1,27 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 5.0, + "y": 6.0 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 5.0, + "y": 6.343420974730547 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.439971575099845, - "y": 5.028486410249174 + "x": 5.0, + "y": 7.0 }, "prevControl": { - "x": 7.182700177366168, - "y": 6.2563726267053665 + "x": 5.0, + "y": 6.0 }, "nextControl": null, "isLocked": false, @@ -30,23 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0292, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -33.02386755579669, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Right.path similarity index 69% rename from pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/Right.path index f745d1b..cc0c7e6 100644 --- a/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Right.path @@ -1,15 +1,15 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { - "x": 2.0, + "x": 4.0, "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 4.0, + "y": 6.0 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 6.0 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 4.0, + "y": 7.0 }, "nextControl": null, "isLocked": false, @@ -30,23 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0292, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..9dce420 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,34 @@ +{ + "robotWidth": 0.787, + "robotLength": 0.787, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 58.1, + "robotMOI": 5.332, + "robotTrackwidth": 0.5588, + "driveWheelRadius": 0.0483, + "driveGearing": 5.14, + "maxDriveSpeed": 4.7, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 120.0, + "wheelCOF": 1.7, + "flModuleX": 0.2286, + "flModuleY": 0.229, + "frModuleX": 0.229, + "frModuleY": -0.229, + "blModuleX": -0.229, + "blModuleY": 0.229, + "brModuleX": -0.229, + "brModuleY": -0.229, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [ + "{\"name\":\"Circle\",\"type\":\"circle\",\"data\":{\"center\":{\"x\":0.0,\"y\":0.0},\"radius\":0.1,\"strokeWidth\":0.02,\"filled\":false}}" + ] +} \ No newline at end of file diff --git a/src/main/deploy/position_details.json b/src/main/deploy/position_details.json new file mode 100644 index 0000000..4bfcb50 --- /dev/null +++ b/src/main/deploy/position_details.json @@ -0,0 +1,127 @@ +{ + "reef": { + "coral": { + "stage1": { + "verticalPos": 0.46, + "systemPositions": { + "elevator": , + "elbow": , + "wrist": + }, + "horizontalOffsets": { + "left": -0.301625, + "right": 0.301625 + } + }, + "stage2": { + "verticalPos": 0.81, + "systemPositions": { + "elevator": , + "elbow": , + "wrist": + }, + "horizontalOffsets": { + "left": -0.1651, + "right": 0.1651 + } + }, + "stage3": { + "verticalPos": 1.21, + "systemPositions": { + "elevator": , + "elbow": , + "wrist": + }, + "horizontalOffsets": { + "left": -0.1651, + "right": 0.1651 + } + }, + "stage4": { + "verticalPos": 1.83, + "systemPositions": { + "elevator": , + "elbow": , + "wrist": + }, + "horizontalOffsets": { + "left": -0.1651, + "right": 0.1651 + } + } + }, + + "tag6": { + "face": "closeLeftFace", + "algaeVerticalPos": 0.965, + "algaeSystemPositions": { + "elevator": , + "elbow": , + "wrist": + } + }, + + "tag7": { + "face": "closeCenterFace", + "algaeVerticalPos": 0.965, + "algaeSystemPositions": { + "elevator": , + "elbow": , + "wrist": + } + }, + + "tag9": { + "face": "farRightFace", + "algaeVerticalPos": 1.365, + "algaeSystemPositions": { + "elevator": , + "elbow": , + "wrist": + } + }, + + "tag10": { + "face": "farCenterFace", + "algaeVerticalPos": 0.965, + "algaeSystemPositions": { + "elevator": , + "elbow": , + "wrist": + } + }, + + "tag11": { + "face": "farLeftFace", + "algaeVerticalPos": 1.365, + "algaeSystemPositions": { + "elevator": , + "elbow": , + "wrist": + } + } + }, + + "processor": { + "verticalPos": 0.43, + "horizontalOffset": 0 + }, + + "coralStation": { + "verticalPos": 0.95, + "algaeSystemPositions": { + "elevator": , + "elbow": , + "wrist": + }, + "tag1": { + "side": "left", + "horizontalOffsets": [-0.85777, -0.64333, -0.42888, -0.21444, 0, 0.21444, 0.42888, 0.64333, 0.85777] + }, + "tag2": { + "side": "right", + "horizontalOffsets": [-0.85777, -0.64333, -0.42888, -0.21444, 0, 0.21444, 0.42888, 0.64333, 0.85777] + } + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..17333a7 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,93 @@ +package frc.robot; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; + +public class Constants { + public static class ControllerConstants { + + public static final int DRIVER_CONTROLLER_PORT = 0; + public static final int OPERATOR_CONTROLLER_PORT = 1; + + // Joystick Deadband + public static final double LEFT_X_DEADBAND = 0.1; + public static final double LEFT_Y_DEADBAND = 0.1; + public static final double RIGHT_X_DEADBAND = 0.1; + } + + public class VisionConstants + { + + public static final String LEFT_CAMERA_NICKNAME = "Microsoft_LifeCam_HD-3000_Left"; // TODO: find proper value + public static final Transform3d LEFT_CAMERA_PLACEMENT = new Transform3d( + new Translation3d(-0.305816, 0.2276856, 0.5478018), new Rotation3d()) + .plus(new Transform3d(new Translation3d(), new Rotation3d(0, 0.959931, 0))) + .plus(new Transform3d(new Translation3d(), new Rotation3d(0, 0, -0.523599))); // TODO: find proper + // value, + // new Rotation3d(0, 0.959931, 2.61799) + public static final String RIGHT_CAMERA_NICKNAME = "Microsoft_LifeCam_HD-3000_Right"; // TODO: find proper value + public static final Transform3d RIGHT_CAMERA_PLACEMENT = new Transform3d( + new Translation3d(-0.305816, -0.2276856, 0.5478018), new Rotation3d()) + .plus(new Transform3d(new Translation3d(), new Rotation3d(0, 0.959931, 0))) + .plus(new Transform3d(new Translation3d(), new Rotation3d(0, 0, 0.523599))); // TODO: find proper + // value, + + } + + public static class LoggingConstants { + + public static final boolean DEFAULT_LOGGING_STATE = false; + } + + public static class ElevatorConstants { + + public static final int ELEVATOR_MOTOR_ID = 21; + public static final int ELEVATOR_POT_ID = 22; + public static final int ELEVATOR_STAGE_1_LIMIT_ID = 23; + public static final int ELEVATOR_STAGE_2_LIMIT_ID = 24; + public static final int ELEVATOR_STAGE_3_LIMIT_ID = 25; + public static final int ELEVATOR_STAGE_4_LIMIT_ID = 26; + + public static final PIDController ELEVATOR_PID = new PIDController(0, 0, 0); + + public static final double STAGE_1_POS = 4; + public static final double STAGE_2_POS = 8; + public static final double STAGE_3_POS = 12; + public static final double STAGE_4_POS = 157; + + public static final double ELEVATOR_POS_TOLERANCE = 2; + public static final double ELEVATOR_MOTOR_VELOCITY_TOLERANCE = 0.2; + } + + public static class IntakeConstants { + + public static final int INTAKE_MOTOR_ID = 31; + public static final int INTAKE_SENSOR_ID = 32; + // public static final int INTAKE_SENSOR2_ID = 33; + + public static final double INTAKE_MOTOR_SPEED = 0.5; + public static final double PLACE_MOTOR_SPEED = 0.5; + public static final double EJECT_MOTOR_SPEED = 0.5; + + public static final double PLACE_TIME = 0.5; + public static final double EJECT_TIME = 0.5; + } + + public static class ElbowConstants { + public static final int ELBOW_MOTOR_ID = 41; + public static final int ELBOW_ENCODER_ID = 52; + + public static final PIDController ELBOW_PID = new PIDController(0, 0, 0); + } + + public static class WristConstants { + public static final int WRIST_MOTOR_ID = 42; + public static final int WRIST_ENCODER_ID = 51; + + public static final PIDController WRIST_PID = new PIDController(0, 0, 0); + } + +} + diff --git a/Main.java b/src/main/java/frc/robot/Main.java similarity index 100% rename from Main.java rename to src/main/java/frc/robot/Main.java diff --git a/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java similarity index 94% rename from java/frc/robot/Robot.java rename to src/main/java/frc/robot/Robot.java index 0556048..6973bbe 100644 --- a/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,10 +11,9 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; - private RobotContainer m_robotContainer; + private final RobotContainer m_robotContainer; - @Override - public void robotInit() { + public Robot() { m_robotContainer = new RobotContainer(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..21ed2d8 --- /dev/null +++ b/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,98 @@ +// 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. + +package frc.robot; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants.ControllerConstants; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.DriveSystem; +import frc.robot.subsystems.ElevatorSystem; +import frc.robot.subsystems.IntakeSystem; +import frc.robot.subsystems.JointSystem; +import frc.robot.subsystems.VisionSystem; + +public class RobotContainer { + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed + private double MaxAngularRate = RotationsPerSecond.of(1).in(RadiansPerSecond); // 1 of a rotation per second max angular velocity + + /* Setting up bindings for necessary control of the swerve drive platform */ + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + + private final Telemetry logger = new Telemetry(MaxSpeed); + + private final CommandXboxController joystick = new CommandXboxController(0); + + public final DriveSystem drivetrain = TunerConstants.createDrivetrain(); + private final ElevatorSystem elevator = new ElevatorSystem(); + private final IntakeSystem intake = new IntakeSystem(); + private final JointSystem elbow = new JointSystem(true); + private final JointSystem wrist = new JointSystem(false); + public final VisionSystem visionSystem = new VisionSystem(); + + private final SendableChooser autoChooser; + + public RobotContainer() { + configureBindings(); + System.out.println("Im about to make autobuilder"); + autoChooser = AutoBuilder.buildAutoChooser("New Auto"); + + SmartDashboard.putData("Auto Chooser", autoChooser); + } + + private void configureBindings() { + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain.applyRequest(() -> + drive.withVelocityX(MathUtil.applyDeadband(-joystick.getLeftY(), ControllerConstants.LEFT_Y_DEADBAND) * MaxSpeed) // Drive forward with negative Y (forward) + .withVelocityY(MathUtil.applyDeadband(-joystick.getLeftX(), ControllerConstants.LEFT_X_DEADBAND) * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(MathUtil.applyDeadband(-joystick.getRightX(), ControllerConstants.RIGHT_X_DEADBAND) * MaxAngularRate) // Drive counterclockwise with negative X (left) + ) + ); + + joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); + joystick.b().whileTrue(drivetrain.applyRequest(() -> + point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())) + )); + + System.out.print( visionSystem.getTagPose(3)); + + // Run SysId routines when holding back/start and X/Y. + // Note that each routine should be run exactly once in a single log. + // joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); + // joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); + // joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); + // joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + + // reset the field-centric heading on left bumper press + joystick.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + + drivetrain.registerTelemetry(logger::telemeterize); + } + + public Command getAutonomousCommand() { + return autoChooser.getSelected(); + // return Commands.print("No autonomous command configured"); + } +} diff --git a/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java similarity index 99% rename from main/java/frc/robot/Telemetry.java rename to src/main/java/frc/robot/Telemetry.java index a2d36dd..edf1979 100644 --- a/main/java/frc/robot/Telemetry.java +++ b/src/main/java/frc/robot/Telemetry.java @@ -121,4 +121,4 @@ public void telemeterize(SwerveDriveState state) { SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/elevator_commands/GoToExtrema.java b/src/main/java/frc/robot/commands/elevator_commands/GoToExtrema.java new file mode 100644 index 0000000..418d907 --- /dev/null +++ b/src/main/java/frc/robot/commands/elevator_commands/GoToExtrema.java @@ -0,0 +1,49 @@ +// 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. + +package frc.robot.commands.elevator_commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.parsing.PositionDetails; +import frc.robot.subsystems.ElevatorSystem; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class GoToExtrema extends Command { + private final ElevatorSystem elevator; + private final PositionDetails positionDetails; + private final boolean isTop; + + /** Creates a new GoToStage1. */ + public GoToExtrema(final ElevatorSystem elevator, final PositionDetails positionDetails, final boolean isTop) { + // Use addRequirements() here to declare subsystem dependencies. + this.elevator = elevator; + this.positionDetails = positionDetails; + this.isTop = isTop; + addRequirements(elevator); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + elevator.runMotor(elevator.getNewSpeed(isTop ? positionDetails.getElevatorPosAtStage(1) : positionDetails.getElevatorPosAtStage(4))); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + elevator.runMotor(elevator.getNewSpeed(isTop ? positionDetails.getElevatorPosAtStage(1) : positionDetails.getElevatorPosAtStage(4))); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + elevator.runMotor(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (elevator.isOscillating(isTop ? positionDetails.getElevatorPosAtStage(1) : positionDetails.getElevatorPosAtStage(4)) || isTop ? elevator.atBottom() : elevator.atTop()); + } +} diff --git a/src/main/java/frc/robot/commands/elevator_commands/GoToIntermediatePos.java b/src/main/java/frc/robot/commands/elevator_commands/GoToIntermediatePos.java new file mode 100644 index 0000000..64780c9 --- /dev/null +++ b/src/main/java/frc/robot/commands/elevator_commands/GoToIntermediatePos.java @@ -0,0 +1,47 @@ +// 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. + +package frc.robot.commands.elevator_commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.parsing.PositionDetails; +import frc.robot.subsystems.ElevatorSystem; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class GoToIntermediatePos extends Command { + private final ElevatorSystem elevator; + private final double stageHeight; + + /** Creates a new GoToStage1. */ + public GoToIntermediatePos(final ElevatorSystem elevator, final PositionDetails positionDetails, final int stage) { + // Use addRequirements() here to declare subsystem dependencies. + this.elevator = elevator; + addRequirements(elevator); + stageHeight = positionDetails.getElevatorPosAtStage(stage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + elevator.runMotor(elevator.getNewSpeed(stageHeight)); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + elevator.runMotor(elevator.getNewSpeed(stageHeight)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + elevator.runMotor(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return elevator.isOscillating(stageHeight); + } +} diff --git a/src/main/java/frc/robot/commands/intake_commands/EjectCoral.java b/src/main/java/frc/robot/commands/intake_commands/EjectCoral.java new file mode 100644 index 0000000..8c276f5 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake_commands/EjectCoral.java @@ -0,0 +1,46 @@ +// 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. + +package frc.robot.commands.intake_commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.IntakeConstants; +import frc.robot.subsystems.IntakeSystem; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class EjectCoral extends Command { + private final IntakeSystem intakeSystem; + private Timer timer = new Timer(); + + /** Creates a new EjectCoral. */ + public EjectCoral(final IntakeSystem intakeSystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.intakeSystem = intakeSystem; + addRequirements(intakeSystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + intakeSystem.run(-IntakeConstants.EJECT_MOTOR_SPEED); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intakeSystem.run(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get() > IntakeConstants.EJECT_TIME; + } +} diff --git a/src/main/java/frc/robot/commands/intake_commands/IntakeCoral.java b/src/main/java/frc/robot/commands/intake_commands/IntakeCoral.java new file mode 100644 index 0000000..e81a006 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake_commands/IntakeCoral.java @@ -0,0 +1,43 @@ +// 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. + +package frc.robot.commands.intake_commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.IntakeConstants; +import frc.robot.subsystems.IntakeSystem; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class IntakeCoral extends Command { + private final IntakeSystem intakeSystem; + + /** Creates a new IntakeCoral. */ + public IntakeCoral(final IntakeSystem intakeSystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.intakeSystem = intakeSystem; + addRequirements(intakeSystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + intakeSystem.run(IntakeConstants.INTAKE_MOTOR_SPEED); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intakeSystem.run(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return intakeSystem.hasCoral(); + } +} diff --git a/src/main/java/frc/robot/commands/intake_commands/PlaceCoral.java b/src/main/java/frc/robot/commands/intake_commands/PlaceCoral.java new file mode 100644 index 0000000..b0b7f66 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake_commands/PlaceCoral.java @@ -0,0 +1,46 @@ +// 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. + +package frc.robot.commands.intake_commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.IntakeConstants; +import frc.robot.subsystems.IntakeSystem; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class PlaceCoral extends Command { + private final IntakeSystem intakeSystem; + private Timer timer = new Timer(); + + /** Creates a new PlaceCoral. */ + public PlaceCoral(final IntakeSystem intakeSystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.intakeSystem = intakeSystem; + addRequirements(intakeSystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + intakeSystem.run(-IntakeConstants.PLACE_MOTOR_SPEED); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intakeSystem.run(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get() > IntakeConstants.PLACE_TIME; + } +} diff --git a/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java similarity index 88% rename from generated/TunerConstants.java rename to src/main/java/frc/robot/generated/TunerConstants.java index cbbbb05..abee4cf 100644 --- a/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -24,14 +24,14 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) // 100, 0, 0.5 - .withKS(0.1).withKV(1.91).withKA(0) // 0, 1.5, 0 + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.59).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) // 3, 0, 0 - .withKS(0).withKV(0.124); // removed KA changed and KV from 0 + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -51,7 +51,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); // 150 + private static final Current kSlipCurrent = Amps.of(120.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -70,19 +70,19 @@ public class TunerConstants { // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); // Default Name + public static final CANBus kCANBus = new CANBus("Default Name", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); // 5 + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(6.21); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; // 3.5714285714285716 + private static final double kCoupleRatio = 3; - private static final double kDriveGearRatio = 7.363636363636365; // 6.122448979591837 - private static final double kSteerGearRatio = 15.42857142857143; // 12.8 - private static final Distance kWheelRadius = Inches.of(2.167); // 1.92 + private static final double kDriveGearRatio = 5.142857142857142; + private static final double kSteerGearRatio = 12.8; + private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; @@ -90,11 +90,11 @@ public class TunerConstants { private static final int kPigeonId = 13; // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); // 0.00001 - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); // 0.001 + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); // 0.25 - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); // 0.25 + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() .withCANBusName(kCANBus.getName()) @@ -129,45 +129,45 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 11; private static final int kFrontLeftSteerMotorId = 12; private static final int kFrontLeftEncoderId = 10; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); // -0.052734375 - private static final boolean kFrontLeftSteerMotorInverted = true; // false - private static final boolean kFrontLeftEncoderInverted = false; // new + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.0537109375); + private static final boolean kFrontLeftSteerMotorInverted = false; + private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(10); // 9.25 - private static final Distance kFrontLeftYPos = Inches.of(10); // 9.25 + private static final Distance kFrontLeftXPos = Inches.of(9); + private static final Distance kFrontLeftYPos = Inches.of(9); // Front Right private static final int kFrontRightDriveMotorId = 2; private static final int kFrontRightSteerMotorId = 3; private static final int kFrontRightEncoderId = 1; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); // -0.6796875 - private static final boolean kFrontRightSteerMotorInverted = true; // false - private static final boolean kFrontRightEncoderInverted = false; // new + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.328125); + private static final boolean kFrontRightSteerMotorInverted = false; + private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(10); // 9.25 - private static final Distance kFrontRightYPos = Inches.of(-10); // -9.25 + private static final Distance kFrontRightXPos = Inches.of(9); + private static final Distance kFrontRightYPos = Inches.of(-9); // Back Left private static final int kBackLeftDriveMotorId = 8; private static final int kBackLeftSteerMotorId = 9; private static final int kBackLeftEncoderId = 7; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); // -0.304931640625 - private static final boolean kBackLeftSteerMotorInverted = true; // false - private static final boolean kBackLeftEncoderInverted = false; // new + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.30224609375); + private static final boolean kBackLeftSteerMotorInverted = false; + private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-10); // -9.25 - private static final Distance kBackLeftYPos = Inches.of(10); // 9.25 + private static final Distance kBackLeftXPos = Inches.of(-9); + private static final Distance kBackLeftYPos = Inches.of(9); // Back Right private static final int kBackRightDriveMotorId = 5; private static final int kBackRightSteerMotorId = 6; private static final int kBackRightEncoderId = 4; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); // -0.032470703125 - private static final boolean kBackRightSteerMotorInverted = true; // false - private static final boolean kBackRightEncoderInverted = false; // new + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.031005859375); + private static final boolean kBackRightSteerMotorInverted = false; + private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-10); // -9.25 - private static final Distance kBackRightYPos = Inches.of(-10); // -9.25 + private static final Distance kBackRightXPos = Inches.of(-9); + private static final Distance kBackRightYPos = Inches.of(-9); public static final SwerveModuleConstants FrontLeft = @@ -283,4 +283,4 @@ public TunerSwerveDrivetrain( ); } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/parsing/PositionDetails.java b/src/main/java/frc/robot/parsing/PositionDetails.java new file mode 100644 index 0000000..d503fc9 --- /dev/null +++ b/src/main/java/frc/robot/parsing/PositionDetails.java @@ -0,0 +1,127 @@ +// 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. + +package frc.robot.parsing; + +import java.io.File; +import java.io.IOException; +import java.util.ArrayList; +import java.util.Dictionary; +import java.util.Hashtable; +import java.util.Iterator; +import java.util.List; + +import edu.wpi.first.wpilibj.Filesystem; +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.ObjectMapper; + +/** Add your docs here. */ +public class PositionDetails { + private final String JSONPath = "/position_details.json"; + public Stage[] corals; + public Dictionary reefTags = new Hashtable<>(); + + public class Stage { + public double leftOffset; + public double rightOffset; + public double elevatorPos; + public double elbowPos; + public double wristPos; + + + public Stage(JsonNode stageJSON) { + this.leftOffset = stageJSON.get("horizontalOffsets").get("left").asDouble(); + this.rightOffset = stageJSON.get("horizontalOffsets").get("right").asDouble(); + this.elevatorPos = stageJSON.get("systemPositions").get("elevator").asDouble(); + this.elbowPos = stageJSON.get("systemPositions").get("elbow").asDouble(); + this.wristPos = stageJSON.get("systemPositions").get("wrist").asDouble(); + } + } + + public PositionDetails() { + File file = new File(Filesystem.getDeployDirectory().toPath() + JSONPath); + ObjectMapper objectMapper = new ObjectMapper(); + try { + JsonNode json = objectMapper.readTree(file); + for (Iterator i = json.get("reef").fieldNames(); i.hasNext();) { + String currentTag = i.next(); + int tagID = Integer.parseInt(currentTag.substring(3)); + reefTags.put(tagID, new ReefTag(json.get("reef").get(currentTag), tagID)); + } + List coralList = new ArrayList(4); + for(int i = 1; i <= 4; i++) { + coralList.add(new Stage(json.get("reef").get("coral").get("stage" + i))); + } + coralList.toArray(corals); + } catch (IOException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + + /** + * Get elevator position for the algae + * @param tagID - red oriented tag IDs ie: blue 17 -> red 6 + * @return a double representing the elevator position + */ + public double getElevatorPosAtAlgae(int tagID) { + return reefTags.get(tagID).getElevatorPosAtAlgae(); + } + /** + * Get elbow position for the algae + * @param tagID - red oriented tag IDs ie: blue 17 -> red 6 + * @return a double representing the elbow position + */ + public double getElbowPosAtAlgae(int tagID) { + return reefTags.get(tagID).getElbowPosAtAlgae(); + } + /** + * Get wrist position for the algae + * @param tagID - red oriented tag IDs ie: blue 17 -> red 6 + * @return a double representing the wrist position + */ + public double getWristPosAtAlgae(int tagID) { + return reefTags.get(tagID).getWristPosAtAlgae(); + } + /** + * Get elevator position for the specified stage + * @param stage - one-indexed ie: 1,2,3,4 + * @return a double representing the elevator position + */ + public double getElevatorPosAtStage(int stage) { + return corals[stage - 1].elevatorPos; + } + /** + * Get elbow position for the specified stage + * @param stage - one-indexed ie: 1,2,3,4 + * @return a double representing the elbow position + */ + public double getElbowPosAtStage(int stage) { + return corals[stage - 1].elbowPos; + } + /** + * Get wrist position for the specified stage + * @param stage - one-indexed ie: 1,2,3,4 + * @return a double representing the wrist position + */ + public double getWristPosAtStage(int stage) { + return corals[stage - 1].wristPos; + } + /** + * Get left offset for the specified stage + * @param stage - one-indexed ie: 1,2,3,4 + * @return a double representing the left offset + */ + public double getLeftOffsetAtStage(int stage) { + return corals[stage - 1].leftOffset; + } + /** + * Get right offset for the specified stage + * @param stage - one-indexed ie: 1,2,3,4 + * @return a double representing the right offset + */ + public double getRightOffsetAtStage(int stage) { + return corals[stage - 1].rightOffset; + } +} diff --git a/src/main/java/frc/robot/parsing/ReefTag.java b/src/main/java/frc/robot/parsing/ReefTag.java new file mode 100644 index 0000000..2f5a526 --- /dev/null +++ b/src/main/java/frc/robot/parsing/ReefTag.java @@ -0,0 +1,48 @@ +// 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. + +package frc.robot.parsing; + +import com.fasterxml.jackson.databind.JsonNode; + +/** Add your docs here. */ +public class ReefTag { + public int tagID; + public String face; + public double algaeVerticalPos; + public double elevatorPos; + public double elbowPos; + public double wristPos; + + public ReefTag(JsonNode tagJSON, int tagID) { + this.tagID = tagID; // TODO: Try to get tag ID from tagJSON + this.face = tagJSON.get("face").asText(); + this.algaeVerticalPos = tagJSON.get("algaeVerticalPos").asDouble(); + this.elevatorPos = tagJSON.get("systemPositions").get("elevator").asDouble(); + this.elbowPos = tagJSON.get("systemPositions").get("elbow").asDouble(); + this.wristPos = tagJSON.get("systemPositions").get("wrist").asDouble(); + } + + /** + * Get elevator position for the algae + * @return a double representing the elevator position + */ + public double getElevatorPosAtAlgae() { + return algaeVerticalPos; + } + /** + * Get elbow position for the algae + * @return a double representing the elbow position + */ + public double getElbowPosAtAlgae() { + return algaeVerticalPos; + } + /** + * Get wrist position for the algae + * @return a double representing the wrist position + */ + public double getWristPosAtAlgae() { + return algaeVerticalPos; + } +} \ No newline at end of file diff --git a/DriveSystem.java b/src/main/java/frc/robot/subsystems/DriveSystem.java similarity index 90% rename from DriveSystem.java rename to src/main/java/frc/robot/subsystems/DriveSystem.java index 3447342..05f937a 100644 --- a/DriveSystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSystem.java @@ -9,7 +9,6 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; @@ -122,8 +121,8 @@ public class DriveSystem extends TunerSwerveDrivetrain implements Subsystem { * the devices themselves. If they need the devices, they can access them through * getters in the classes. * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module */ public DriveSystem( SwerveDrivetrainConstants drivetrainConstants, @@ -143,11 +142,11 @@ public DriveSystem( * the devices themselves. If they need the devices, they can access them through * getters in the classes. * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module */ public DriveSystem( SwerveDrivetrainConstants drivetrainConstants, @@ -168,17 +167,17 @@ public DriveSystem( * the devices themselves. If they need the devices, they can access them through * getters in the classes. * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation * in the form [x, y, theta]ᵀ, with units in meters * and radians * @param visionStandardDeviation The standard deviation for vision calculation * in the form [x, y, theta]ᵀ, with units in meters * and radians - * @param modules Constants for each specific module + * @param modules Constants for each specific module */ public DriveSystem( SwerveDrivetrainConstants drivetrainConstants, @@ -191,10 +190,11 @@ public DriveSystem( if (Utils.isSimulation()) { startSimThread(); } - configureAutoBuilder(); + configureAutoBuilder(); } private void configureAutoBuilder() { + System.out.println("Im about to configure the autobuilder"); try { var config = RobotConfig.fromGUISettings(); AutoBuilder.configure( @@ -209,9 +209,9 @@ private void configureAutoBuilder() { ), new PPHolonomicDriveController( // PID constants for translation - new PIDConstants(10, 0, 0), // 5, 0, 0 + new PIDConstants(10, 0, 0), // PID constants for rotation - new PIDConstants(7, 0, 0) // 5, 0, 0 + new PIDConstants(7, 0, 0) ), config, // Assume the path needs to be flipped for Red vs Blue, this is normally the case @@ -219,6 +219,7 @@ private void configureAutoBuilder() { this // Subsystem for requirements ); } catch (Exception ex) { + System.out.println(ex); DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); } } @@ -290,4 +291,4 @@ private void startSimThread() { }); m_simNotifier.startPeriodic(kSimLoopPeriod); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSystem.java b/src/main/java/frc/robot/subsystems/ElevatorSystem.java new file mode 100644 index 0000000..942169f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSystem.java @@ -0,0 +1,64 @@ +// 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. + +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ElevatorConstants; +import frc.utilities.PosUtils; + +public class ElevatorSystem extends SubsystemBase implements PosUtils { + + private static SparkMax motor = new SparkMax(ElevatorConstants.ELEVATOR_MOTOR_ID, MotorType.kBrushless); + private static AnalogInput pot = new AnalogInput(ElevatorConstants.ELEVATOR_POT_ID); + private static DigitalInput bottomLimit = new DigitalInput(ElevatorConstants.ELEVATOR_STAGE_1_LIMIT_ID); + private static DigitalInput topLimit = new DigitalInput(ElevatorConstants.ELEVATOR_STAGE_4_LIMIT_ID); + private static PIDController PID = ElevatorConstants.ELEVATOR_PID; + + /** Creates a new ElevatorSystem. */ + public ElevatorSystem() {} + + public void runMotor(double velocity) { + motor.set(velocity); + } + + private double getMotorVelocity() { + return motor.getEncoder().getVelocity(); + } + + private double getPos() { + return pot.getValue(); + } + + public boolean atStage(boolean top) { + return top? atTop() : atBottom(); + } + + public boolean atBottom() { + return bottomLimit.get(); + } + + public boolean atTop() { + return topLimit.get(); + } + + public double getNewSpeed(double desiredPos) { + return PID.calculate(getPos(), desiredPos); + } + + public boolean isOscillating(double desiredPos) { + return PosUtils.isOscillating(desiredPos, getPos(), ElevatorConstants.ELEVATOR_POS_TOLERANCE, getMotorVelocity(), ElevatorConstants.ELEVATOR_MOTOR_VELOCITY_TOLERANCE); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSystem.java b/src/main/java/frc/robot/subsystems/IntakeSystem.java new file mode 100644 index 0000000..2d756e0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSystem.java @@ -0,0 +1,33 @@ +// 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. + +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.IntakeConstants; + +public class IntakeSystem extends SubsystemBase { + private static SparkMax motor = new SparkMax(IntakeConstants.INTAKE_MOTOR_ID, MotorType.kBrushless); + private static DigitalInput sensor = new DigitalInput(IntakeConstants.INTAKE_SENSOR_ID); + + /** Creates a new IntakeSystem. */ + public IntakeSystem() {} + + public void run(double speed) { + motor.set(speed); + } + + public boolean hasCoral() { + return sensor.get(); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/JointSystem.java b/src/main/java/frc/robot/subsystems/JointSystem.java new file mode 100644 index 0000000..32870fa --- /dev/null +++ b/src/main/java/frc/robot/subsystems/JointSystem.java @@ -0,0 +1,57 @@ +// 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. + +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ElbowConstants; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.Constants.WristConstants; +import frc.utilities.PosUtils; + +public class JointSystem extends SubsystemBase implements PosUtils { + private static SparkBase motor; + private static AnalogInput absEncoder; + private static PIDController PID; + + /** Creates a new JointSystem. */ + public JointSystem(boolean isElbow) { + motor = isElbow ? new SparkFlex(ElbowConstants.ELBOW_MOTOR_ID, MotorType.kBrushless) : new SparkMax(WristConstants.WRIST_MOTOR_ID, MotorType.kBrushless); + absEncoder = new AnalogInput(isElbow ? ElbowConstants.ELBOW_ENCODER_ID : WristConstants.WRIST_ENCODER_ID); + PID = isElbow ? ElbowConstants.ELBOW_PID : WristConstants.WRIST_PID; + } + + public void runMotor(double velocity) { + motor.set(velocity); + } + + private double getMotorVelocity() { + return motor.getEncoder().getVelocity(); + } + + private int getPos() { + return absEncoder.getValue(); + } + + public boolean isOscillating(double desiredPos) { + return PosUtils.isOscillating(desiredPos, getPos(), ElevatorConstants.ELEVATOR_POS_TOLERANCE, getMotorVelocity(), ElevatorConstants.ELEVATOR_MOTOR_VELOCITY_TOLERANCE); + } + + + public double getNewSpeed(double desiredPos) { + return PID.calculate(getPos(), desiredPos); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java new file mode 100644 index 0000000..4513141 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -0,0 +1,382 @@ +// 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. + +package frc.robot.subsystems; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.common.hardware.VisionLEDMode; +import org.photonvision.PhotonUtils; +import org.photonvision.targeting.PhotonTrackedTarget; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.wpilibj.DriverStation; +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.VisionConstants; + +public class VisionSystem extends SubsystemBase { + + PhotonCamera leftCamera; + PhotonCamera rightCamera; + AprilTagFieldLayout tagLayout; + PhotonPoseEstimator poseEstimatorLeft; + PhotonPoseEstimator poseEstimatorRight; + private Field2d vision_field = new Field2d(); + + boolean blueAlliance = true; + + /** Creates a new vision. */ + public VisionSystem() { + leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NICKNAME); + rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NICKNAME); + PortForwarder.add(5800, "photonvision.local", 5800); + + try { + tagLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); + } catch (IOException exception) { + leftCamera.close(); + rightCamera.close(); + throw new RuntimeException(exception); + } + + poseEstimatorLeft = new PhotonPoseEstimator(tagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_PLACEMENT); // TODO: decide which pose strategy to use + poseEstimatorRight = new PhotonPoseEstimator(tagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_PLACEMENT); // TODO: decide which pose strategy to use + + SmartDashboard.putData("vision based field", vision_field); + + } + + public void updateAlliance() { + var alliance = DriverStation.getAlliance(); + blueAlliance = alliance.get() == DriverStation.Alliance.Blue; + } + + public PhotonTrackedTarget findSpeakerTag() { + List targets = findTargets(); + List speakerTags = new ArrayList<>(); + + if (!blueAlliance) { + for (PhotonTrackedTarget target : targets) { + if (target.getFiducialId() == 3 | target.getFiducialId() == 4) { + speakerTags.add(target); + } + } + } else { + for (PhotonTrackedTarget target : targets) { + if (target.getFiducialId() == 7 | target.getFiducialId() == 8) { + speakerTags.add(target); + } + } + } + + PhotonTrackedTarget bestSpeakerTarget = Collections.max(targets, this::compareTargets); + return bestSpeakerTarget; + } + + public PhotonTrackedTarget findAmpTag() { + List targets = findTargets(); + PhotonTrackedTarget ampTag = null; + + if (!blueAlliance) { + for (PhotonTrackedTarget target : targets) { + if (target.getFiducialId() == 5) { + ampTag = target; + break; + } + } + } else { + for (PhotonTrackedTarget target : targets) { + if (target.getFiducialId() == 6) { + ampTag = target; + break; + } + } + } + + return ampTag; + } + + public int compareTargets(PhotonTrackedTarget a, PhotonTrackedTarget b) { + if (a.getArea() > b.getArea()) { + return 1; + } + if (a.getArea() == b.getArea()) { + return 0; + } else { + return -1; + } + } + + /* + * Get a list of tracked targets from the latest pipeline result. Returns null + * if there are no targets. + */ + + public List findTargets() { + var visionFrame = leftCamera.getLatestResult(); + if (visionFrame.hasTargets()) { + List targets = visionFrame.getTargets(); + return targets; + } else { + return null; + } + } + + /* + * Get the "best" target from the latest pipeline result. Returns null if there + * are no targets. + */ + + public PhotonTrackedTarget findBestTarget() { + var visionFrame = leftCamera.getLatestResult(); + if (visionFrame.hasTargets()) { + PhotonTrackedTarget target = visionFrame.getBestTarget(); + return target; + } else { + return null; + } + } + + /* + * Gets the yaw of the target in degrees (positive right). + */ + + public double getTargetYaw(PhotonTrackedTarget target) { + return target.getYaw(); + } + + /* + * Get the pitch of the target in degrees (positive up). + */ + + public double getTargetPitch(PhotonTrackedTarget target) { + return target.getPitch(); + } + + /* + * Get the area of the target (how much of the camera feed the bounding box + * takes up) as a percent (0-100). + */ + + public double getTargetArea(PhotonTrackedTarget target) { + return target.getArea(); + } + + /* + * Get the skew of the target (the skew of the target in degrees counter-clockwise positive). + */ + + public double getTargetSkew(PhotonTrackedTarget target) { + return target.getSkew(); + } + + // /* TODO: Added in docs but not in library + // * Get 4 corners of the minimum bounding box rectagle + // */ + + // public List getTargetCorners(PhotonTrackedTarget target) { + // return target.getCorners(); + // } + + + /* + * Get the ID of the detected fiducial marker. + */ + + public int getTargetID(PhotonTrackedTarget target) { + return target.getFiducialId(); + } + + /* + * + */ + + public double getPoseAmbiguity(PhotonTrackedTarget target) { + return target.getPoseAmbiguity(); + } + + // /* TODO: Included in docs but not in library + // * Get the transform that maps camera space (X = forward, Y = left, Z = up) to + // * object/fiducial tag space (X forward, Y left, Z up) with the lowest + // * reprojection error. + // */ + + // public Transform2d getCameraToTarget(PhotonTrackedTarget target) { + // return target.getCameraToTarget(); + // } + + /* + * Get the transform that maps camera space (X = forward, Y = left, Z = up) to + * object/fiducial tag space (X forward, Y left, Z up) with the lowest + * reprojection error. + */ + + public Transform3d getBestPathToTarget(PhotonTrackedTarget target) { + return target.getBestCameraToTarget(); + } + + /* + * Get the transform that maps camera space (X = forward, Y = left, Z = up) to + * object/fiducial tag space (X forward, Y left, Z up) with the highest + * reprojection error. + */ + + public Transform3d getOtherPathToTarget(PhotonTrackedTarget target) { + return target.getAlternateCameraToTarget(); + } + + /* + * Estimate the position of the robot relitive to the field. + */ + + public Optional getEstimatedGlobalPoseLeft() { + // poseEstimator.setReferencePose(prevRobotPose); + var visionFrame = leftCamera.getLatestResult(); + return poseEstimatorLeft.update(visionFrame); + } + + public Optional getEstimatedGlobalPoseRight() { + // poseEstimator.setReferencePose(prevRobotPose);\ + var visionFrame = rightCamera.getLatestResult(); + return poseEstimatorRight.update(visionFrame); + } + + public Optional getEstimatedGlobalPose() { + var poseLeft = poseEstimatorLeft.update(leftCamera.getLatestResult()); + var poseRight = poseEstimatorRight.update(rightCamera.getLatestResult()); + if (poseLeft.isPresent() && poseRight.isPresent()) { + return Optional.of(poseLeft.get().estimatedPose.interpolate(poseRight.get().estimatedPose, 0.5)); + } else if (poseLeft.isPresent()) { + return Optional.of(poseLeft.get().estimatedPose); + } else if (poseRight.isPresent()) { + return Optional.of(poseRight.get().estimatedPose); + } else { + return Optional.empty(); + } + } + + /* + * Get the position of the tag relitive to the field. + */ + + public Optional getTagPose(int targetID) { + return tagLayout.getTagPose(targetID); // TODO: make this return a non-optional Pose3d + } + + /* + * Calculate your robot’s Pose3d on the field using the pose of the AprilTag + * relative to the camera, pose of the AprilTag relative to the field, and the + * transform from the camera to the origin of the robot. + */ + // TODO: Only use function if (tagLayout.getTagPose(target.getFiducialId()).isPresent()) + + public Pose3d getFieldRelativePose( Pose3d tagPose, Transform3d cameraToTarget) { + return PhotonUtils.estimateFieldToRobotAprilTag(cameraToTarget, tagPose, VisionConstants.LEFT_CAMERA_PLACEMENT); + } + + // TODO: Define 2d version of camera placement to use this function + // public Pose2d getFieldRelativePose( Pose2d tagPose, Transform2d cameraToTarget) { + // return PhotonUtils.estimateFieldToRobot(cameraToTarget, tagPose, VisionConstants.LEFT_CAMERA_PLACEMENT); + // } + + /* + * Calculate the distance to the target based on the hieght of the camera off of + * the ground, the hieght of the target off of the ground, the camera’s pitch, + * and the pitch to the target. + */ + + public double getDistanceToTarget(double targetHeight, double cameraPitch, + double targetPitch) { + return PhotonUtils.calculateDistanceToTargetMeters(VisionConstants.LEFT_CAMERA_PLACEMENT.getY(), targetHeight, + cameraPitch, + Units.degreesToRadians(targetPitch)); // TODO: convert cameraPitch to use a constant + } + + /* + * Calculate the distance between two poses. This is useful when using + * AprilTags, given that there may not be an AprilTag directly on the target. + */ + + public double getDistanceToPose(Pose2d robotPose, Pose2d targetPose) { + return PhotonUtils.getDistanceToPose(robotPose, targetPose); + } + + /* + * Calculate translation to the target based on the distance to the target and + * angle to the target (yaw). + */ + + public Translation2d getTranslationToTarget(double distanceToTarget, double targetYaw) { + return PhotonUtils.estimateCameraToTargetTranslation(distanceToTarget, Rotation2d.fromDegrees(-targetYaw)); + } + + /* + * Calculate the Rotation2d between your robot and a target. This is useful when + * turning towards an arbitrary target on the field. + */ + + public Rotation2d getYawToPose(Pose2d robotPose, Pose2d targetPose) { + return PhotonUtils.getYawToPose(robotPose, targetPose); + } + + /* + * Toggle driver mode on or off. Driver mode is an unfiltered/normal view of the + * camera to be used while driving the robot. + */ + + public void driverModeToggle(boolean toggleOn) { + leftCamera.setDriverMode(toggleOn); + } + + /* + * Set the pipeline used by the camera. + */ + + public void setPipelineIndex(int index) { + leftCamera.setPipelineIndex(index); + } + + // /* TODO: Docs say we don't care + // * Get the latency of the pipeline in miliseconds. + // */ + + // public double getPipelineLatency() { + // var visionFrame = leftCamera.getLatestResult(); + // return visionFrame.getLatencyMillis(); + // } + + /* + * Set the mode of the camera LED(s). + */ + + public void setLED(VisionLEDMode LEDMode) { + leftCamera.setLED(LEDMode); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + var pose = getEstimatedGlobalPose(); + if (pose.isPresent()) { + vision_field.setRobotPose(pose.get().toPose2d()); + } + } +} diff --git a/src/main/java/frc/utilities/PosUtils.java b/src/main/java/frc/utilities/PosUtils.java new file mode 100644 index 0000000..1d7e5f4 --- /dev/null +++ b/src/main/java/frc/utilities/PosUtils.java @@ -0,0 +1,34 @@ +// 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. + +package frc.utilities; + +/** Add your docs here. */ +public interface PosUtils { + + /** + * checks if the currentPos is within posTolerance of desiredPos and the motorVelocity is within velocityTolerance in the direction towards desiredPos. + * @param desiredPos - the position you wish the system to reach + * @param currentPos - the position the system is currently at + * @param posTolerance - the allowable deviation from the desired position + * @param motorVelocity - the velocity that the motor is currently spinning at + * @param velocityTolerance - the maximum motor velocity for the system to not overshoot the desired position tolerance + * @return - boolean + */ + static boolean isOscillating(double desiredPos, double currentPos, double posTolerance, double motorVelocity, double velocityTolerance) { + boolean retval = false; + + if ((currentPos >= desiredPos - posTolerance) & ((motorVelocity >= 0) & (motorVelocity <= velocityTolerance))) { + // checks if the elevator's current position is below the desired position within ELEVATOR_POS_TOLERANCE and the elevator is moving up at a speed below ELEVATOR_MOTOR_VELOCITY_TOLERANCE + retval = true; + } + else if ((currentPos <= desiredPos + posTolerance) & ((motorVelocity <= 0) & (motorVelocity >= - velocityTolerance))) { + // checks if the elevator's current position is above the desired position within ELEVATOR_POS_TOLERANCE and the elevator is moving down at a speed below ELEVATOR_MOTOR_VELOCITY_TOLERANCE + retval = true; + } + + return retval; + } + +} diff --git a/subsystems/DriveSystem.java b/subsystems/DriveSystem.java deleted file mode 100644 index 3447342..0000000 --- a/subsystems/DriveSystem.java +++ /dev/null @@ -1,293 +0,0 @@ -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.*; - -import java.util.function.Supplier; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -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.generated.TunerConstants.TunerSwerveDrivetrain; - -/** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements - * Subsystem so it can easily be used in command-based projects. - */ -public class DriveSystem extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; - - /** Swerve request to apply during robot-centric path following */ - private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this - ) - ); - - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public DriveSystem( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public DriveSystem( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public DriveSystem( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - private void configureAutoBuilder() { - try { - var config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( - () -> getState().Pose, // Supplier of current robot pose - this::resetPose, // Consumer for seeding pose against auto - () -> getState().Speeds, // Supplier of current robot speeds - // Consumer of ChassisSpeeds and feedforwards to drive the robot - (speeds, feedforwards) -> setControl( - m_pathApplyRobotSpeeds.withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) - ), - new PPHolonomicDriveController( - // PID constants for translation - new PIDConstants(10, 0, 0), // 5, 0, 0 - // PID constants for rotation - new PIDConstants(7, 0, 0) // 5, 0, 0 - ), - config, - // Assume the path needs to be flipped for Red vs Blue, this is normally the case - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this // Subsystem for requirements - ); - } catch (Exception ex) { - DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); - } - } - - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - - @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 (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); - m_hasAppliedOperatorPerspective = true; - }); - } - } - - 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); - } -} \ No newline at end of file diff --git a/tuner-project.json b/tuner-project.json new file mode 100644 index 0000000..19324c1 --- /dev/null +++ b/tuner-project.json @@ -0,0 +1 @@ +{"Version":"1.0.0.0","LastState":11,"Modules":[{"ModuleName":"Front Left","ModuleId":0,"Encoder":{"Id":10,"Name":"frontLeft","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":12,"Name":"frontLeftAngle","Model":"Talon FX vers. C","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7},"IsStandaloneFx":false},"DriveMotor":{"Id":11,"Name":"frontLeftDrive","Model":"Talon FX vers. C","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":false,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.0537109375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":12,"ValidatedDriveId":11,"ValidatedEncoderId":10,"AvailEncoders":[{"Id":1,"Name":"frontRight","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},{"Id":4,"Name":"backRight","Model":"CANCoder vers. H","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},{"Id":7,"Name":"backLeft","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},{"Id":10,"Name":"frontLeft","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false}],"IsEncoderTypeCAN":true,"IsDeviceSelectionCompleted":true,"IsConfigurationCompleted":true,"AvailEncoderTypes":["CANcoder"]},{"ModuleName":"Front Right","ModuleId":1,"Encoder":{"Id":1,"Name":"frontRight","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":3,"Name":"frontRightAngle","Model":"Talon FX vers. C","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7},"IsStandaloneFx":false},"DriveMotor":{"Id":2,"Name":"frontRightDrive","Model":"Talon FX vers. C","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":false,"SelectedEncoderType":"CANcoder","EncoderOffset":0.328125,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":3,"ValidatedDriveId":2,"ValidatedEncoderId":1,"AvailEncoders":[{"Id":1,"Name":"frontRight","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},{"Id":4,"Name":"backRight","Model":"CANCoder vers. H","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},{"Id":7,"Name":"backLeft","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},{"Id":10,"Name":"frontLeft","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false}],"IsEncoderTypeCAN":true,"IsDeviceSelectionCompleted":true,"IsConfigurationCompleted":true,"AvailEncoderTypes":["CANcoder"]},{"ModuleName":"Back Left","ModuleId":2,"Encoder":{"Id":7,"Name":"backLeft","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":9,"Name":"backLeftAngle","Model":"Talon FX vers. C","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7},"IsStandaloneFx":false},"DriveMotor":{"Id":8,"Name":"backLeftDrive","Model":"Talon FX vers. C","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":false,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.30224609375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":9,"ValidatedDriveId":8,"ValidatedEncoderId":7,"AvailEncoders":[{"Id":1,"Name":"frontRight","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},{"Id":4,"Name":"backRight","Model":"CANCoder vers. H","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},{"Id":7,"Name":"backLeft","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},{"Id":10,"Name":"frontLeft","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false}],"IsEncoderTypeCAN":true,"IsDeviceSelectionCompleted":true,"IsConfigurationCompleted":true,"AvailEncoderTypes":["CANcoder"]},{"ModuleName":"Back Right","ModuleId":3,"Encoder":{"Id":4,"Name":"backRight","Model":"CANCoder vers. H","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":6,"Name":"backRightAngle","Model":"Talon FX vers. C","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7},"IsStandaloneFx":false},"DriveMotor":{"Id":5,"Name":"backRightDrive","Model":"Talon FX vers. C","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":false,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.031005859375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":6,"ValidatedDriveId":5,"ValidatedEncoderId":4,"AvailEncoders":[{"Id":1,"Name":"frontRight","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},{"Id":4,"Name":"backRight","Model":"CANCoder vers. H","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},{"Id":7,"Name":"backLeft","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},{"Id":10,"Name":"frontLeft","Model":"CANCoder","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false}],"IsEncoderTypeCAN":true,"IsDeviceSelectionCompleted":true,"IsConfigurationCompleted":true,"AvailEncoderTypes":["CANcoder"]}],"SwerveOptions":{"kSpeedAt12Volts":6.206390820091836,"Gyro":{"Id":13,"Name":"Pigeon 2 vers. S (Device ID 13)","Model":"Pigeon 2 vers. S","CANbus":"02C14EB74C32435320202059092017FF","CANbusFriendly":"Default Name","SelectedMotorType":null,"IsStandaloneFx":false},"IsValidGyroCANbus":true,"VerticalTrackSizeInches":18.0,"HorizontalTrackSizeInches":18.0,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":3,"SwerveModuleConfiguration":{"ModuleBrand":3,"DriveRatio":5.142857142857142,"SteerRatio":12.8,"CouplingRatio":3.0,"CustomName":"L4"},"HasVerifiedSteer":true,"HasVerifiedDrive":true,"IsValidConfiguration":true}} \ No newline at end of file diff --git a/vcs-1/gc.properties b/vcs-1/gc.properties deleted file mode 100644 index e69de29..0000000 diff --git a/vendordeps/PathplannerLib-2025.1.1.json b/vendordeps/PathplannerLib-2025.2.1.json similarity index 87% rename from vendordeps/PathplannerLib-2025.1.1.json rename to vendordeps/PathplannerLib-2025.2.1.json index 6322388..71e25f3 100644 --- a/vendordeps/PathplannerLib-2025.1.1.json +++ b/vendordeps/PathplannerLib-2025.2.1.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.1.1.json", + "fileName": "PathplannerLib-2025.2.1.json", "name": "PathplannerLib", - "version": "2025.1.1", + "version": "2025.2.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.1.1" + "version": "2025.2.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.1.1", + "version": "2025.2.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 99% rename from vendordeps/Phoenix6-25.1.0.json rename to vendordeps/Phoenix6-frc2025-latest.json index 473f6a8..7f4bd2e 100644 --- a/vendordeps/Phoenix6-25.1.0.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,5 +1,5 @@ { - "fileName": "Phoenix6-25.1.0.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", "version": "25.1.0", "frcYear": "2025", diff --git a/vendordeps/REVLib-2025.0.1.json b/vendordeps/REVLib-2025.0.1.json new file mode 100644 index 0000000..c998054 --- /dev/null +++ b/vendordeps/REVLib-2025.0.1.json @@ -0,0 +1,71 @@ +{ + "fileName": "REVLib-2025.0.1.json", + "name": "REVLib", + "version": "2025.0.1", + "frcYear": "2025", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2025.0.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..6af3d3e --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.1.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.1.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.1.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.1.1" + } + ] +} \ No newline at end of file