diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index 9eaf0d0..7d89e58 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -13,10 +13,10 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v1 - - name: Set up JDK 1.8 + - name: Set up JDK 17 uses: actions/setup-java@v1 with: - java-version: 1.12 + java-version: 17 - name: Change wrapper permissions run: chmod +x ./gradlew - name: Build with Gradle diff --git a/.gitignore b/.gitignore index a8d1911..b5b18bb 100644 --- a/.gitignore +++ b/.gitignore @@ -170,3 +170,11 @@ out/ # Simulation GUI and other tools window save file *-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ +simgui.json +simgui-ds.json diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 9c62ece..f523e9c 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2023", + "projectYear": "2024", "teamNumber": 4388 } \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md index 3d5a824..43b62ec 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors +Copyright (c) 2009-2023 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/autos/final_1note_stationary.auto b/autos/final_1note_stationary.auto new file mode 100644 index 0000000..f83851c Binary files /dev/null and b/autos/final_1note_stationary.auto differ diff --git a/autos/final_red_center_4note_taxi.auto b/autos/final_red_center_4note_taxi.auto new file mode 100644 index 0000000..b20ab84 Binary files /dev/null and b/autos/final_red_center_4note_taxi.auto differ diff --git a/build.gradle b/build.gradle index a2aa528..dc84aef 100644 --- a/build.gradle +++ b/build.gradle @@ -1,10 +1,12 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.3" + id "edu.wpi.first.GradleRIO" version "2024.3.1" } -sourceCompatibility = JavaVersion.VERSION_11 -targetCompatibility = JavaVersion.VERSION_11 +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} def ROBOT_MAIN_CLASS = "frc4388.robot.Main" @@ -65,15 +67,14 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - testImplementation 'org.junit.jupiter:junit-jupiter-api:5.8.2' - testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2' - testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2' + //testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + //testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } -test { - useJUnitPlatform() - systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' -} +// test { +// useJUnitPlatform() +// systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +//} // Simulation configuration (e.g. environment variables). wpi.sim.addGui().defaultEnabled = true @@ -84,6 +85,7 @@ wpi.sim.addDriverstation() // knows where to look for our Robot Class. jar { from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } @@ -96,4 +98,4 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' -} +} \ No newline at end of file diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index 249e583..d64cd49 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index c23a1b3..5e82d67 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,7 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip +networkTimeout=10000 +validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew index a69d9cb..1aa94a4 100644 --- a/gradlew +++ b/gradlew @@ -55,7 +55,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -80,13 +80,11 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit - -APP_NAME="Gradle" +# This is normally unused +# shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} - -# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum @@ -133,22 +131,29 @@ location of your Java installation." fi else JAVACMD=java - which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. Please set the JAVA_HOME variable in your environment to match the location of your Java installation." + fi fi # Increase the maximum file descriptors if we can. if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then case $MAX_FD in #( max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 MAX_FD=$( ulimit -H -n ) || warn "Could not query maximum file descriptor limit" esac case $MAX_FD in #( '' | soft) :;; #( *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 ulimit -n "$MAX_FD" || warn "Could not set maximum file descriptor limit to $MAX_FD" esac @@ -193,11 +198,15 @@ if "$cygwin" || "$msys" ; then done fi -# Collect all arguments for the java command; -# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of -# shell script including quotes and variable substitutions, so put them in -# double quotes to make sure that they get re-expanded; and -# * put everything else in single quotes, so that it's not re-expanded. + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. set -- \ "-Dorg.gradle.appname=$APP_BASE_NAME" \ diff --git a/gradlew.bat b/gradlew.bat index f127cfd..93e3f59 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -26,6 +26,7 @@ if "%OS%"=="Windows_NT" setlocal set DIRNAME=%~dp0 if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused set APP_BASE_NAME=%~n0 set APP_HOME=%DIRNAME% diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/settings.gradle b/settings.gradle index 48c039e..d94f73c 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -25,3 +25,6 @@ pluginManagement { } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index efc80a5..827a6b5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,10 +7,10 @@ package frc4388.robot; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; - -import frc4388.utility.LEDPatterns; import frc4388.utility.Gains; +import frc4388.utility.LEDPatterns; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -23,78 +23,89 @@ import frc4388.utility.Gains; public final class Constants { public static final class SwerveDriveConstants { - public static final double MAX_ROT_SPEED = 1.5; - public static final double MIN_ROT_SPEED = 0.8; + public static final double MAX_ROT_SPEED = 3.5; + public static final double AUTO_MAX_ROT_SPEED = 1.5; + public static final double MIN_ROT_SPEED = 1.0; public static double ROTATION_SPEED = MAX_ROT_SPEED; + public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED; public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; + + public static final String CANBUS_NAME = "IDK"; public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; - public static final double SLOW_SPEED = 0.8; + public static final double SLOW_SPEED = 0.5; public static final double FAST_SPEED = 1.0; public static final double TURBO_SPEED = 4.0; + public static final class DefaultSwerveRotOffsets { + public static final double FRONT_LEFT_ROT_OFFSET = 220; + public static final double FRONT_RIGHT_ROT_OFFSET = 136.365;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; + public static final double BACK_LEFT_ROT_OFFSET = -279.08349884; + public static final double BACK_RIGHT_ROT_OFFSET = 140.8887656; + } + public static final class IDs { - public static final int LEFT_FRONT_WHEEL_ID = 2; - public static final int LEFT_FRONT_STEER_ID = 3; - public static final int LEFT_FRONT_ENCODER_ID = 10; - - public static final int RIGHT_FRONT_WHEEL_ID = 4; - public static final int RIGHT_FRONT_STEER_ID = 5; - public static final int RIGHT_FRONT_ENCODER_ID = 11; - - public static final int LEFT_BACK_WHEEL_ID = 6; - public static final int LEFT_BACK_STEER_ID = 7; - public static final int LEFT_BACK_ENCODER_ID = 12; + public static final int RIGHT_FRONT_WHEEL_ID = 0; + public static final int RIGHT_FRONT_STEER_ID = 1; + public static final int RIGHT_FRONT_ENCODER_ID = 12; - public static final int RIGHT_BACK_WHEEL_ID = 8; - public static final int RIGHT_BACK_STEER_ID = 9; - public static final int RIGHT_BACK_ENCODER_ID = 13; + // public static final int LEFT_FRONT_WHEEL_ID = 4; + // public static final int LEFT_FRONT_STEER_ID = 5; + // public static final int LEFT_FRONT_ENCODER_ID = 11; + + // public static final int LEFT_BACK_WHEEL_ID = 6; + // public static final int LEFT_BACK_STEER_ID = 7; + // public static final int LEFT_BACK_ENCODER_ID = 12; + + // public static final int RIGHT_BACK_WHEEL_ID = 8; + // public static final int RIGHT_BACK_STEER_ID = 9; + // public static final int RIGHT_BACK_ENCODER_ID = 13; } public static final class PIDConstants { - public static final int SWERVE_SLOT_IDX = 0; - public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); + public static final int SWERVE_SLOT_IDX = 0; + public static final int SWERVE_PID_LOOP_IDX = 1; + public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); } public static final class AutoConstants { - public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); - public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune - - public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value - public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value + public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); + public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune + + public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value + public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value } public static final class Conversions { - public static final int CANCODER_TICKS_PER_ROTATION = 4096; - - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; - - public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; - public static final double MOTOR_REV_PER_STEER_REV = 12.8; - - public static final double TICKS_PER_MOTOR_REV = 2048; - public static final double WHEEL_DIAMETER_INCHES = 3.9; - public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; - - public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; - public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; - - public static final double TICK_TIME_TO_SECONDS = 10; - public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; + public static final int CANCODER_TICKS_PER_ROTATION = 4096; + + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; + + public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; + public static final double MOTOR_REV_PER_STEER_REV = 12.8; + + public static final double TICKS_PER_MOTOR_REV = 2048; + public static final double WHEEL_DIAMETER_INCHES = 3.9; + public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; + + public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; + + public static final double TICK_TIME_TO_SECONDS = 10; + public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; } public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value - public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value - public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value + public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value } public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value @@ -111,30 +122,41 @@ public final class Constants { public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } - public static final class VisionConstants { - public static final String NAME = "photonCamera"; + public static final class VisionConstants { + // public static final String NAME = "photonCamera"; - public static final int LIME_HIXELS = 640; - public static final int LIME_VIXELS = 480; + // public static final int LIME_HIXELS = 640; + // public static final int LIME_VIXELS = 480; - public static final double H_FOV = 59.6; - public static final double V_FOV = 45.7; + // public static final double H_FOV = 59.6; + // public static final double V_FOV = 45.7; - public static final double LIME_HEIGHT = 6.0; - public static final double LIME_ANGLE = 55.0; + // public static final double LIME_HEIGHT = 6.0; + // public static final double LIME_ANGLE = 55.0; - // public static final double HIGH_TARGET_HEIGHT = 46.0; - public static final double HIGH_TAPE_HEIGHT = 44.0; + // // public static final double HIGH_TARGET_HEIGHT = 46.0; + // public static final double HIGH_TAPE_HEIGHT = 44.0; - // public static final double MID_TARGET_HEIGHT = 34.0; - public static final double MID_TAPE_HEIGHT = 24.0; + // // public static final double MID_TARGET_HEIGHT = 34.0; + // public static final double MID_TAPE_HEIGHT = 24.0; - public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value - - } + // public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value + + public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609); + public static final Translation2d BlueSpeakerCenter = new Translation2d(-8.308975, 1.442593); + + public static final double SpeakerBubbleDistance = 3; + public static final double targetPosDistance = 1.5; + } + + public static final class AutoAlignConstants { + public static final double MoveSpeed = 0.0; + public static final double RotSpeed = 0.0; + } + public static final class DriveConstants { - public static final int DRIVE_PIGEON_ID = 6; + public static final int DRIVE_PIGEON_ID = 14; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } @@ -145,6 +167,34 @@ public final class Constants { public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; } + public static final class ShooterConstants { + public static final int LEFT_SHOOTER_ID = 15; // final + public static final int RIGHT_SHOOTER_ID = 16; // final + public static final double SHOOTER_SPEED = 0.50; // final + public static final double SHOOTER_IDLE = 0.20; // final + public static final double SHOOTER_IDLE_LIMELIGHT = 0.20; + } + + public static final class IntakeConstants { + public static final int INTAKE_MOTOR_ID = 18; + public static final int PIVOT_MOTOR_ID = 17; + public static final double INTAKE_SPEED = 0.75; + public static final double INTAKE_OUT_SPEED_UNPRESSED = 1.0; + public static final double INTAKE_OUT_SPEED_PRESSED = 0.5; + public static final double HANDOFF_SPEED = 0.4; + public static final double PIVOT_SPEED = 0.2; + + public static final class ArmPID { + public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0); + } + } + + public static final class ClimbConstants { + public static final int CLIMB_MOTOR_ID = 19; + public static final double CLIMB_IN_SPEED = -1.0; + public static final double CLIMB_OUT_SPEED = 0.87; + } + public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e555b09..36c4330 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,12 +7,14 @@ package frc4388.robot; +import edu.wpi.first.cameraserver.CameraServer; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.DeferredBlock; import frc4388.utility.RobotTime; - +//import frc4388.robot.subsystems.LED; /** * The VM is configured to automatically run this class, and to call the * functions corresponding to each mode, as described in the TimedRobot @@ -22,16 +24,17 @@ import frc4388.utility.RobotTime; */ public class Robot extends TimedRobot { Command m_autonomousCommand; - + private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - + //private LED mled = new LED(); /** * This function is run when the robot is first started up and should be * used for any initialization code. */ @Override public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); @@ -40,14 +43,16 @@ public class Robot extends TimedRobot { /** * This function is called every robot packet, no matter the mode. Use * this for items like diagnostics that you want ran during disabled, - * autonomous, teleoperated and test. + * autonomous, teleoperated and test.doubl * *

