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/README.md b/README.md index 9020b7f..981c2e9 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,34 @@ -# Robot-Essentials - Basic code for any Ridgebotics robot project +# FIRST FRC 2024 Season (Crescendo) "Kickdrum" Robot Code. +Code for our robot named "Kickdrum" + +## Driving instructions + +### Driver (Port 0) +**A**: Reset the gryo. + +**Left Bumper**: Gear shift down. + +**Right Bumper**: Gear shift up. + +### Operator (Port 1) + +**Right Bumper**: Moves out the intake and starts intaking a note, when the note enters the intake move the intake back into the robot, then idles the shooter. + +**Left Bumper**: Spins up the shooter to speaker speed. + +**A**: *Outtakes* the *intake*. If the intake is in the robot then it will pass it off to the shooter (and its recomended that the shooter is at speaker speed when it gets handed off), if its outside the robot it will just eject it onto the floor. + +**Right Trigger**: While held it moves the climbers up. + +**Left Trigger**: While held it moves the climbers down. + +### Programer (Port 2) + +**Left Bumper**: While pressed it records using the new autonomus recording and playback system, and when released it saves the autonomus to the robot under the name that you put into shuffleboard in the "Auto Playback Name" section. (**Warning**: If the robot gets reimaged (Not redeployed) it will lose all of the auto files and you are stuck using a tool like SCP to send the autos back to the robot from this repository) + +**Right Bumper**: When pressed it loads the auto from the "Auto Playback Name" in shuffleboard with the new autonomus recording and playback system, and plays it back. If the file you put in doesn't exist in autos/ in the home directory of the robot it will print the error to console but the robot will remain functional. (**Warning**: If the robot gets reimaged (Not redeployed) it will lose all of the auto files and you are stuck using a tool like SCP to send the autos back to the robot from this repository) + +### Shuffleboard + +**Auto Playback Name**: a string value of the auto name you wish to load or save to, this gets read at the start of the atonomus phase and loads the corisponding auto from ~/autos/ on the robot + 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..9638f90 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.2" } -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..7027b5e 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,82 +23,98 @@ 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[] GEARS = {0.25, 0.5, 1.0}; + + public static final double SLOW_SPEED = 0.25; + public static final double FAST_SPEED = 0.5; + public static final double TURBO_SPEED = 1.0; - public static final double SLOW_SPEED = 0.8; - 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 = 0.36962890625 + 0.5; + public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 + 0.5; + public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875 + 0.5; + public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 + 0.5; + } + 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_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 RIGHT_FRONT_WHEEL_ID = 2; + public static final int RIGHT_FRONT_STEER_ID = 3; + public static final int RIGHT_FRONT_ENCODER_ID = 10; + + 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 int DRIVE_PIGEON_ID = 14; } 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(50, 0.0, 0.32, 0.0, 0, 0.0); + + public static final Gains TEST_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 double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22; + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5; + + 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 = 0.5; + 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; + public static final double CLOSED_LOOP_RAMP_RATE = 0.2; + public static final double NEUTRAL_DEADBAND = 0.04; } - public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value - public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value + public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; + public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // dimensions public static final double WIDTH = 18.5; @@ -111,30 +127,19 @@ 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 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 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 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 class VisionConstants { + // 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 DriveConstants { - public static final int DRIVE_PIGEON_ID = 6; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } @@ -145,9 +150,38 @@ 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; + public static final int XBOX_PROGRAMMER_ID = 2; public static final double LEFT_AXIS_DEADBAND = 0.1; } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e555b09..58adaea 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -12,7 +12,7 @@ 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 +22,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 +41,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 +122,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..10caf89 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,20 +7,39 @@ package frc4388.robot; -import edu.wpi.first.wpilibj.Joystick; +// Drive Systems +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.wpilibj.GenericHID; +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; + +// Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc4388.robot.Constants.*; -import frc4388.robot.commands.Swerve.JoystickPlayback; -import frc4388.robot.commands.Swerve.JoystickRecorder; -import frc4388.robot.subsystems.LED; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// Autos +import frc4388.robot.commands.Intake.ArmIntakeIn; +import frc4388.utility.controller.VirtualController; +import frc4388.robot.commands.Swerve.neoJoystickPlayback; +import frc4388.robot.commands.Swerve.neoJoystickRecorder; + +// Subsystems +import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Limelight; +import frc4388.robot.subsystems.Shooter; +// import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.LEDPatterns; -import frc4388.utility.controller.DeadbandedXboxController; -import frc4388.utility.controller.IHandController; -import frc4388.utility.controller.XboxController; + +// Utilites +import frc4388.utility.DeferredBlock; +import frc4388.utility.configurable.ConfigurableString; /** * This class is where the bulk of the robot should be declared. Since @@ -31,32 +50,120 @@ import frc4388.utility.controller.XboxController; */ public class RobotContainer { /* RobotMap */ - private final RobotMap m_robotMap = new RobotMap(); - + public final RobotMap m_robotMap = new RobotMap(); + /* Subsystems */ - private final LED m_robotLED = new LED(m_robotMap.LEDController); + // 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); - /* 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(OIConstants.XBOX_PROGRAMMER_ID); + + 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); + + /* 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); + + // ! Teleop Commands + + private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.PIDIn()), + new InstantCommand(() -> m_robotShooter.idle()) + ); + + private SequentialCommandGroup intakeNotePullInIdle = new SequentialCommandGroup( + intakeToShootStuff, intakeToShoot, + new InstantCommand(() -> m_robotShooter.idle()) + ); + + 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) + ); + + // ! /* Autos */ + private String lastAutoName = "four_note_taxi_kracken.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 neoJoystickPlayback ampShoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto", + new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + false, true); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - configureButtonBindings(); + configureButtonBindings(); + configureVirtualButtonBindings(); + new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip()); + DriverStation.silenceJoystickConnectionWarning(true); + // CameraServer.startAutomaticCapture(); /* Default Commands */ + // ! Swerve Drive Default Command (Regular Rotation) // drives the robot with a two-axis input from the driver controller + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(), + true); + }, m_robotSwerveDrive) + .withName("SwerveDrive DefaultCommand")); + m_robotSwerveDrive.setToSlow(); + + // ! Swerve Drive One Module Test + // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + // m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft()); + // } + + // ! 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)); + + // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + // m_robotSwerveDrive.driveWithInput( + // getDeadbandedDriverController().getLeft(), + // getDeadbandedDriverController().getRight(), + // true); + // }, m_robotSwerveDrive)); + + + + } /** @@ -66,26 +173,123 @@ 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 - // 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()); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) - // .onFalse(new InstantCommand()); + // ? /* Driver Buttons */ + + DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip())); + + // ! /* Speed */ + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + + 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())); + + // ? /* Operator Buttons */ + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.PIDIn())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + + 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(intakeNotePullInIdle) + .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(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))); + + new Trigger(() -> getDeadbandedOperatorController().getPOV() != -1) + .onTrue(ampShoot) + .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter, m_robotSwerveDrive)); + + // ? /* Programer Buttons (Controller 3)*/ + + // * /* 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()); + } + + /** + * 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. + */ + + // ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto. - /* 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,13 +298,22 @@ 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() {}; } /** - * Add your docs here. + * 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))); + } + public DeadbandedXboxController getDeadbandedDriverController() { return this.m_driverXbox; } @@ -108,4 +321,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..4d1869f 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,16 +7,16 @@ 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.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import frc4388.robot.Constants.LEDConstants; +// 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,9 +25,9 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { - private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); + private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON_ID); public RobotGyro gyro = new RobotGyro(m_pigeon2); - + public SwerveModule leftFront; public SwerveModule rightFront; public SwerveModule leftBack; @@ -39,58 +39,48 @@ 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); /* 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 TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); + public final TalonFX leftFrontSteer = new 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 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 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); + + public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); + public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); + public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); + + public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); + public final TalonFX rightBackSteer = new 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() { + } + void configureDriveMotorControllers() { - // config factory default - leftFrontWheel.configFactoryDefault(); - leftFrontSteer.configFactoryDefault(); - - rightFrontWheel.configFactoryDefault(); - rightFrontSteer.configFactoryDefault(); - - leftBackWheel.configFactoryDefault(); - leftBackSteer.configFactoryDefault(); - - rightBackWheel.configFactoryDefault(); - rightBackSteer.configFactoryDefault(); - - // 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, -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); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); + this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_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/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java new file mode 100644 index 0000000..5fb161a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Shooter; + +public class ArmIntakeIn extends Command { + /** Creates a new ArmIntakeIn. */ + private Intake robotIntake; + private Shooter robotShooter; + + public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) { + // Use addRequirements() here to declare subsystem dependencies. + this.robotIntake = robotIntake; + this.robotShooter = robotShooter; + + addRequirements(this.robotIntake, this.robotShooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + robotIntake.PIDOut(); + robotIntake.spinIntakeMotor(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return robotIntake.getIntakeLimitSwitchState(); + // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) + // { + // return !true==true; + // } + // else + // { + // return !false==!(!(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..ae054ed 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -9,11 +9,11 @@ import java.io.FileNotFoundException; import java.util.ArrayList; import java.util.Scanner; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.UtilityStructs.TimedOutput; -public class JoystickPlayback extends CommandBase { +public class JoystickPlayback extends Command { private final SwerveDrive swerve; private String filename; private int mult = 1; @@ -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..0224fcf 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -11,11 +11,11 @@ import java.util.ArrayList; import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.UtilityStructs.TimedOutput; -public class JoystickRecorder extends CommandBase { +public class JoystickRecorder extends Command { public final SwerveDrive swerve; public final Supplier leftX; @@ -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/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java new file mode 100644 index 0000000..5ed1472 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -0,0 +1,57 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ClimbConstants; + +public class Climber extends SubsystemBase { + /** Creates a new Climber. */ + TalonFX climbMotor; + + public Climber(TalonFX climbMotor) { + this.climbMotor = climbMotor; + this.climbMotor.setInverted(true); + + climbMotor.setNeutralMode(NeutralModeValue.Brake); + + var slot0Configs = new Slot0Configs(); + slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output + slot0Configs.kI = 0.0; // no output for integrated error + slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output + + climbMotor.getConfigurator().apply(slot0Configs); + } + + public void climbOut() { + //PositionVoltage request = new PositionVoltage(0); + //climbMotor.setControl(request.withPosition(-520)); + + climbMotor.set(ClimbConstants.CLIMB_OUT_SPEED); + } + + public void climbIn() { + //PositionVoltage request = new PositionVoltage(-520); + //climbMotor.setControl(request.withPosition(0)); + climbMotor.set(ClimbConstants.CLIMB_IN_SPEED); + } + + public void stopClimb() { + climbMotor.set(0.d); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + //SmartDashboard.putNumber("climber pos", climbMotor.getPosition().getValue()); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java index 9ec39df..91de2e9 100644 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/DiffDrive.java @@ -7,7 +7,8 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -26,23 +27,25 @@ public class DiffDrive extends SubsystemBase { private RobotTime m_robotTime = RobotTime.getInstance(); - private WPI_TalonFX m_leftFrontMotor; - private WPI_TalonFX m_rightFrontMotor; - private WPI_TalonFX m_leftBackMotor; - private WPI_TalonFX m_rightBackMotor; + private TalonFX m_leftFrontMotor; + private TalonFX m_rightFrontMotor; + private TalonFX m_leftBackMotor; + private TalonFX m_rightBackMotor; private DifferentialDrive m_driveTrain; private RobotGyro m_gyro; /** * Add your docs here. */ - public DiffDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor, - WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { + public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor, + TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { m_leftFrontMotor = leftFrontMotor; m_rightFrontMotor = rightFrontMotor; m_leftBackMotor = leftBackMotor; m_rightBackMotor = rightBackMotor; + m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false)); + m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false)); m_driveTrain = driveTrain; m_gyro = gyro; } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java new file mode 100644 index 0000000..5ffa72a --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -0,0 +1,110 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.IntakeConstants; +import frc4388.utility.Gains; + +public class Intake extends SubsystemBase { + private TalonFX intakeMotor; + private TalonFX pivotMotor; + public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS; + + /** Creates a new Intake. */ + public Intake(TalonFX intakeMotor, TalonFX pivotMotor) { + this.intakeMotor = intakeMotor; + this.pivotMotor = pivotMotor; + + intakeMotor.getConfigurator().apply(new TalonFXConfiguration()); + pivotMotor.getConfigurator().apply(new TalonFXConfiguration()); + + intakeMotor.setNeutralMode(NeutralModeValue.Brake); + pivotMotor.setNeutralMode(NeutralModeValue.Brake); + + // in init function, set slot 0 gains + var slot0Configs = new Slot0Configs(); + slot0Configs.kP = 1.3; // An error of 0.5 rotations results in 12 V output + slot0Configs.kI = 0.0; // no output for integrated error + slot0Configs.kD = 0.21; // A velocity of 1 rps results in 0.1 V output + + pivotMotor.getConfigurator().apply(slot0Configs); + } + + // ! Talon Methods + public void PIDIn() { + PIDPosition(0); + } + + public void PIDOut() { + PIDPosition(-53); + } + + public void PIDPosition(double pos) { + PositionVoltage request = new PositionVoltage(pos); + pivotMotor.setControl(request); + } + + public void handoff() { + intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + } + + public void spinIntakeMotor() { + intakeMotor.set(IntakeConstants.INTAKE_SPEED); + } + + public void spinIntakeMotor(double speed) { + intakeMotor.set(speed); + } + + public boolean getIntakeLimitSwitchState() { + return intakeMotor.getForwardLimit().getValue().value == 0; + } + + public void setPivotEncoderPosition(double val) { + pivotMotor.setPosition(val); + } + + public void stopIntakeMotors() { + intakeMotor.set(0); + } + + public void stopArmMotor() { + pivotMotor.set(0); + } + + public void stop() { + intakeMotor.set(0); + pivotMotor.set(0); + } + + public double getArmPos() { + return pivotMotor.getPosition().getValue(); + } + + public void resetArmPosition() { + if (getIntakeLimitSwitchState()) { + // talonPivot.setPosition(0); + } + } + + public void ampPosition() { + PIDPosition(-59); //TODO: Find actual value + } + + public void ampOuttake(double speed) { + spinIntakeMotor(speed); + } + + @Override + public void periodic() { + resetArmPosition(); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 0d4af80..e9e070c 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -7,6 +7,8 @@ package frc4388.robot.subsystems; +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,44 +22,69 @@ import frc4388.utility.LEDPatterns; */ public class LED extends SubsystemBase { - private LEDPatterns m_currentPattern; - private Spark m_LEDController; - + static AddressableLED m_led; + static AddressableLEDBuffer m_ledBuffer; + static LED m_self; /** * Add your docs here. */ - public LED(Spark LEDController){ - m_LEDController = LEDController; - setPattern(LEDConstants.DEFAULT_PATTERN); - updateLED(); + + public LED(){ + if(m_self != null) + return; + m_led = new AddressableLED(9); + m_ledBuffer = new AddressableLEDBuffer(10); + m_led.setLength(m_ledBuffer.getLength()); + m_led.setData(m_ledBuffer); + m_led.start(); System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); } - + public static LED getInstance() { + if(m_self == null) + m_self = new LED(); + return m_self; + } @Override public void periodic(){ - SmartDashboard.putNumber("LED", m_currentPattern.getValue()); + //gamermode(); + //SmartDashboard.putNumber("LED", m_currentPattern.getValue()); + return; + } + static int firstcolor = 0; + static void gamermode() { + for(int i = 0; i < m_ledBuffer.getLength(); i++) { + final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180; + setLEDHSV(i, hue, 255, 128); + } + firstcolor +=3; + firstcolor %= 180; + } + /** + * Add your docs here. + */ + public static void updateLED (){ + gamermode(); + // m_LEDController.set(m_currentPattern.getValue()); } /** * Add your docs here. */ - public void updateLED(){ - m_LEDController.set(m_currentPattern.getValue()); + public static void setLEDRGB(int lednum, int r, int g, int b){ + m_ledBuffer.setRGB(lednum, r, g, b); + //m_currentPattern = pattern; + // m_LEDController.set(m_currentPattern.getValue()); } - - /** - * Add your docs here. - */ - public void setPattern(LEDPatterns pattern){ - m_currentPattern = pattern; - m_LEDController.set(m_currentPattern.getValue()); + public static void setLEDHSV(int lednum, int hue, int sat, int val){ + m_ledBuffer.setRGB(lednum, hue, sat, val); + //m_currentPattern = pattern; + // m_LEDController.set(m_currentPattern.getValue()); } - /** * Add your docs here. * @return */ - public LEDPatterns getPattern() { - return m_currentPattern; + public AddressableLEDBuffer getBuffer() { + return m_ledBuffer; } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 9d1289b..2200b07 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -3,163 +3,80 @@ // the WPILib BSD license file in the root directory of this project. package frc4388.robot.subsystems; - -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.common.hardware.VisionLEDMode; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; + import frc4388.robot.Constants.VisionConstants; +// Look at vvv for networktables stuff +// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data + public class Limelight extends SubsystemBase { - - private PhotonCamera cam; - private PhotonPoseEstimator photonPoseEstimator; - private boolean lightOn; + // // [X, Y, Z, Roll, Pitch, Yaw] + // private double[] cameraPose; + // private boolean isTag; - /** Creates a new Limelight. */ - public Limelight() { - cam = new PhotonCamera(VisionConstants.NAME); - cam.setDriverMode(false); - } + // private Pose2d pose; + // private boolean isNearSpeaker; - public void setLEDs(boolean on) { - lightOn = on; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } + // public boolean getIsTag() { + // return isTag; + // } - public void toggleLEDs() { - lightOn = !lightOn; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } + // private void update() { + // SmartDashboard.putBoolean("Apriltag", isTag); + // if(!isTag){ + // return; + // } - public void setDriverMode(boolean driverMode) { - cam.setDriverMode(driverMode); - } + // double x = cameraPose[0]; + // double y = cameraPose[1]; + // double yaw = cameraPose[5]; - public void setToLimePipeline() { - cam.setPipelineIndex(1); - setLEDs(true); - } + // Rotation2d rot = Rotation2d.fromDegrees(yaw); - public void setToAprilPipeline() { - cam.setPipelineIndex(0); - setLEDs(false); - } + // pose = new Pose2d(x, y, rot); - public PhotonTrackedTarget getAprilPoint() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - return result.getBestTarget(); - } - - private List getAprilCorners() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - return result.getBestTarget().getDetectedCorners(); - } - - public double getAprilSkew() { - List corners = getAprilCorners(); - ArrayList bottomSide = getAprilBottomSide(corners); - - if (bottomSide == null) return 0; - - TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); - TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0); - - return bottomLeft.y - bottomRight.y; - } - - private ArrayList getAprilBottomSide(List box) { - if (box == null) return null; - - ArrayList bottomSide = new ArrayList<>(); - - TargetCorner l1 = new TargetCorner(-1, -1); - TargetCorner l2 = new TargetCorner(-1, -1); + // boolean isRed = DriverStation.getAlliance().get() == Alliance.Red; - for (TargetCorner c : box) { - if (c.y > l1.y) l1 = c; - } + // double distance; - for (TargetCorner c : box) { - if (c.y == l1.y) continue; - if (c.y > l2.y) l2 = c; - } + // if(isRed){ + // distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter); + // }else{ + // distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter); + // } + + // isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; - bottomSide.add(l1); - bottomSide.add(l2); + // //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); + // //SmartDashboard.putNumber("speakerDistance", distance); + // } - return bottomSide; - } + // public Pose2d getPose() { + // return pose; + // } - public double getDistanceToApril() { - PhotonTrackedTarget aprilPoint = getAprilPoint(); - if (aprilPoint == null) return -1; + // public boolean isNearSpeaker() { + // return isNearSpeaker; + // } - double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; - double theta = 35.0 + aprilPoint.getPitch(); + // @Override + // public void periodic() { + // // This method will be called once per scheduler run - double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); - return distanceToApril; - } + // //isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0; + // //double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); - public PhotonTrackedTarget getLowestTape() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - ArrayList points = (ArrayList) result.getTargets(); - - PhotonTrackedTarget lowest = points.get(0); - for (PhotonTrackedTarget point : points) { - if (point.getPitch() < lowest.getPitch()) { - lowest = point; - } - } - - return lowest; - } - - public double getDistanceToTape() { - PhotonTrackedTarget tapePoint = getLowestTape(); - if (tapePoint == null) return -1; - - double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; - double theta = 35.0 + tapePoint.getPitch(); - - double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); - return distanceToTape; - } - - @Override - public void periodic() {} -} + // //if(newPose != cameraPose){ + // // cameraPose = newPose; + // //update(); + // //} + // } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java new file mode 100644 index 0000000..fb80e19 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -0,0 +1,114 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.IntakeConstants; +import frc4388.robot.Constants.ShooterConstants; + +import frc4388.robot.subsystems.Limelight; + +// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +// import com.ctre.phoenix.motorcontrol.NeutralMode; + + +public class Shooter extends SubsystemBase { + + private TalonFX leftShooter; + private TalonFX rightShooter; + + private Limelight limelight; + + private int spinMode = 0; + // 0 = Stop / Coast + // 1 = Idle + // 2 = Idle Near Speaker + // 3 = Spin + // 4 = SingleSpin + + private double smartDashboardShooterSpeed; + + /** Creates a new Shooter. */ + public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) { + leftShooter = leftTalonFX; + rightShooter = rightTalonFX; + + limelight = tmplimelight; + + leftShooter.setNeutralMode(NeutralModeValue.Coast); + rightShooter.setNeutralMode(NeutralModeValue.Coast); + SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); + + } + + public Shooter(TalonFX leftShooter) { + this.leftShooter = leftShooter; + leftShooter.setNeutralMode(NeutralModeValue.Coast); + } + + public void singleSpin() { + leftShooter.set(1.0); + spinMode = 4; + } + + public void singleSpin(double speed) { + leftShooter.set(speed); + spinMode = 4; + } + + public void spin() { + spin(smartDashboardShooterSpeed); + spinMode = 3; + } + + public void spin(double speed) { + leftShooter.set(-speed); + rightShooter.set(-speed); + spinMode = 3; + } + + public void spin(double leftSpeed, double rightSpeed) { + leftShooter.set(leftSpeed); + rightShooter.set(-rightSpeed); + spinMode = 3; + } + + public void stop() { + spin(0.d); + spinMode = 0; + } + + public void idle() { + spin(ShooterConstants.SHOOTER_IDLE); + spinMode = 1; + } + + public int getMode(){ + return spinMode; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + // SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); + //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); + //smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); + + // If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed. + // Else if the robot is not near the speaker, then set the speed back to idle. + // if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){ + // leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + // rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + // spinMode = 2; + // }else if(!limelight.isNearSpeaker() && spinMode == 2){ + // idle(); + // } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 055230b..bd35742 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,17 +4,21 @@ 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.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.Constants.SwerveDriveConstants.Conversions; import frc4388.utility.RobotGyro; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.utility.RobotUnits; -public class SwerveDrive extends SubsystemBase { +public class SwerveDrive extends SubsystemBase { private SwerveModule leftFront; private SwerveModule rightFront; @@ -24,17 +28,23 @@ 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; + + private int gear_index; + private boolean stopped = false; - 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; + public double rotSpeedAdjust = SwerveDriveConstants.MAX_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. */ @@ -45,20 +55,37 @@ public class SwerveDrive extends SubsystemBase { this.rightBack = rightBack; this.gyro = gyro; - + reset_index(); this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } - boolean stopped = false; + public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ + // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); + // rightStick.getAngle() + double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2)); + // System.out.println(ang); + // module.go(ang); + // Rotation2d rot = Rotation2d.fromRadians(ang); + Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); + SwerveModuleState state = new SwerveModuleState(speed, rot); + module.setDesiredState(state); + } + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + + double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0; + SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction)); + 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); + rot_correction = 0; + // rot = rightStick.getX(); + // SmartDashboard.putBoolean("drift correction", false); stopped = false; } else if(leftStick.getNorm() > 0.05) { if (!stopped) { @@ -66,8 +93,9 @@ public class SwerveDrive extends SubsystemBase { stopped = true; } - SmartDashboard.putBoolean("drift correction", true); - rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + // SmartDashboard.putBoolean("drift correction", true); + + // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; } @@ -76,14 +104,82 @@ public class SwerveDrive extends SubsystemBase { // 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. + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, 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); + stopped = false; + } else if(leftStick.getNorm() > 0.05) { + if (!stopped) { + stopModules(); + stopped = true; + } + + // SmartDashboard.putBoolean("drift correction", true); + // double rot_correction = ((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() * 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(), -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)); + } + + public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + + // Translation2d rightStick = new Translation2d(-rightX, rightY); + double rightX = rightStick.getX(); + double rightY = rightStick.getY(); + + double rot_correction = 0; + + // double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + + 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) + rot_correction, 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. @@ -111,12 +207,33 @@ public class SwerveDrive extends SubsystemBase { } public double getGyroAngle() { - return gyro.getAngle(); + 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,24 +246,45 @@ 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("RotTartget", rotTarget); + } + + private void reset_index() { + gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?) } public void shiftDown() { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { - - } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; - } else { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; - } + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index + int i = gear_index - 1; + if (i == -1) i = 0; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } + + public void shiftUp() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index + int i = gear_index + 1; + if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } + + public void setPercentOutput(double speed) { + speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed; + gear_index = -1; } public void setToSlow() { - this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; + this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED; System.out.println("SLOW"); System.out.println("SLOW"); System.out.println("SLOW"); @@ -155,7 +293,7 @@ public class SwerveDrive extends SubsystemBase { } public void setToFast() { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED; System.out.println("FAST"); System.out.println("FAST"); System.out.println("FAST"); @@ -164,7 +302,7 @@ public class SwerveDrive extends SubsystemBase { } public void setToTurbo() { - this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; + this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED; System.out.println("TURBO"); System.out.println("TURBO"); System.out.println("TURBO"); @@ -172,16 +310,6 @@ public class SwerveDrive extends SubsystemBase { System.out.println("TURBO"); } - public void shiftUp() { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; - } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; - } else { - - } - } - public void toggleGear(double angle) { if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; @@ -192,4 +320,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..b9895fb 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -4,56 +4,126 @@ package frc4388.robot.subsystems; -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.phoenix6.StatusSignal; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +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; 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 final StatusSignal cc_pos; + // private final StatusSignal cc_vel; + // 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; - TalonFXConfiguration angleConfig = new TalonFXConfiguration(); - angleConfig.slot0.kP = swerveGains.kP; - angleConfig.slot0.kI = swerveGains.kI; - angleConfig.slot0.kD = swerveGains.kD; + var motorCfg = new TalonFXConfiguration() + .withOpenLoopRamps( + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) + ).withClosedLoopRamps( + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + ).withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(100) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(100) + .withSupplyCurrentLimitEnable(true) + ); - // 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); + driveMotor.getConfigurator().apply(motorCfg); - driveMotor.setSelectedSensorPosition(0); - driveMotor.config_kP(0, 0.2); + TalonFXConfiguration angleConfig = new TalonFXConfiguration() + .withOpenLoopRamps( + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) + ).withClosedLoopRamps( + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + ); + + angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + angleConfig.Slot0.kP = swerveGains.kP; + angleConfig.Slot0.kI = swerveGains.kI; + angleConfig.Slot0.kD = swerveGains.kD; + + angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID(); + angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + angleMotor.getConfigurator().apply(angleConfig); + + CANcoderConfiguration canconfig = new CANcoderConfiguration(); + canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + canconfig.MagnetSensor.MagnetOffset = offset; + encoder.getConfigurator().apply(canconfig); + + rotateToAngle(0); } + // public void go(double ang){ + // // double curang = this.encoder.getAbsolutePosition().getValue(); + // System.out.println(getAngle().getDegrees()); + // rotateToAngle(ang); + // } + + @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 +131,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 +139,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 +149,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 Rotation2d.fromRotations(encoder.getPosition().getValue()); } public double getAngularVel() { - return this.angleMotor.getSelectedSensorVelocity(); + // return this.angleMotor.getSelectedSensorVelocity(); + return angleMotor.getVelocity().getValueAsDouble(); } public double getDrivePos() { - return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; + // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; + return driveMotor.getPosition().getValueAsDouble(); } public double getDriveVel() { - return this.driveMotor.getSelectedSensorVelocity(0); + // return this.driveMotor.getSelectedSensorVelocity(0); + return driveMotor.getVelocity().getValueAsDouble(); } public void stop() { @@ -100,7 +174,8 @@ public class SwerveModule extends SubsystemBase { } public void rotateToAngle(double angle) { - angleMotor.set(TalonFXControlMode.Position, angle); + final PositionVoltage m_request = new PositionVoltage(angle); + angleMotor.setControl(m_request); } /** @@ -109,23 +184,30 @@ public class SwerveModule extends SubsystemBase { */ public SwerveModuleState getState() { return new SwerveModuleState( - Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, + Units.inchesToMeters(driveMotor.getVelocity().getValue() * + SwerveDriveConstants.Conversions.INCHES_PER_WHEEL_REV * + SwerveDriveConstants.Conversions.WHEEL_REV_PER_MOTOR_REV), getAngle() ); } + // private SwerveModuleState optimizeState(SwerveModuleState desiredState) { + // Rotation2d curRot = this.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(); @@ -134,28 +216,27 @@ public class SwerveModule extends SubsystemBase { // 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; + double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND; - // convert the CANCoder from its position reading to ticks - double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); + rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations()); - angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); - - double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - - driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + driveMotor.set(Math.max(Math.min(speed, 1.), -1.)); } - public void reset(double position) { - encoder.setPositionToAbsolute(); + public void reset() { + // encoder.setPosition(0); } - 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..966d2e0 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -7,20 +7,22 @@ package frc4388.utility; -import com.ctre.phoenix.sensors.WPI_Pigeon2; +// import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.ctre.phoenix6.hardware.Pigeon2; import com.kauailabs.navx.frc.AHRS; // import edu.wpi.first.wpilibj.GyroBase; -import edu.wpi.first.wpilibj.interfaces.Gyro; +// import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; /** * Gyro class that allows for interchangeable use between a pigeon and a navX */ -public class RobotGyro implements Gyro { +public class RobotGyro { private RobotTime m_robotTime = RobotTime.getInstance(); - private WPI_Pigeon2 m_pigeon = null; + private Pigeon2 m_pigeon = null; private AHRS m_navX = null; public boolean m_isGyroAPigeon; //true if pigeon, false if navX @@ -34,7 +36,7 @@ public class RobotGyro implements Gyro { * Creates a Gyro based on a pigeon * @param gyro the gyroscope to use for Gyro */ - public RobotGyro(WPI_Pigeon2 gyro) { + public RobotGyro(Pigeon2 gyro) { m_pigeon = gyro; m_isGyroAPigeon = true; } @@ -54,8 +56,8 @@ public class RobotGyro implements Gyro { public void resetZeroValues() { if (!m_isGyroAPigeon) return; - pitchZero = m_pigeon.getPitch(); - rollZero = m_pigeon.getRoll(); + // pitchZero = m_pigeon.getPitch(); + // rollZero = m_pigeon.getRoll(); } /** @@ -84,16 +86,15 @@ public class RobotGyro implements Gyro { * is typically done when the robot is first turned on while it's sitting at rest before the * competition starts. */ - @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 public void reset() { resetZeroValues(); @@ -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(); + } + } /** @@ -113,16 +181,19 @@ public class RobotGyro implements Gyro { * Roll is within [-90,+90] degrees. */ private double[] getPigeonAngles() { - double[] ypr = new double[3]; - m_pigeon.getYawPitchRoll(ypr); + m_pigeon.getAngle(); + var rotation = m_pigeon.getRotation3d(); - return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)}; + return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())}; + } + + public Rotation2d getRotation2d() { + return m_pigeon.getRotation2d(); } - @Override public double getAngle() { if (m_isGyroAPigeon) { - return getPigeonAngles()[0]; + return getPigeonAngles()[2]; } else { return m_navX.getAngle(); } @@ -176,7 +247,6 @@ public class RobotGyro implements Gyro { } } - @Override public double getRate() { if (m_isGyroAPigeon) { return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000; @@ -185,7 +255,7 @@ public class RobotGyro implements Gyro { } } - public WPI_Pigeon2 getPigeon(){ + public Pigeon2 getPigeon(){ return m_pigeon; } @@ -193,7 +263,6 @@ public class RobotGyro implements Gyro { return m_navX; } - @Override public void close() throws Exception { } 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/src/test/java/frc4388/mocks/MockPigeonIMU.java b/src/test/java/frc4388/mocks/MockPigeonIMU.java.old similarity index 100% rename from src/test/java/frc4388/mocks/MockPigeonIMU.java rename to src/test/java/frc4388/mocks/MockPigeonIMU.java.old 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/Phoenix.json b/vendordeps/Phoenix6.json similarity index 69% rename from vendordeps/Phoenix.json rename to vendordeps/Phoenix6.json index 614dc3a..0322385 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix6.json @@ -1,56 +1,32 @@ { - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.31.0+23.2.2", - "frcYear": 2023, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "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/phoenix/Phoenix5-frc2023-latest.json", + "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.phoenix", - "artifactId": "api-java", - "version": "5.31.0" - }, - { - "groupId": "com.ctre.phoenix", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "5.31.0" + "version": "24.3.0" } ], "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.31.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.31.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.2.2", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -63,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.2.2", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -76,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.2.2", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -89,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.2.2", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -102,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.2.2", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -115,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.2.2", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -128,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.2.2", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -141,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.2.2", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -154,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.2.2", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -167,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.2.2", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,40 +156,10 @@ ], "cppDependencies": [ { - "groupId": "com.ctre.phoenix", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "5.31.0", - "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.31.0", - "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.31.0", - "libName": "CTRE_PhoenixCCI", + "version": "24.3.0", + "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -227,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.2.2", + "version": "24.3.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,40 +186,10 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenix.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.31.0", - "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.31.0", - "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.31.0", - "libName": "CTRE_PhoenixCCISim", + "version": "24.3.0", + "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -287,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.2.2", + "version": "24.3.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -302,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.2.2", + "version": "24.3.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -317,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.2.2", + "version": "24.3.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -332,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.2.2", + "version": "24.3.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -347,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.2.2", + "version": "24.3.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -362,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.2.2", + "version": "24.3.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -377,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.2.2", + "version": "24.3.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -392,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.2.2", + "version": "24.3.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -407,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.2.2", + "version": "24.3.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, 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": [ diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index c940b75..0000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,42 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2024.1.1-beta-3.2", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2024", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "PhotonLib-cpp", - "version": "v2024.1.1-beta-3.2", - "libName": "Photon", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "PhotonLib-java", - "version": "v2024.1.1-beta-3.2" - }, - { - "groupId": "org.photonvision", - "artifactId": "PhotonTargeting-java", - "version": "v2024.1.1-beta-3.2" - } - ] -} \ No newline at end of file