This runs after the mode specific periodic functions, but before * LiveWindow and SmartDashboard integrated updating. */ @Override - public void robotPeriodic() { + public void robotPeriodic() { m_robotTime.updateTimes(); + //System.out.println(m_robotContainer.limelight.isNearSpeaker()); + //mled.updateLED(); // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic @@ -119,7 +124,7 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - + m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5934d2f..3888775 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,20 +7,50 @@ package frc4388.robot; +// Drive Systems +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import frc4388.utility.controller.XboxController; +import frc4388.utility.controller.DeadbandedXboxController; +import frc4388.robot.Constants.OIConstants; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +// Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc4388.robot.Constants.*; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +// Autos +import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.commands.Swerve.JoystickRecorder; -import frc4388.robot.subsystems.LED; +import frc4388.utility.controller.VirtualController; +import frc4388.robot.commands.Swerve.neoJoystickPlayback; +import frc4388.robot.commands.Swerve.neoJoystickRecorder; +// import frc4388.robot.commands.Intake.ArmIntakeIn; +//import frc4388.robot.commands.Autos.AutoAlign; + +// Subsystems +// import frc4388.robot.subsystems.LED; +// import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.LEDPatterns; -import frc4388.utility.controller.DeadbandedXboxController; -import frc4388.utility.controller.IHandController; -import frc4388.utility.controller.XboxController; +// import frc4388.robot.subsystems.Shooter; +// import frc4388.robot.subsystems.Climber; +// import frc4388.robot.subsystems.Intake; + +// Utilites +import frc4388.utility.DeferredBlock; +import frc4388.utility.configurable.ConfigurableString; /** * This class is where the bulk of the robot should be declared. Since @@ -31,34 +61,160 @@ import frc4388.utility.controller.XboxController; */ public class RobotContainer { /* RobotMap */ - private final RobotMap m_robotMap = new RobotMap(); - - /* Subsystems */ - private final LED m_robotLED = new LED(m_robotMap.LEDController); - - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, - m_robotMap.rightFront, - m_robotMap.leftBack, - m_robotMap.rightBack, - m_robotMap.gyro); + public final RobotMap m_robotMap = new RobotMap(); + /* Subsystems */ + // private final LED m_robotLED = new LED(); + + // private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); + + // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, + // m_robotMap.rightFront, + // m_robotMap.leftBack, + // m_robotMap.rightBack, + + // m_robotMap.gyro); + /* Limelight */ + // private final Limelight limelight = new Limelight(); + + // private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight); + + // private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor); /* Controllers */ - private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); + private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + // private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2); + + + /* Virtual Controllers */ + // private final VirtualController m_virtualDriver = new VirtualController(0); + // private final VirtualController m_virtualOperator = new VirtualController(1); + + // private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); + // private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); + + // private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotIntake.PIDIn()), + // new InstantCommand(() -> m_robotShooter.idle()) + // // new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))), + // // new InstantCommand(() -> m_robotShooter.spin()) + // ); + + + // ! Teleop Commands + + //private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); + + // private SequentialCommandGroup autoShoot = new SequentialCommandGroup( + // // MoveToSpeaker, + // //autoAlign, + // new InstantCommand(() -> m_robotShooter.spin()), + // new WaitCommand(3.0), + // new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + // new WaitCommand(3.0), + // new InstantCommand(() -> m_robotShooter.idle()) + // // new InstantCommand(() -> autoAlign.reverse()), + // // autoAlign.asProxy() + // ); + + + // private SequentialCommandGroup i = new SequentialCommandGroup( + // intakeToShootStuff, intakeToShoot, + // new InstantCommand(() -> m_robotShooter.idle()) + // ); + + // private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + // new WaitCommand(0.75), + // new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake) + // ); + + // private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter) + // // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + // ); + + // private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( + // interrupt, + // new InstantCommand(() -> m_robotIntake.PIDIn(), m_robotIntake), + // new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) + // ); + + // private SequentialCommandGroup ampShoot = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotIntake.ampPosition()), + // new InstantCommand(() -> m_robotIntake.ampOuttake(0.1)) //TODO: Find Actual Speed + // ); + + // ! /* Autos */ + // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + // private String lastAutoName = "final_red_center_4note_taxi.auto"; + // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); + // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, + // () -> autoplaybackName.get(), // lastAutoName + // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + // true, false); + + + // private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) + // .addOption("Taxi Auto", taxi.asProxy()) + // .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy()) + // .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy()) + // // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy()) + // // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy()) + // .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy()) + // .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy()) + // .buildDisplay(); + + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - configureButtonBindings(); + configureButtonBindings(); + configureVirtualButtonBindings(); + // new Trigger(() -> autoplaybackName.get().equals(lastAutoName)).onTrue(new InstantCommand(() -> changeAuto())); + // new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); - /* Default Commands */ - // drives the robot with a two-axis input from the driver controller + + // DriverStation.silenceJoystickConnectionWarning(true); + // // CameraServer.startAutomaticCapture(); + + // /* Default Commands */ + // // drives the robot with a two-axis input from the driver controller + // ! Swerve Drive Default Command (Regular Rotation) + + // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + // m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft()); + // } + // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + // m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), + // getDeadbandedDriverController().getRight(), + // true); + // }, m_robotSwerveDrive) + // .withName("SwerveDrive DefaultCommand")); + // m_robotSwerveDrive.setToSlow(); + + // ! Swerve Drive Default Command (Orientation Rotation) + // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + // m_robotSwerveDrive.driveWithInputOrientation(getDeadbandedDriverController().getLeft(), + // getDeadbandedDriverController().getRightX(), + // getDeadbandedDriverController().getRightY(), + // true); + // }, m_robotSwerveDrive) + // .withName("SwerveDrive OrientationCommand")); // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + } + // private void changeAuto() { + // autoPlayback.unloadAuto(); + // autoPlayback.loadAuto(); + // lastAutoName = autoplaybackName.get(); + // System.out.println("AUTO: Changed auto to; `" + lastAutoName + "`"); + // } /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses @@ -66,26 +222,191 @@ public class RobotContainer { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - /* Driver Buttons */ - // test command to spin the robot while pressing A on the driver controller + + // ? /* Driver Buttons */ + + // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); + + // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); + + // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); + + // * /* D-Pad Stuff */ + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // ! /* Auto Recording */ + // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) + // .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, + // new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, + // () -> autoplaybackName.get())) + // .onFalse(new InstantCommand()); + + // new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, + // () -> autoplaybackName.get(), + // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + // true, false)) + // .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - // () -> getDeadbandedDriverController().getLeftX(), - // () -> getDeadbandedDriverController().getLeftY(), - // () -> getDeadbandedDriverController().getRightX(), - // () -> getDeadbandedDriverController().getRightY(), - // "Blue1Path.txt")) - // .onFalse(new InstantCommand()); + // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + // () -> getDeadbandedDriverController().getLeftX(), + // () -> getDeadbandedDriverController().getLeftY(), + // () -> getDeadbandedDriverController().getRightX(), + // () -> getDeadbandedDriverController().getRightY(), + // "Taxi.txt")) + // .onFalse(new InstantCommand()); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) - // .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) + // .onFalse(new InstantCommand()); + // ! /* Speed */ + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + // new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); + + // new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .whileTrue(new InstantCommand(() -> + // m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), + // new Translation2d(0, 0), + // true), m_robotSwerveDrive)); + + + //? /* Operator Buttons */ + + // new Joystick(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotMap.rightFront.go(new Translation2d()))); + + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.PIDOut())) + // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.handoff())) + // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); + + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) + // .onTrue(emergencyRetract); + + + // Override Intake Position encoder: out + // new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-55), m_robotIntake)); + + // // Override Intake Position encoder: in + // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-6.2), m_robotIntake)); + + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) + // .onFalse(turnOffShoot); + + + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(i) + // .onFalse(new InstantCommand(() -> m_robotIntake.PIDIn())); + + // //spins up shooter, no wind down + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); + + // // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) + // // .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor(), m_robotIntake)) + // // .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)); + + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) + // .onTrue(emergencyRetract); + + // new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) + // .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) + // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + // new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) + // .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) + // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + // new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) + // .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); + + } + + /** + * This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}.

+ * Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons. + */ + private void configureVirtualButtonBindings() { + + // ? /* Driver Buttons */ + + /* Notice: the following buttons have not been replicated + * Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback + * Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode. + * Auto Recording controls : We don't want an Null Ouroboros for an auto. + */ + + // ? /* Operator Buttons */ + + /* Notice: the following buttons have not been replicated + * Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot. + * We don't need it in an auto. + * Climbing controls : We don't need to climb in auto. + */ + + // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) + // .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) + // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) + // .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) + // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + // new Trigger(() -> getVirtualOperatorController().getPOV() == 0) + // .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); - /* Operator Buttons */ - // activates "Lit Mode" - // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - // .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - // .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); } /** @@ -94,8 +415,20 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // no auto - return new InstantCommand(); + //no auto + // return autoPlayback; + //return playbackChooser.getCommand(); + return new Command() {}; + } + + /** + * A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller} + * @param joystickA A controller + * @param joystickB A controller + * @param buttonNumber The button to bind to + */ + public Trigger DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) { + return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber))); } /** @@ -108,4 +441,12 @@ public class RobotContainer { public DeadbandedXboxController getDeadbandedOperatorController() { return this.m_operatorXbox; } + + // public VirtualController getVirtualDriverController() { + // return m_virtualDriver; + // } + + // public VirtualController getVirtualOperatorController() { + // return m_virtualOperator; + // } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index b7f03c4..aa9fba8 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,16 +7,20 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.PigeonIMU; -import com.ctre.phoenix.sensors.WPI_Pigeon2; +// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +// import com.ctre.phoenix.sensors.CANCoder; +// import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.CANcoder; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.ClimbConstants; +import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; @@ -25,8 +29,8 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { - private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); - public RobotGyro gyro = new RobotGyro(m_pigeon2); + // private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); + // public RobotGyro gyro = new RobotGyro(m_pigeon2); public SwerveModule leftFront; public SwerveModule rightFront; @@ -39,58 +43,118 @@ public class RobotMap { } /* LED Subsystem */ - public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); + public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); + public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); + /* Swreve Drive Subsystem */ - public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); - public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); - public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); + // public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); + // public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); + // public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); - public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); - public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); - public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); + + // public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); + // public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); + // public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); - public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); - public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); - public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); + // public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); + // public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); + // public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); - public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); - public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); - public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); + // public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); + // public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); + // public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); + + /* Shooter Subsystem */ + // public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID); + // public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID); + + /* Intake Subsystem */ + // public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); + // public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); + + /* Climber Subsystem */ + // public final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID); void configureLEDMotorControllers() { } + void configureIntakeMotorControllers() { + // intakeMotor.getConfigurator().apply(new TalonFXConfiguration()); + // pivotMotor.getConfigurator().apply(new TalonFXConfiguration()); + } + void configureDriveMotorControllers() { // config factory default - leftFrontWheel.configFactoryDefault(); - leftFrontSteer.configFactoryDefault(); + // leftFrontWheel.configFactoryDefault(); + // leftFrontSteer.configFactoryDefault(); - rightFrontWheel.configFactoryDefault(); - rightFrontSteer.configFactoryDefault(); + // rightFrontWheel.configFactoryDefault(); + // rightFrontSteer.configFactoryDefault(); - leftBackWheel.configFactoryDefault(); - leftBackSteer.configFactoryDefault(); + // leftBackWheel.configFactoryDefault(); + // leftBackSteer.configFactoryDefault(); - rightBackWheel.configFactoryDefault(); - rightBackSteer.configFactoryDefault(); + // rightBackWheel.configFactoryDefault(); + // rightBackSteer.configFactoryDefault(); - // set neutral mode - leftFrontWheel.setNeutralMode(NeutralMode.Brake); - rightFrontWheel.setNeutralMode(NeutralMode.Brake); - leftBackWheel.setNeutralMode(NeutralMode.Brake); - rightBackWheel.setNeutralMode(NeutralMode.Brake); + // // config open loop ramp + // leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - leftFrontSteer.setNeutralMode(NeutralMode.Brake); - rightFrontSteer.setNeutralMode(NeutralMode.Brake); - leftBackSteer.setNeutralMode(NeutralMode.Brake); - rightBackSteer.setNeutralMode(NeutralMode.Brake); + // // config closed loop ramp + // leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - // initialize SwerveModules - this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469); - this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234); - this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297); - this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142); + // // config neutral deadband + // leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // // set neutral mode + // leftFrontWheel.setNeutralMode(NeutralMode.Brake); + // rightFrontWheel.setNeutralMode(NeutralMode.Brake); + // leftBackWheel.setNeutralMode(NeutralMode.Brake); + // rightBackWheel.setNeutralMode(NeutralMode.Brake); + + // leftFrontSteer.setNeutralMode(NeutralMode.Brake); + // rightFrontSteer.setNeutralMode(NeutralMode.Brake); + // leftBackSteer.setNeutralMode(NeutralMode.Brake); + // rightBackSteer.setNeutralMode(NeutralMode.Brake); + + // // initialize SwerveModules + // this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); + // this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET); + // this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET); } } diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java new file mode 100644 index 0000000..58eb3ed --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java @@ -0,0 +1,208 @@ +// package frc4388.robot.commands.Autos; +// import frc4388.robot.subsystems.Limelight; +// import frc4388.robot.subsystems.SwerveDrive; +// import frc4388.robot.Constants.AutoAlignConstants; +// import frc4388.robot.Constants.VisionConstants; +// import edu.wpi.first.wpilibj2.command.Command; + +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.geometry.Rotation2d; + +// import java.util.Optional; + +// import org.opencv.core.RotatedRect; + +// import edu.wpi.first.math.geometry.Pose2d; + +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; + +// public class AutoAlign extends Command { +// private SwerveDrive swerve; +// private Limelight limelight; +// private Pose2d pose; +// private Translation2d targetPos; +// private Rotation2d targetRot; + +// private Optional alliance; +// private boolean isRed; + +// private boolean isFinished; +// private boolean isReverseFinished; + +// private boolean reverseAfterFinish; +// private Translation2d[] moveStickReplayArr; +// private Translation2d[] rotStickReplayArr; +// private int replayIndex; + +// // PID Stuff +// private double prevError; +// private double cumError; + +// public AutoAlign(SwerveDrive swerve, Limelight limelight) { +// this.swerve = swerve; +// this.limelight = limelight; +// } + +// // Calc the closest point on a circle, to the center of the speaker +// private Translation2d calcTargetPos(){ +// final double R = VisionConstants.targetPosDistance; +// final double cX; +// final double cY; +// if(isRed){ +// cX = VisionConstants.RedSpeakerCenter.getX(); +// cY = VisionConstants.RedSpeakerCenter.getY(); +// }else{ +// cX = VisionConstants.BlueSpeakerCenter.getX(); +// cY = VisionConstants.BlueSpeakerCenter.getY(); +// } +// final double pX = pose.getX(); +// final double pY = pose.getY(); + +// // Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point +// double vX = pX - cX; +// double vY = pY - cY; +// double magV = Math.sqrt(vX*vX + vY*vY); +// double aX = cX + vX / magV * R; +// double aY = cY + vY / magV * R; + +// return new Translation2d(aX, aY); +// } + +// // Calculate the angle facing the center, at the target rot +// private Rotation2d calcTargetRot() { +// final double R = VisionConstants.targetPosDistance; +// final double cX; +// final double cY; +// if(isRed){ +// cX = VisionConstants.RedSpeakerCenter.getX(); +// cY = VisionConstants.RedSpeakerCenter.getY(); +// }else{ +// cX = VisionConstants.BlueSpeakerCenter.getX(); +// cY = VisionConstants.BlueSpeakerCenter.getY(); +// } +// final double pX = pose.getX(); +// final double pY = pose.getY(); + +// final double dX = pX - cX; +// final double dY = pY - cY; + +// final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360); + +// return Rotation2d.fromDegrees(yaw); +// } + +// // Clamp to a circle, like an xbox controller +// private Translation2d clamp(double oldX, double oldY){ +// // Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley +// final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360; +// final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI)); +// final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI)); + +// final double newX = Math.max(-maxX, Math.min(maxX, oldX)); +// final double newY = Math.max(-maxY, Math.min(maxY, oldY)); + +// return new Translation2d(newX, newY); +// } + +// private Translation2d calcMoveStick(){ +// final double curX = pose.getX(); +// final double curY = pose.getY(); + +// // I think this might work, assuming the constants are good +// double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed; +// double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed; + +// return clamp(stickX, stickY); +// } + +// private Translation2d calcRotStick(){ +// double error = pose.getRotation().getDegrees() - targetRot.getDegrees(); +// cumError += error * .02; // 20 ms +// double delta = error - prevError; + +// final double kP = 4; +// final double kI = 4; +// final double kD = 4; +// final double kF = 4; + +// double output = error * kP; +// output += cumError * kI; +// output += delta * kD; +// output += kF; + +// prevError = error; +// return clamp(output, 0); +// } + +// public void reverse() { +// this.reverseAfterFinish = true; +// } + +// // Called when the command is initially scheduled. +// @Override +// public final void initialize() { +// isRed = alliance.get() == DriverStation.Alliance.Red; +// if(reverseAfterFinish){ +// // isReverseFinished = false; +// replayIndex = 0; +// }else{ +// moveStickReplayArr = new Translation2d[]{}; +// rotStickReplayArr = new Translation2d[]{}; +// } +// } + +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { +// // Update limelight pos +// pose = limelight.getPose(); + +// // These must be 0, or it will error +// Translation2d moveStick = new Translation2d(0, 0); +// Translation2d rotStick = new Translation2d(0, 0); + +// // Regular replay +// if(!isFinished){ +// targetPos = calcTargetPos(); +// targetRot = calcTargetRot(); + +// moveStick = calcMoveStick(); +// rotStick = calcRotStick(); + +// // This is a way of appending... +// moveStickReplayArr[moveStickReplayArr.length] = moveStick; +// rotStickReplayArr[rotStickReplayArr.length] = rotStick; + +// // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){ +// // replayIndex +// // } +// isFinished = limelight.isNearSpeaker(); + +// // If reverseAfterFinish, then loop back over and replay in reverse +// }else if(reverseAfterFinish && !isReverseFinished){ +// // Get reverse direction +// moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1]; +// rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1]; + +// // Invert sticks +// moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1); +// rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1); + +// replayIndex++; + +// if(replayIndex >= moveStickReplayArr.length){ +// reverseAfterFinish = true; +// } +// } + +// // This would greatly benifit from having feild Relative implemented. +// swerve.driveWithInput(moveStick, rotStick, true); +// } + +// // Returns true when the command should end. +// @Override +// public final boolean isFinished() { +// return isFinished && (isReverseFinished || !reverseAfterFinish); +// } +// } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index 2438a38..f3d636d 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -42,6 +42,7 @@ public class PlaybackChooser { m_playback = m_choosers.get(0); nextChooser(); + // ! This does not work, why? Shuffleboard.getTab("Auto Chooser") .add("Add Sequence", new InstantCommand(() -> nextChooser())) .withPosition(4, 0); @@ -66,9 +67,15 @@ public class PlaybackChooser { public void nextChooser() { SendableChooser chooser = m_choosers.get(m_cmdNum++); - for (String auto : m_dir.list()) { - chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); + String[] dirs = m_dir.list(); + + if(dirs != null){ // Fix funny error + for (String auto : dirs) { + chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); + } } + + for (var cmd_name : m_commandPool.keySet()) { chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); } diff --git a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt new file mode 100644 index 0000000..a65aea9 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt @@ -0,0 +1,20 @@ +AUTO file format + +HEADER static size 0x5 +0x00 BYTE NUM AXES: defualts to 6 +0x01 BYTE NUM POV: defualts to 1 +0x02 BYTE NUM CONTROLLERS: defualts to 2 +0x03 SHORT FRAMES: any value greator or equal than one. + +FRAME PER CONTROLLER: defualt size 0x34 +0x00 DOUBLE AXES[NUM AXES] +0x30 SHORT BUTTONS +0x32 SHORT POVs[NUM POV] + +FRAME: size varrys +FRAME PER CONTROLLER[NUM CONTROLLERS] +INT UNIXTIMESTAMP + +FILE: +HEADER +FRAME[FRAMES] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java new file mode 100644 index 0000000..86bc7b2 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java @@ -0,0 +1,107 @@ +// package frc4388.robot.commands.Autos; + +// import java.io.File; +// import java.util.ArrayList; +// import java.util.HashMap; + +// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; +// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import frc4388.robot.commands.Swerve.JoystickPlayback; +// import frc4388.robot.commands.Swerve.neoJoystickPlayback; +// import frc4388.robot.subsystems.SwerveDrive; +// import frc4388.utility.controller.VirtualController; + +// public class neoPlaybackChooser { +// private final SendableChooser m_teamChosser = new SendableChooser(); +// private final SendableChooser m_possitionChosser = new SendableChooser(); +// private final SendableChooser m_autoNameChosser = new SendableChooser(); + +// private final SwerveDrive m_swerve; +// private final VirtualController[] m_controllers; +// // private final ArrayList> m_choosers = new ArrayList<>(); +// // private SendableChooser m_playback = null; +// private final ArrayList m_widgets = new ArrayList<>(); +// // private final HashMap m_commandPool = new HashMap<>(); + +// // private final File m_dir = new File("/home/lvuser/autos/"); +// // private int m_cmdNum = 0; + +// // commands +// private Command m_noAuto = new InstantCommand(); + +// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) { +// m_teamChosser.addOption("Red", "red"); +// m_teamChosser.setDefaultOption("Blue", "blue"); +// m_teamChosser.addOption("Nuetral", "nuetral"); +// m_possitionChosser.addOption("AMP", "amp"); +// m_possitionChosser.setDefaultOption("Center", "center"); +// m_possitionChosser.addOption("Source", "source"); +// m_swerve = swerve; +// m_controllers = controllers; +// } + +// public neoPlaybackChooser addOption(String name, String option) { +// m_autoNameChosser.addOption(name, option); +// return this; +// } + +// // public PlaybackChooser buildDisplay() { +// // for (int i = 0; i < 10; i++) { +// // appendCommand(); +// // } +// // m_playback = m_choosers.get(0); +// // nextChooser(); + +// // // ! This does not work, why? +// // Shuffleboard.getTab("Auto Chooser") +// // .add("Add Sequence", new InstantCommand(() -> nextChooser())) +// // .withPosition(4, 0); +// // return this; +// // } + +// // This will be bound to a button for the time being +// public void render() { +// // var chooser = new SendableChooser(); +// // // chooser.setDefaultOption("No Auto", m_noAuto); + +// // m_choosers.add(chooser); +// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser") +// .add("Command: " + m_choosers.size(), chooser) +// .withSize(4, 1) +// .withPosition(0, m_choosers.size() - 1) +// .withWidget(BuiltInWidgets.kSplitButtonChooser) +// .withWidget(BuiltInWidgets.kComboBoxChooser); + + +// m_widgets.add(widget); +// } + +// // public void nextChooser() { +// // SendableChooser chooser = m_choosers.get(m_cmdNum++); + +// // String[] dirs = m_dir.list(); + +// // if(dirs != null){ // Fix funny error +// // for (String auto : dirs) { +// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); +// // } +// // } + + +// // for (var cmd_name : m_commandPool.keySet()) { +// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); +// // } +// // } + +// public String autoName() { +// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto"; +// } + +// public Command getCommand() { +// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true); +// } +// } diff --git a/src/main/java/frc4388/robot/commands/PID.java b/src/main/java/frc4388/robot/commands/PID.java index fdbda0b..97233f8 100644 --- a/src/main/java/frc4388/robot/commands/PID.java +++ b/src/main/java/frc4388/robot/commands/PID.java @@ -4,10 +4,10 @@ package frc4388.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import frc4388.utility.Gains; -public abstract class PID extends CommandBase { +public abstract class PID extends Command { protected Gains gains; private double output = 0; private double tolerance = 0; diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index 56e8d43..e92b487 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -119,7 +119,11 @@ public class JoystickPlayback extends CommandBase { // new Translation2d(out.rightX, out.rightY), // true); - this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), + // this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), + // new Translation2d(lerpRX, lerpRY), + // true); + + this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY), new Translation2d(lerpRX, lerpRY), true); @@ -138,4 +142,4 @@ public class JoystickPlayback extends CommandBase { public boolean isFinished() { return m_finished; } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index 84608cc..82ba36e 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -64,11 +64,11 @@ public class JoystickRecorder extends CommandBase { outputs.add(inputs); - swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), + swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY), new Translation2d(inputs.rightX, inputs.rightY), true); - System.out.println("RECORDING"); + //System.out.println("RECORDING"); } // Called once the command ends or is interrupted. @@ -94,4 +94,4 @@ public class JoystickRecorder extends CommandBase { public boolean isFinished() { return false; } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java new file mode 100644 index 0000000..8b5afdf --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java @@ -0,0 +1,197 @@ +package frc4388.robot.commands.Swerve; + +import java.io.FileInputStream; +import java.util.ArrayList; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.DataUtils; +import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.UtilityStructs.AutoRecordingFrame; +import frc4388.utility.controller.VirtualController; + + +/** + * The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s + * @author Zachary Wilke + */ +public class neoJoystickPlayback extends Command { + private final SwerveDrive swerve; + private final VirtualController[] controllers; + private final ArrayList frames = new ArrayList<>(); + private final Supplier filenameGetter; + private String filename; + private int frame_index = 0; + private long startTime = 0; + private long playbackTime = 0; + private boolean m_finished = false; // ! There is no better way. + private boolean m_shouldfree = false; // should free memory on ending + + private byte m_numAxes = 0; + private byte m_numPOVs = 0; + private byte m_numControllers = 0; + private short m_numFrames = -1; + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param shouldfree Unloads the auto on compleation or intruption. + * @param instantload Load the auto on object instantiation + */ + public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) { + this.swerve = swerve; + this.filenameGetter = filenameGetter; + this.controllers = controllers; + this.m_shouldfree = shouldfree; + + if (instantload) loadAuto(); + addRequirements(this.swerve); + } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filename a String containing the name of the auto file you wish to playback. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param shouldfree unloads the auto on compleation or intruption. + * @param instantload load the auto on object instantiation + */ + public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) { + this(swerve, () -> filename, controllers, shouldfree, instantload); + } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + */ + public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers) { + this(swerve, filenameGetter, controllers, true, false); + } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filename a String containing the name of the auto file you wish to playback. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + */ + public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) { + this(swerve, () -> filename, controllers, true, false); + } + + /** + * Load the auto file from disk into memory + * @return Returns true if loading was successful, else wise; return false + * @implNote if the auto is already loaded, it will return true. + */ + public boolean loadAuto() { + filename = filenameGetter.get(); + try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) { + if (m_numFrames != -1 && m_numFrames == frames.size()) { + System.out.println("AUTOPLAYBACK: Auto Already loaded."); + return true; + } + + m_numAxes = stream.readNBytes(1)[0]; + m_numPOVs = stream.readNBytes(1)[0]; + m_numControllers = stream.readNBytes(1)[0]; + m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2)); + + if (m_numControllers > controllers.length) { + System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers + + " virtual controllers but only " + controllers.length + " were given"); + return false; + } + + for (int i = 0; i < m_numFrames; i++) { + AutoRecordingFrame frame = new AutoRecordingFrame(); + for (int j = 0; j < m_numControllers; j++) { + AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); + double[] axes = new double[m_numAxes]; + for (int k = 0; k < m_numAxes; k++) { // we love third level for loops. + axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8)); + } + short button = DataUtils.byteArrayToShort(stream.readNBytes(2)); + short[] POV = new short[m_numPOVs]; + for (int k = 0; k < m_numPOVs; k++) { + POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2)); + } + controllerFrame.axes = axes; + controllerFrame.button = button; + controllerFrame.POV = POV; + frame.controllerFrames[j] = controllerFrame; + } + frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4)); + frames.add(frame); + } + + System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long"); + return true; + + } catch (Exception e) { + e.printStackTrace(); + System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`'); + return false; + } + } + + /** + * Unloads the auto. + */ + public void unloadAuto() { + System.out.println("AUTOPLAYBACK: Auto unloaded"); + frames.clear(); + } + + @Override + public void initialize() { + startTime = System.currentTimeMillis(); + playbackTime = 0; + frame_index = 0; + + m_finished = !loadAuto(); + } + + @Override + public void execute() { + if (frame_index >= m_numFrames) m_finished = true; + if (m_finished) return; + + // if (frame_index == 0) { + // startTime = System.currentTimeMillis(); + // playbackTime = 0; + // } else { + // playbackTime = System.currentTimeMillis() - startTime; + // } + + AutoRecordingFrame frame = frames.get(frame_index); + for (int i = 0; i < controllers.length; i++) { + AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i]; + controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV); + if (i == 0) { + this.swerve.driveWithInput( + new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)), + new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)), + true); + } + } + frame_index++; + } + + @Override + public void end(boolean interrupted) { + for (VirtualController controller : controllers) controller.zeroControls(); + swerve.stopModules(); + if (m_shouldfree) unloadAuto(); + } + + @Override + public boolean isFinished() { + return m_finished; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java new file mode 100644 index 0000000..7f48a6c --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java @@ -0,0 +1,129 @@ +package frc4388.robot.commands.Swerve; + +import java.io.FileOutputStream; +import java.util.ArrayList; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.DataUtils; +import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.UtilityStructs.AutoRecordingFrame; +import frc4388.utility.controller.DeadbandedXboxController; + +/** + * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s + * @author Zachary Wilke + */ +public class neoJoystickRecorder extends Command { + private final SwerveDrive swerve; + private final XboxController[] controllers; + private String filename; + private final Supplier filenameGetter; + private long startTime = -1; + private final ArrayList frames = new ArrayList<>(); + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. + */ + public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier filenameGetter) { + this.swerve = swerve; + this.controllers = controllers; + this.filenameGetter = filenameGetter; + this.filename = ""; + + addRequirements(this.swerve); + } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param filename a String containing the name of the auto file you wish to playback. + */ + public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) { + this(swerve, controllers, () -> filename); + } + + @Override + public void initialize() { + frames.clear(); + + this.startTime = System.currentTimeMillis(); + AutoRecordingFrame frame = new AutoRecordingFrame(); + frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()}; + frames.add(frame); + this.filename = this.filenameGetter.get(); + } + + + @Override + public void execute() { + System.out.println("AUTORECORD: RECORDING"); + AutoRecordingFrame frame = new AutoRecordingFrame(); + frame.timeStamp = (int) (System.currentTimeMillis() - startTime); + for (int i = 0; i < controllers.length; i++) { + XboxController controller = controllers[i]; + AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); + double[] axes = {controller.getLeftX(), controller.getLeftY(), + controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(), + controller.getRightX(), controller.getRightY()}; + short button = 0; + for (int j = 0; j < 10; j++) + if (controller.getRawButton(j+1)) + button |= 1 << j; + short[] POV = {(short)(controller.getPOV())}; + controllerFrame.axes = axes; + controllerFrame.button = button; + controllerFrame.POV = POV; + frame.controllerFrames[i] = controllerFrame; + } + + frames.add(frame); + + swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]), + new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), + true); // Really jank way of doing this. + + } + @Override + public void end(boolean interrupted) { + try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) { + // header: size of 0x5 + // byte Number of axes per controller + // byte Number of POVs per controller + // byte Number of controllers + // short Number of frames + stream.write(new byte[]{6, 1, (byte) controllers.length}); + stream.write(DataUtils.shortToByteArray((short) frames.size())); + + // frame + // controller frame * number of controllers + // int unix time stamp. + for (AutoRecordingFrame frame : frames) { + // controller frame + // double axis * Number of axes per controller + // short button states + // short POV * Number of POVs per controller + for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) { + for (double axis: controllerFrame.axes) { + stream.write(DataUtils.doubleToByteArray(axis)); + } + stream.write(DataUtils.shortToByteArray(controllerFrame.button)); + for (short POV: controllerFrame.POV) { + stream.write(DataUtils.shortToByteArray(POV)); + } + } + stream.write(DataUtils.intToByteArray(frame.timeStamp)); + } + System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long."); + } catch (Exception e) { + e.printStackTrace(); + } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 055230b..965bbcb 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,17 +4,18 @@ package frc4388.robot.subsystems; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -public class SwerveDrive extends SubsystemBase { +public class SwerveDrive extends SubsystemBase { private SwerveModule leftFront; private SwerveModule rightFront; @@ -24,17 +25,20 @@ public class SwerveDrive extends SubsystemBase { private SwerveModule[] modules; private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); private RobotGyro gyro; - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default + public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default + public double rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public double rotTarget = 0.0; + public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ @@ -51,14 +55,51 @@ public class SwerveDrive extends SubsystemBase { boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + rightFront.go(leftStick); + // if (fieldRelative) { + + // double rot = 0; + + // // ! drift correction + // if (rightStick.getNorm() > 0.05) { + // rotTarget = gyro.getAngle(); + // rot = rightStick.getX(); + // // SmartDashboard.putBoolean("drift correction", false); + // stopped = false; + // } else if(leftStick.getNorm() > 0.05) { + // if (!stopped) { + // stopModules(); + // stopped = true; + // } + + // // SmartDashboard.putBoolean("drift correction", true); + + // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + + // } + + // // Use the left joystick to set speed. Apply a cubic curve and the set max speed. + // Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + // // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); + + // // Convert field-relative speeds to robot-relative speeds. + // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1)); + // } else { // Create robot-relative speeds. + // chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + // } + // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + } + + public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (fieldRelative) { double rot = 0; + // ! drift correction if (rightStick.getNorm() > 0.05) { rotTarget = gyro.getAngle(); rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; - SmartDashboard.putBoolean("drift correction", false); + // SmartDashboard.putBoolean("drift correction", false); stopped = false; } else if(leftStick.getNorm() > 0.05) { if (!stopped) { @@ -66,36 +107,63 @@ public class SwerveDrive extends SubsystemBase { stopped = true; } - SmartDashboard.putBoolean("drift correction", true); + // SmartDashboard.putBoolean("drift correction", true); + rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; } // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust); // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); - } else { - // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); + } else { // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED); } - setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } + public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) { + + Translation2d rightStick = new Translation2d(-rightX, rightY); + + if(fieldRelative) { + double rot = 0; + if(rightStick.getNorm() > 0.5) { + orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); + + Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5)); + double min = tmp.getDegrees(); + min = Math.max(Math.abs(min), 2); + if(tmp.getDegrees() < 0) + min*=-1; + tmp = new Rotation2d(min * Math.PI / 180); + rot = tmp.getRadians(); // x x - y/x + } + + Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1); + } else { // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + } + // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + } + /** * Set each module of the swerve drive to the corresponding desired state. * @param desiredStates Array of module states to set. */ - public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - for (int i = 0; i < desiredStates.length; i++) { - SwerveModule module = modules[i]; - SwerveModuleState state = desiredStates[i]; - module.setDesiredState(state); - } - } + // public void setModuleStates(SwerveModuleState[] desiredStates) { + // SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + // for (int i = 0; i < desiredStates.length; i++) { + // SwerveModule module = modules[i]; + // SwerveModuleState state = desiredStates[i]; + // module.setDesiredState(state); + // } + // } public boolean rotateToTarget(double angle) { double currentAngle = getGyroAngle(); @@ -114,9 +182,30 @@ public class SwerveDrive extends SubsystemBase { return gyro.getAngle(); } + public void add180() { + gyro.reset(gyro.getAngle()+180); + rotTarget = gyro.getAngle(); + + } + public void resetGyro() { gyro.reset(); - rotTarget = 0.0; + rotTarget = gyro.getAngle(); + } + + public void resetGyroFlip() { + gyro.resetFlip(); + rotTarget = gyro.getAngle(); + } + + public void resetGyroRightBlue() { + gyro.resetRightSideBlue(); + rotTarget = gyro.getAngle(); + } + + public void resetGyroRightAmp() { + gyro.resetAmpSide(); + rotTarget = gyro.getAngle(); } public void stopModules() { @@ -129,10 +218,15 @@ public class SwerveDrive extends SubsystemBase { return this.kinematics; } + public boolean getSpeedState() { + + return false; + } + @Override public void periodic() { // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); + // SmartDashboard.putNumber("Gyro", getGyroAngle()); } public void shiftDown() { @@ -142,6 +236,7 @@ public class SwerveDrive extends SubsystemBase { this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; } else { this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + } } @@ -192,4 +287,14 @@ public class SwerveDrive extends SubsystemBase { } } + public void shiftUpRot() { + rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; + } + + public void shiftDownRot() { + rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + } + + + } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 1ddab51..ef0d062 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -4,56 +4,92 @@ package frc4388.robot.subsystems; +// import javax.swing.text.StyleContext.SmallAttributeSet; + +// import com.ctre.phoenix.ErrorCode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.CANCoder; +// import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +// import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; +// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +// import com.ctre.phoenix.sensors.CANCoder; +// import com.ctre.phoenix.sensors.SensorInitializationStrategy; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.hardware.CANcoder; + +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; +// import frc4388.utility.configurable.ConfigurableDouble; public class SwerveModule extends SubsystemBase { - private WPI_TalonFX driveMotor; - private WPI_TalonFX angleMotor; - private CANCoder encoder; - + private TalonFX driveMotor; + private TalonFX angleMotor; + private CANcoder encoder; + // private int selfid; + // private ConfigurableDouble offsetGetter; + private static int swerveId = 0; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; + /** Creates a new SwerveModule. */ - public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) { + public SwerveModule(TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.encoder = encoder; + // // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); + // this.selfid = swerveId; + // swerveId++; + // TalonFXConfiguration angleConfig = new TalonFXConfiguration(); + // angleConfig.slot0.kP = swerveGains.kP; + // angleConfig.slot0.kI = swerveGains.kI; + // angleConfig.slot0.kD = swerveGains.kD; - TalonFXConfiguration angleConfig = new TalonFXConfiguration(); - angleConfig.slot0.kP = swerveGains.kP; - angleConfig.slot0.kI = swerveGains.kI; - angleConfig.slot0.kD = swerveGains.kD; - - // use the CANcoder as the remote sensor for the primary TalonFX PID - angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); - angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; - angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; - angleMotor.configAllSettings(angleConfig); - - encoder.configMagnetOffset(offset); + // // use the CANcoder as the remote sensor for the primary TalonFX PID + // angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); + // angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; + // angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; + // angleMotor.configAllSettings(angleConfig); - driveMotor.setSelectedSensorPosition(0); - driveMotor.config_kP(0, 0.2); + // //encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); + // reset(0); + // encoder.configMagnetOffset(offset); + + // driveMotor.setSelectedSensorPosition(0); + // driveMotor.config_kP(0, 0.2); } + public void go(Translation2d leftStick){ + System.out.println(leftStick.getY()); + driveMotor.set(leftStick.getY()); + } + + @Override + public void periodic() { + //encoder.configMagnetOffset(offsetGetter.get()); + //SmartDashboard.putString("Error Code: " + selfid, getstuff()); + // SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees()); + // SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel()); + // SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos()); + // SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel()); + } /** * Get the drive motor of the SwerveModule * @return the drive motor of the SwerveModule */ - public WPI_TalonFX getDriveMotor() { + public TalonFX getDriveMotor() { return this.driveMotor; } @@ -61,7 +97,7 @@ public class SwerveModule extends SubsystemBase { * Get the angle motor of the SwerveModule * @return the angle motor of the SwerveModule */ - public WPI_TalonFX getAngleMotor() { + public TalonFX getAngleMotor() { return this.angleMotor; } @@ -69,7 +105,7 @@ public class SwerveModule extends SubsystemBase { * Get the CANcoder of the SwerveModule * @return the CANcoder of the SwerveModule */ - public CANCoder getEncoder() { + public CANcoder getEncoder() { return this.encoder; } @@ -79,19 +115,23 @@ public class SwerveModule extends SubsystemBase { */ public Rotation2d getAngle() { // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. - return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); + // return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); + return new Rotation2d(); } public double getAngularVel() { - return this.angleMotor.getSelectedSensorVelocity(); + // return this.angleMotor.getSelectedSensorVelocity(); + return 0; } public double getDrivePos() { - return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; + // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; + return 0; } public double getDriveVel() { - return this.driveMotor.getSelectedSensorVelocity(0); + // return this.driveMotor.getSelectedSensorVelocity(0); + return 0.0; } public void stop() { @@ -100,62 +140,67 @@ public class SwerveModule extends SubsystemBase { } public void rotateToAngle(double angle) { - angleMotor.set(TalonFXControlMode.Position, angle); + // angleMotor.set(TalonFXControlMode.Position, angle); } /** * Get state of swerve module * @return speed in m/s and angle in degrees */ - public SwerveModuleState getState() { - return new SwerveModuleState( - Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, - getAngle() - ); - } + // public SwerveModuleState getState() { + // return new SwerveModuleState( + // Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, + // getAngle() + // ); + // } /** * Returns the current position of the SwerveModule * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor. - */ - public SwerveModulePosition getPosition() { - return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); - } + // */ + // public SwerveModulePosition getPosition() { + // return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); + // } /** * Set the speed and rotation of the SwerveModule from a SwerveModuleState object * @param desiredState a SwerveModuleState representing the desired new state of the module - */ - public void setDesiredState(SwerveModuleState desiredState) { - Rotation2d currentRotation = this.getAngle(); + // */ + // public void setDesiredState(SwerveModuleState desiredState) { + // Rotation2d currentRotation = this.getAngle(); - SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); + // SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); - // calculate the difference between our current rotational position and our new rotational position - Rotation2d rotationDelta = state.angle.minus(currentRotation); + // // calculate the difference between our current rotational position and our new rotational position + // Rotation2d rotationDelta = state.angle.minus(currentRotation); - // calculate the new absolute position of the SwerveModule based on the difference in rotation - double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; + // // calculate the new absolute position of the SwerveModule based on the difference in rotation + // double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; - // convert the CANCoder from its position reading to ticks - double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); + // // convert the CANCoder from its position reading to ticks + // double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); - angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + // angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); - double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); + // double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - } + // driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + // } - public void reset(double position) { - encoder.setPositionToAbsolute(); - } + // public void reset(double position) { + // encoder.setPositionToAbsolute(); + // } - public double getCurrent() { - return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); - } + // public double getCurrent() { + // return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); + // } - public double getVoltage() { - return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); - } + // public double getVoltage() { + // return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); + // } + + // public String getstuff() { + // encoder.getPosition(); + // return "" + encoder.getLastError().value; + // } } diff --git a/src/main/java/frc4388/utility/DataUtils.java b/src/main/java/frc4388/utility/DataUtils.java new file mode 100644 index 0000000..3d998b6 --- /dev/null +++ b/src/main/java/frc4388/utility/DataUtils.java @@ -0,0 +1,35 @@ +package frc4388.utility; + +import java.nio.ByteBuffer; + +public class DataUtils { + public static byte[] doubleToByteArray(double value) { + byte[] bytes = new byte[8]; + ByteBuffer.wrap(bytes).putDouble(value); + return bytes; + } + + public static double byteArrayToDouble(byte[] bytes) { + return ByteBuffer.wrap(bytes).getDouble(); + } + + public static byte[] intToByteArray(int value) { + byte[] bytes = new byte[4]; + ByteBuffer.wrap(bytes).putInt(value); + return bytes; + } + + public static int byteArrayToInt(byte[] bytes) { + return ByteBuffer.wrap(bytes).getInt(); + } + + public static byte[] shortToByteArray(short value) { + byte[] bytes = new byte[2]; + ByteBuffer.wrap(bytes).putShort(value); + return bytes; + } + + public static short byteArrayToShort(byte[] bytes) { + return ByteBuffer.wrap(bytes).getShort(); + } +} diff --git a/src/main/java/frc4388/utility/DualJoystickButton.java b/src/main/java/frc4388/utility/DualJoystickButton.java new file mode 100644 index 0000000..e4ef5ed --- /dev/null +++ b/src/main/java/frc4388/utility/DualJoystickButton.java @@ -0,0 +1,22 @@ +package frc4388.utility; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.Trigger; + + +/** + * A button binding for two controllers, preferably an {@link frc4388.utility.controller.DeadbandedXboxController Xbox Controller} and {@link frc4388.utility.controller.VirtualController Virtual Xbox Controller} + * @author Zachary Wilke + */ +public class DualJoystickButton extends Trigger { + + /** + * Creates an Button binding on two controllers + * @param joystickA A controller + * @param joystickB A controller + * @param buttonNumber The button to bind to + */ + public DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) { + super(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber))); + } +} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index a037914..294dd6c 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -86,11 +86,12 @@ public class RobotGyro implements Gyro { */ @Override public void calibrate() { - if (m_isGyroAPigeon) { - m_pigeon.calibrate(); - } else { - m_navX.calibrate(); - } + return; + // if (m_isGyroAPigeon) { + // m_pigeon.calibrate(); + // } else { + // m_navX.calibrate(); + // } } @Override @@ -102,6 +103,73 @@ public class RobotGyro implements Gyro { } else { m_navX.reset(); } + + } + + public void reset(double val) { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(val); + } else { + m_navX.reset(); + } + + } + + public void resetFlip() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(180); + } else { + m_navX.reset(); + } + + } + + public void resetNinety() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(90); + } else { + m_navX.reset(); + } + + } + + public void resetTwoSeventy() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(270); + } else { + m_navX.reset(); + } + + } + + public void resetRightSideBlue() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(60); + } else { + m_navX.reset(); + } + + } + + public void resetAmpSide() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(-60); + } else { + m_navX.reset(); + } + } /** diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/UtilityStructs.java index e8b10cc..935dbbe 100644 --- a/src/main/java/frc4388/utility/UtilityStructs.java +++ b/src/main/java/frc4388/utility/UtilityStructs.java @@ -6,7 +6,21 @@ public class UtilityStructs { public double leftY = 0.0; public double rightX = 0.0; public double rightY = 0.0; + + public boolean OPLB; + public boolean OPRB; + public long timedOffset = 0; } + public static class AutoRecordingControllerFrame { + public double[] axes = new double[6]; + public short button = 0; + public short[] POV = new short[1]; + + } + public static class AutoRecordingFrame { + public AutoRecordingControllerFrame[] controllerFrames = new AutoRecordingControllerFrame[2]; + public int timeStamp; + } } diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java new file mode 100644 index 0000000..c0384db --- /dev/null +++ b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java @@ -0,0 +1,23 @@ +package frc4388.utility.configurable; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ConfigurableDouble { + private double defualtValue; + private String name; + + /** + * Creates an new ConfigurableDouble through Smart Dashboard. + * @param name the name of the Smart Dashboard key. + * @param defualtValue the initilization value + */ + public ConfigurableDouble(String name, double defualtValue) { + this.name = name; + this.defualtValue = defualtValue; + SmartDashboard.putNumber(name, defualtValue); + } + + public double get() { + return SmartDashboard.getNumber(name, defualtValue); + } +} diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableString.java b/src/main/java/frc4388/utility/configurable/ConfigurableString.java new file mode 100644 index 0000000..34c0290 --- /dev/null +++ b/src/main/java/frc4388/utility/configurable/ConfigurableString.java @@ -0,0 +1,23 @@ +package frc4388.utility.configurable; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ConfigurableString { + private String defualtValue; + private String name; + + /** + * Creates an new ConfigurableString through Smart Dashboard. + * @param name the name of the Smart Dashboard key. + * @param defualtValue the initilization value + */ + public ConfigurableString(String name, String defualtValue) { + this.name = name; + this.defualtValue = defualtValue; + SmartDashboard.putString(name, defualtValue); + } + + public String get() { + return SmartDashboard.getString(name, defualtValue); + } +} diff --git a/src/main/java/frc4388/utility/controller/VirtualController.java b/src/main/java/frc4388/utility/controller/VirtualController.java new file mode 100644 index 0000000..85adb64 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/VirtualController.java @@ -0,0 +1,145 @@ +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj.GenericHID; + +/** + * A virtual controller that can be bound like an standard controller. + * @author Zachary Wilke + */ +public class VirtualController extends GenericHID { + private short m_buttonStates = 0; + private short m_buttonStatesLastFrame = 0; + private double[] m_axes = new double[6]; + private short[] m_pov = new short[1]; + + /** + * Create an virtual controller + * @param port virtual port (merely a formality). + */ + public VirtualController(int port) { + super(port); + } + + /** + * Set the curent inputs to the new frames. + * @param axes joystick axes, (i.e. joysticks and triggers). + * @param buttonFlags the bit packed button states. + * @param pov the array of dpads. + */ + public void setFrame(double[] axes, short buttonFlags, short[] pov) { + m_axes = axes; + setOutputs(buttonFlags); + m_pov = pov; + } + + /** + * Zero outs the controls. + */ + public void zeroControls() { + m_axes = new double[6]; + m_buttonStates = 0; + m_buttonStatesLastFrame = 0; + m_pov = new short[] {-1}; + } + + /** + * Gets the value of a bitflag from an int + * @param value int to search + * @param index index of bit + * @return if the bit is set + */ + public static boolean getFlag(int value, int index) { + return ((value & 1 << index) != 0); + } + + @Override + public boolean getRawButton(int button) { // man why are buttons indexed at 1. + return getFlag(m_buttonStates, button - 1); + } + + @Override + public boolean getRawButtonPressed(int button) { + return (!getFlag(m_buttonStatesLastFrame, button - 1) && getRawButton(button)); + } + + @Override + public boolean getRawButtonReleased(int button) { + return (getFlag(m_buttonStatesLastFrame, button - 1) && !getRawButton(button)); + } + + @Override + public double getRawAxis(int axis) { + return m_axes[axis]; + } + + @Override + public int getPOV(int pov) { + return m_pov[pov]; + } + + @Override + public int getAxisCount() { + return m_axes.length; + } + + @Override + public int getPOVCount() { + return m_pov.length; + } + + @Override + public int getButtonCount() { + return 10; + } + + @Override + public boolean isConnected() { + return true; + } + + @Override + public HIDType getType() { + return HIDType.kXInputGamepad; + } + + @Override + public String getName() { + return "Virtual Controller"; + } + + @Override + public int getAxisType(int axis) { + return 1; /* ! Warning, does not return accurate data. + Hopefully this isn't a problem */ + } + + /** + * Use {@link VirtualController#setFrame} or {@link VirtualController#setOutputs}. + * this is an no-op overide. + */ + @Override + public void setOutput(int outputNumber, boolean value) { + // do not use + //m_buttonStatesLastFrame[outputNumber - 1] = m_buttonStates[outputNumber - 1]; + //m_buttonStates[outputNumber - 1] = value; + } + + /** + * Set buttons from a packed int, if you want to set joysticks and dpad use {@link VirtualController#SetFrame} + */ + @Override + public void setOutputs(int value) { + m_buttonStatesLastFrame = m_buttonStates; + m_buttonStates = (short) value; + } + + /** + * Why are you Setting rumble on an virtual controller? + * @param type the rumble type (even though it won't do anything) + * @param value the rumble strength (always multiplyed by 0.0) + */ + @Override + public void setRumble(RumbleType type, double value) { + System.out.println("Why are you Setting rumble on an virtual controller?"); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java index 8c2fe88..0a56841 100644 --- a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java +++ b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java @@ -1,13 +1,13 @@ package frc4388.utility.controller; -import edu.wpi.first.wpilibj2.command.button.Button; +//import edu.wpi.first.wpilibj2.command.button.Trigger; /** * Mapping for the Xbox controller triggers to allow triggers to be defined as * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger * exceeds a tolerance defined in {@link XboxController}. */ -public class XboxTriggerButton extends Button { +public class XboxTriggerButton {//extends Trigger { public static final int RIGHT_TRIGGER = 0; public static final int LEFT_TRIGGER = 1; public static final int RIGHT_AXIS_UP_TRIGGER = 2; diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json index 33d81f6..e978a5f 100644 --- a/vendordeps/NavX.json +++ b/vendordeps/NavX.json @@ -1,17 +1,18 @@ { "fileName": "NavX.json", - "name": "KauaiLabs_navX_FRC", - "version": "2023.0.4", + "name": "NavX", + "version": "2024.1.0", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", "mavenUrls": [ - "https://dev.studica.com/maven/release/2023/" + "https://dev.studica.com/maven/release/2024/" ], - "jsonUrl": "https://dev.studica.com/releases/2023/NavX.json", + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", "javaDependencies": [ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-java", - "version": "2023.0.4" + "version": "2024.1.0" } ], "jniDependencies": [], @@ -19,7 +20,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-cpp", - "version": "2023.0.4", + "version": "2024.1.0", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false, diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 0000000..ff7359e --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.1", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json new file mode 100644 index 0000000..0322385 --- /dev/null +++ b/vendordeps/Phoenix6.json @@ -0,0 +1,339 @@ +{ + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.3.0", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "24.3.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "24.3.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.3.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "24.3.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.3.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.3.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.3.0", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.3.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.3.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.3.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.3.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.3.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.3.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index bd535bf..67bf389 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,6 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2024", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [