diff --git a/.gitignore b/.gitignore index 9535c83..44bdfbb 100644 --- a/.gitignore +++ b/.gitignore @@ -158,5 +158,26 @@ gradle-app.setting .settings/ bin/ +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + # 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/ + +# VSCode config files +.vscode/* +.vscode/settings.json +.vscode/launch.json diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..ee83cb1 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,21 +1,36 @@ { - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false + }, + { + "name": "C/C++ Runner: Debug Session", + "type": "cppdbg", + "request": "launch", + "args": [], + "stopAtEntry": false, + "externalConsole": true, + "cwd": ".", + "program": "build/Debug/outDebug", + "MIMode": "gdb", + "miDebuggerPath": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] } ] -} +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 64b7017..f0ca392 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,22 +1,64 @@ { - "java.configuration.updateBuildConfiguration": "disabled", - "java.import.gradle.enabled": false, - "files.exclude": { - "**/.git": true, - "**/.svn": true, - "**/.hg": true, - "**/CVS": true, - "**/.DS_Store": true, - "bin/": true, - "**/.classpath": true, - "**/.project": true, - "**/.settings": true, - "**/.factorypath": true, - "**/*~": true - }, - "C_Cpp.default.configurationProvider": "vscode-wpilib", + "C_Cpp_Runner.msvcBatchPath": "", + "C_Cpp_Runner.cCompilerPath": "gcc", + "C_Cpp_Runner.cppCompilerPath": "g++", + "C_Cpp_Runner.debuggerPath": "gdb", + "C_Cpp_Runner.cStandard": "", + "C_Cpp_Runner.cppStandard": "", + "C_Cpp_Runner.useMsvc": false, + "C_Cpp_Runner.warnings": [ + "-Wall", + "-Wextra", + "-Wpedantic", + "-Wshadow", + "-Wformat=2", + "-Wcast-align", + "-Wconversion", + "-Wsign-conversion", + "-Wnull-dereference" + ], + "C_Cpp_Runner.msvcWarnings": [ + "/W4", + "/permissive-", + "/w14242", + "/w14287", + "/w14296", + "/w14311", + "/w14826", + "/w44062", + "/w44242", + "/w14905", + "/w14906", + "/w14263", + "/w44265", + "/w14928" + ], + "C_Cpp_Runner.enableWarnings": true, + "C_Cpp_Runner.warningsAsError": false, + "C_Cpp_Runner.compilerArgs": [], + "C_Cpp_Runner.linkerArgs": [], + "C_Cpp_Runner.includePaths": [], + "C_Cpp_Runner.includeSearch": [ + "*", + "**/*" + ], + "C_Cpp_Runner.excludeSearch": [ + "**/build", + "**/build/**", + "**/.*", + "**/.*/**", + "**/.vscode", + "**/.vscode/**" + ], + "C_Cpp_Runner.useAddressSanitizer": false, + "C_Cpp_Runner.useUndefinedSanitizer": false, + "C_Cpp_Runner.useLeakSanitizer": false, + "C_Cpp_Runner.showCompilationTime": false, + "C_Cpp_Runner.useLinkTimeOptimization": false, + "C_Cpp_Runner.msvcSecureNoWarnings": false, "files.associations": { - "*.inc": "cpp" + "*.inc": "cpp", + "memory": "cpp" }, - "wpilib.skipTests": true -} + "C_Cpp.errorSquiggles": "disabled" +} \ No newline at end of file diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 227c830..719fe35 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": true, "currentLanguage": "cpp", - "projectYear": "2023", - "teamNumber": 7443 + "projectYear": "2024", + "teamNumber": 2559 } \ No newline at end of file diff --git a/Docs/DriveTrainCodePaths.txt b/Docs/DriveTrainCodePaths.txt new file mode 100644 index 0000000..1ce9464 --- /dev/null +++ b/Docs/DriveTrainCodePaths.txt @@ -0,0 +1,35 @@ +Init Path (Runs Once) + RobotContainer::TeleopInit + sets default run command to RobotContainer::DriveCommandFactory + + SwerveModule::SwerveModule + PID inits + +Periodic Paths (Runs at 50Hz) + SwerveModule::Periodic + calculates the next PID step + applies calculated step to turning motor + + RobotContainer::GetDriveTeleopControls + gets controller input and applies deadzone + returns the modified controller input + RobotContainer::DriveCommandFactory + multiplies controller output by max drive/turn speed in constants file + sends the multiplied controller input to drive + DriveSubsystem::Drive + divides controller output by max drive/turn speed in constants file + disables field relative if no gyro + convert controller output to swerve module states (hold speed and angle) + DriveSubsystem::SetModuleStates + stops drive and turn motors if all desired speeds are zero + sends desired speed and angle to individual modules + SwerveModule::SetDesiredState + calls AngleSensor::GetAbsolutePosition + returns current angle bounded [-180, 180) + performs turning optimization + calls SwerveModule::SetTurningPosition + bounds angle between [-180, 180) + sets m_rioPIDController set point to bounded angle + calls SwerveModule::SetDriveVelocity + does some questionable math to set a voltage + I did not see a PID in this code path diff --git a/Docs/NotesOnPID.txt b/Docs/NotesOnPID.txt new file mode 100644 index 0000000..dd29368 --- /dev/null +++ b/Docs/NotesOnPID.txt @@ -0,0 +1,9 @@ +P ~= 1/360 + +update frequency likely too high for anything other than P + +things to try + output could be capped + increased low error sensitivity + nonlinear output scaling + increased low/high error sensitivity, decreased opposite diff --git a/Docs/SubsystemNotes.txt b/Docs/SubsystemNotes.txt new file mode 100644 index 0000000..0f1714d --- /dev/null +++ b/Docs/SubsystemNotes.txt @@ -0,0 +1,8 @@ +Shooter subsystems + 2 motors + both shoot and one inverted + +Intake subsystems + 2 motors + one moves arm position and needs control loop + one moves intake diff --git a/README.md b/README.md deleted file mode 100644 index 57230d1..0000000 --- a/README.md +++ /dev/null @@ -1,26 +0,0 @@ -# Swerve (for Scrappy the Robot) -C++ code for swerve drive: 2x SPARK MAX/NEO plus SRX MAG ENCODER per swerve module (i.e SDS Swerve Module); with Test Mode code. - -See [SwerveSensorInterfaceBoard](https://github.com/Jagwires7443/SwerveSensorInterfaceBoard) for details on electrical connections. - -Here are some of the features implemented in this code: - -* Handling PWM input from SRX MAG ENCODER to derive absolute position; -* Handling SPARK MAX motor controllers; -* Profiled PID control of turning position; -* Using turning position to override commanded drive, when modules are not facing in commanded direction; -* Distance and velocity Profiled PID control of drive motors; -* Test Mode includes interactive adjustment of PID settings; -* Error handling for motor controllers, up to being able to test code on a roboRIO with no motor controllers; -* Test Mode uses Shuffleboard to create a tab for each swerve module, and a tab for the overall swerve drive; -* Step-by-step bring up procedure for swerve modules and drive system is documented in block comments (in [SwerveModule.h](https://github.com/Jagwires7443/Swerve/blob/master/src/main/include/subsystems/SwerveModule.h)); -* Provides several Test Mode routines to automatically have robot drive various fixed test patterns; -* Logic to manage and save configuration of motor controller settings; -* Primitives useful for autonomous driving; -* Integrated with WPILib. - -Please see (and/or use) [shuffleboard.json](https://github.com/Jagwires7443/Swerve/blob/master/shuffleboard.json) for suggested Shuffleboard settings. - -![alt text](https://github.com/Jagwires7443/Swerve/blob/master/TestMode1.PNG?raw=true) - -![alt text](https://github.com/Jagwires7443/Swerve/blob/master/TestMode2.PNG?raw=true) diff --git a/TestMode1.PNG b/TestMode1.PNG deleted file mode 100644 index f94dc2b..0000000 Binary files a/TestMode1.PNG and /dev/null differ diff --git a/TestMode2.PNG b/TestMode2.PNG deleted file mode 100644 index 8616036..0000000 Binary files a/TestMode2.PNG and /dev/null differ 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/build.gradle b/build.gradle index 3e9b217..749489b 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.4.3" + id "edu.wpi.first.GradleRIO" version "2024.2.1" } // Define my targets (RoboRIO) and artifacts (deployable files) 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 old mode 100644 new mode 100755 index a69d9cb..1aa94a4 --- 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/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/shuffleboard.json b/shuffleboard.json deleted file mode 100644 index f1a7b61..0000000 --- a/shuffleboard.json +++ /dev/null @@ -1,36 +0,0 @@ -{ - "tabPane": [ - { - "title": "SmartDashboard", - "autoPopulate": true, - "autoPopulatePrefix": "SmartDashboard/", - "widgetPane": { - "gridSize": 48.0, - "showGrid": true, - "hgap": 0.0, - "vgap": 0.0, - "titleType": 1, - "tiles": {} - } - }, - { - "title": "LiveWindow", - "autoPopulate": true, - "autoPopulatePrefix": "LiveWindow/", - "widgetPane": { - "gridSize": 48.0, - "showGrid": true, - "hgap": 0.0, - "vgap": 0.0, - "titleType": 1, - "tiles": {} - } - } - ], - "windowGeometry": { - "x": -8.0, - "y": -8.0, - "width": 1382.0, - "height": 784.0 - } -} diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index cbc28f6..0b25e97 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -46,7 +46,10 @@ void Robot::DisabledInit() noexcept m_container.DisabledInit(); } -void Robot::DisabledPeriodic() noexcept {} +void Robot::DisabledPeriodic() noexcept +{ + m_container.DisabledPeriodic(); +} void Robot::DisabledExit() noexcept { @@ -55,7 +58,7 @@ void Robot::DisabledExit() noexcept /** * This autonomous runs the autonomous command selected by your {@link - * RobotContainer} class. + * RobotContainer} class.n */ void Robot::AutonomousInit() noexcept { @@ -65,15 +68,21 @@ void Robot::AutonomousInit() noexcept if (m_autonomousCommand) { - m_autonomousCommand->Schedule(); + //m_autonomousCommand->Schedule();----------------------------------------------------------UNCOMMENT TO REENABLE AUTONOMOUS } m_container.AutonomousInit(); } -void Robot::AutonomousPeriodic() noexcept {} +void Robot::AutonomousPeriodic() noexcept +{ + m_container.AutonomousPeriodic(); +} -void Robot::AutonomousExit() noexcept {} +void Robot::AutonomousExit() noexcept +{ + m_container.AutonomousExit(); +} void Robot::TeleopInit() noexcept { @@ -95,9 +104,15 @@ void Robot::TeleopInit() noexcept /** * This function is called periodically during operator control. */ -void Robot::TeleopPeriodic() noexcept {} +void Robot::TeleopPeriodic() noexcept +{ + m_container.TeleopPeriodic(); +} -void Robot::TeleopExit() noexcept {} +void Robot::TeleopExit() noexcept +{ + m_container.TeleopExit(); +} void Robot::TestInit() noexcept { diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 92d4617..be41d3b 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -1,7 +1,6 @@ // 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. - +// the WPILib BSD license file in the root directory of this pr #include "Constants.h" #include "RobotContainer.h" @@ -17,6 +16,15 @@ #include #include #include +#include +#include "commands/IntakeCommand.h" +#include "commands/ClimberLowerCommand.h" +#include "commands/ClimberRaiseCommand.h" +#include "commands/ClimberStopCommand.h" +#include "commands/ShootCommands.h" +#include "commands/PositionTransferArmCommand.h" +#include "commands/PIDTransferArmCommand.h" +#include "commands/IntakeEjectCommand.h" #include #include @@ -24,6 +32,8 @@ #include #include #include +#include +#include RobotContainer::RobotContainer() noexcept { @@ -35,6 +45,67 @@ RobotContainer::RobotContainer() noexcept ConfigureBindings(); } +#pragma region Autonomous +void RobotContainer::AutonomousInit() noexcept +{ + m_driveSubsystem.ClearFaults(); + + m_driveSubsystem.ResetEncoders(); + + m_driveSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void {}, + {&m_driveSubsystem})); + m_intakeSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void {}, + {&m_intakeSubsystem})); + m_transferArmSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void {}, + {&m_transferArmSubsystem})); + m_shooterSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void {}, + {&m_shooterSubsystem})); + m_infrastructureSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void {}, + {&m_infrastructureSubsystem})); +} + +void RobotContainer::AutonomousPeriodic() noexcept {} + +void RobotContainer::AutonomousExit() noexcept {} + +std::optional RobotContainer::GetAutonomousCommand() noexcept +{ + frc::TrajectoryConfig trajectoryConfig{4.0_mps, 2.0_mps_sq}; + frc::SwerveDriveKinematics<4> kinematics{m_driveSubsystem.kDriveKinematics}; + + trajectoryConfig.SetKinematics(kinematics); + + // TODO: Update trajectory path to our autonomous path + frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + {frc::Pose2d{}, + frc::Pose2d{1.0_m, 0.0_m, frc::Rotation2d{}}}, + trajectoryConfig); + + return TrajectoryAuto::TrajectoryAutoCommandFactory(&m_driveSubsystem, "Test Trajectory", trajectory); +} +#pragma endregion + +#pragma region Teleop +void RobotContainer::TeleopInit() noexcept +{ + m_driveSubsystem.ClearFaults(); + + m_driveSubsystem.ResetEncoders(); + m_driveSubsystem.ZeroHeading(); + + m_driveSubsystem.SetDefaultCommand(DriveCommandFactory(this)); + m_intakeSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void {}, + {&m_intakeSubsystem})); + m_shooterSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void {}, + {&m_shooterSubsystem})); + m_infrastructureSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void {}, + {&m_infrastructureSubsystem})); +} + +void RobotContainer::TeleopPeriodic() noexcept {} + +void RobotContainer::TeleopExit() noexcept {} + frc2::CommandPtr RobotContainer::DriveCommandFactory(RobotContainer *container) noexcept { // Set up default drive command; non-owning pointer is passed by value. @@ -62,339 +133,221 @@ frc2::CommandPtr RobotContainer::DriveCommandFactory(RobotContainer *container) driveRequirements)}; } -frc2::CommandPtr RobotContainer::PointCommandFactory(RobotContainer *container) noexcept +std::tuple RobotContainer::GetDriveTeleopControls() noexcept { - // Set up default drive command; non-owning pointer is passed by value. - auto driveRequirements = {dynamic_cast(&container->m_driveSubsystem)}; + /* + The robot's frame of reference is the standard unit circle, from + trigonometry. However, the front of the robot is facing along the positve + X axis. This means the poitive Y axis extends outward from the left (or + port) side of the robot. Poitive rotation is counter-clockwise. On the + other hand, as the controller is held, the Y axis is aligned with forward. + And, specifically, it is the negative Y axis which extends forward. So, + the robot's X is the controllers inverted Y. On the controller, the X + axis lines up with the robot's Y axis. And, the controller's positive X + extends to the right. So, the robot's Y is the controller's inverted X. + Finally, the other controller joystick is used for commanding rotation and + things work out so that this is also an inverted X axis. + */ + double LeftStickX = -m_xboxDrive.GetLeftY(); + double LeftStickY = -m_xboxDrive.GetLeftX(); + double rightStickRot = -m_xboxDrive.GetRightX(); + + if (triggerSpeedEnabled) // scale speed by analog trigger + { + double RightTrigAnalogVal = m_xboxDrive.GetRightTriggerAxis(); + RightTrigAnalogVal = ConditionRawTriggerInput(RightTrigAnalogVal); - // Point swerve modules, but do not actually drive. - return frc2::CommandPtr{std::make_unique( - [container]() -> void + if (LeftStickX != 0 || LeftStickY != 0) + { + if (LeftStickX != 0) { - const auto controls = container->GetDriveTeleopControls(); - - units::angle::radian_t angle{std::atan2(std::get<0>(controls), std::get<1>(controls))}; - - // Ingnore return (bool); no need to check that commanded angle has - // been reached. - (void)container->m_driveSubsystem.SetTurningPosition(angle); - }, - driveRequirements)}; -} - -void RobotContainer::AutonomousInit() noexcept -{ - m_driveSubsystem.ClearFaults(); - m_feederSubsystem.ClearFaults(); - m_shooterSubsystem.ClearFaults(); - - m_driveSubsystem.ResetEncoders(); + double LeftStickTheta = atan(LeftStickY / LeftStickX); + LeftStickX = RightTrigAnalogVal * cos(LeftStickTheta); + LeftStickY = RightTrigAnalogVal * sin(LeftStickTheta); + } + else + { + LeftStickY = std::copysign(RightTrigAnalogVal, LeftStickY); + } + } + } + else // scale speed by analog stick + { + LeftStickX = ConditionRawJoystickInput(LeftStickX); + LeftStickY = ConditionRawJoystickInput(LeftStickY); + } - m_driveSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void {}, - {&m_driveSubsystem})); - m_feederSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void {}, - {&m_feederSubsystem})); - m_shooterSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void {}, - {&m_shooterSubsystem})); - m_infrastructureSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void {}, - {&m_infrastructureSubsystem})); -} + rightStickRot = ConditionRawJoystickInput(rightStickRot, 0.0); -void RobotContainer::TeleopInit() noexcept -{ - m_driveSubsystem.ClearFaults(); - m_feederSubsystem.ClearFaults(); - m_shooterSubsystem.ClearFaults(); + // TODO: decide if this is still needed + LeftStickX *= 2.0; + LeftStickY *= 2.0; + rightStickRot *= 1.6; - m_driveSubsystem.ResetEncoders(); + frc::SmartDashboard::PutNumber("X", LeftStickX); + frc::SmartDashboard::PutNumber("Y", LeftStickY); + frc::SmartDashboard::PutNumber("Z", rightStickRot); - m_driveSubsystem.SetDefaultCommand(DriveCommandFactory(this)); - m_feederSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void - { m_feederSubsystem.Default(m_xbox.GetRightTriggerAxis()); }, - {&m_feederSubsystem})); - m_shooterSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void - { m_shooterSubsystem.Default(m_xbox.GetLeftTriggerAxis(), m_shooterVelocity); }, - {&m_shooterSubsystem})); - m_infrastructureSubsystem.SetDefaultCommand(frc2::RunCommand([&]() -> void - { m_infrastructureSubsystem.SetLEDPattern(m_LEDPattern); }, - {&m_infrastructureSubsystem})); + return std::make_tuple(LeftStickX, LeftStickY, rightStickRot, m_fieldOriented); } -void RobotContainer::ConfigureBindings() noexcept +double RobotContainer::ConditionRawTriggerInput(double RawTrigVal) noexcept { - m_xbox.A().OnTrue(frc2::InstantCommand([&]() -> void - { m_slow = true; }, - {}) - .ToPtr()); - m_xbox.B().OnTrue(frc2::InstantCommand([&]() -> void - { m_slow = false; }, - {}) - .ToPtr()); - - m_xbox.X().OnTrue(frc2::InstantCommand([&]() -> void - { m_fieldOriented = false; }, - {}) - .ToPtr()); - m_xbox.Y().OnTrue(frc2::InstantCommand([&]() -> void - { m_driveSubsystem.ZeroHeading(); - m_fieldOriented = true; }, - {&m_driveSubsystem}) - .ToPtr()); - - m_xbox.LeftBumper().WhileTrue(frc2::InstantCommand([&]() -> void - { m_feederSubsystem.Fire(); }, - {&m_feederSubsystem}) - .ToPtr()); - m_xbox.LeftBumper().OnFalse(frc2::InstantCommand([&]() -> void - { m_feederSubsystem.NoFeed(); }, - {&m_feederSubsystem}) - .ToPtr()); - - m_xbox.RightBumper().WhileTrue(frc2::InstantCommand([&]() -> void - { m_feederSubsystem.Eject(); }, - {&m_feederSubsystem}) - .ToPtr()); - m_xbox.RightBumper().OnFalse(frc2::InstantCommand([&]() -> void - { m_feederSubsystem.NoFeed(); }, - {&m_feederSubsystem}) - .ToPtr()); - - m_xbox.Start().WhileTrue(frc2::InstantCommand([&]() -> void - { m_feederSubsystem.Raise(); }, - {&m_feederSubsystem}) - .ToPtr()); - - m_xbox.Back().WhileTrue(frc2::InstantCommand([&]() -> void - { m_feederSubsystem.Lower(); }, - {&m_feederSubsystem}) - .ToPtr()); - - frc2::POVButton(&m_xbox, 90).WhileTrue(frc2::InstantCommand([&]() -> void - { m_feederSubsystem.LockIntake(); }, - {&m_feederSubsystem}) - .ToPtr()); - - frc2::POVButton(&m_xbox, 270).WhileTrue(frc2::InstantCommand([&]() -> void - { m_feederSubsystem.DropIntake(); }, - {&m_feederSubsystem}) - .ToPtr()); - - frc2::POVButton(&m_xbox, 0).WhileTrue(frc2::InstantCommand([&]() -> void - { m_feederSubsystem.RaiseIntake(); }, - {&m_feederSubsystem}) - .ToPtr()); - - frc2::POVButton(&m_xbox, 180).WhileTrue(frc2::InstantCommand([&]() -> void - { m_feederSubsystem.LowerIntake(); }, - {&m_feederSubsystem}) - .ToPtr()); - - frc2::JoystickButton(&m_buttonBoard, 5).OnTrue(frc2::InstantCommand([&]() -> void - { m_shooterVelocity = -500.0; }, - {}) - .ToPtr()); - - frc2::JoystickButton(&m_buttonBoard, 6).OnTrue(frc2::InstantCommand([&]() -> void - { m_lock = true; }, - {}) - .ToPtr()); - - frc2::JoystickButton(&m_buttonBoard, 6).OnFalse(frc2::InstantCommand([&]() -> void - { m_lock = false; }, - {}) - .ToPtr()); - - frc2::JoystickButton(&m_buttonBoard, 10).OnTrue(frc2::InstantCommand([&]() -> void - { m_shooterVelocity = 1320.0; }, - {}) - .ToPtr()); - - frc2::JoystickButton(&m_buttonBoard, 11).OnTrue(frc2::InstantCommand([&]() -> void - { m_shooterVelocity = 930.0; }, - {}) - .ToPtr()); - - frc2::JoystickButton(&m_buttonBoard, 12).OnTrue(frc2::InstantCommand([&]() -> void - { m_shooterVelocity = 400.0; }, - {}) - .ToPtr()); - - frc2::JoystickButton(&m_buttonBoard, 7).OnTrue(frc2::InstantCommand([&]() -> void - { ++m_LEDPattern; - if (m_LEDPattern >= m_LEDPatternCount) { m_LEDPattern = 0; } - std::printf("LED Pattern[%u]: %s\n", m_LEDPattern, std::string(m_infrastructureSubsystem.GetLEDPatternDescription(m_LEDPattern)).c_str()); }, - {}) - .ToPtr()); -} + // Set a raw trigger value using the right trigger + RawTrigVal = m_xboxDrive.GetRightTriggerAxis(); + double deadZoneVal = 0.05; + double deadZoneCorrection = 1.0 / (1.0 - deadZoneVal); -std::optional RobotContainer::GetAutonomousCommand() noexcept -{ -#if 0 - if (m_buttonBoard.GetRawButton(9)) + if (RawTrigVal < deadZoneVal) { - return TwoBallAuto::TwoBallAutoCommandFactory(&m_driveSubsystem, &m_feederSubsystem, &m_infrastructureSubsystem, &m_shooterSubsystem); + /*if the trigger value is less than deadzonevalue, it will + set the right trigger value to zero to correct drifting*/ + return 0; } else { - return OneBallAuto::OneBallAutoCommandFactory(&m_driveSubsystem, &m_feederSubsystem, &m_infrastructureSubsystem, &m_shooterSubsystem); + /* if the trigger value is greater than the deadzonevalue, it will + make a small correction to stop the increase in speed from being + too big */ + RawTrigVal -= deadZoneVal; + RawTrigVal *= deadZoneCorrection; + return std::pow(RawTrigVal, 3.0); // std::pow(RawTrigVal, 3.0) == RawTrigVal^3 } -#endif - - frc::TrajectoryConfig trajectoryConfig{4.0_mps, 2.0_mps_sq}; - frc::SwerveDriveKinematics<4> kinematics{m_driveSubsystem.kDriveKinematics}; - - trajectoryConfig.SetKinematics(kinematics); - - frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( - {frc::Pose2d{}, - frc::Pose2d{1.0_m, 0.0_m, frc::Rotation2d{}}}, - trajectoryConfig); - - return TrajectoryAuto::TrajectoryAutoCommandFactory(&m_driveSubsystem, "Test Trajectory", trajectory); } -std::tuple RobotContainer::GetDriveTeleopControls() noexcept +double RobotContainer::ConditionRawJoystickInput(double RawJoystickVal, double mixer) noexcept { - // The robot's frame of reference is the standard unit circle, from - // trigonometry. However, the front of the robot is facing along the positve - // X axis. This means the poitive Y axis extends outward from the left (or - // port) side of the robot. Poitive rotation is counter-clockwise. On the - // other hand, as the controller is held, the Y axis is aligned with forward. - // And, specifically, it is the negative Y axis which extends forward. So, - // the robot's X is the controllers inverted Y. On the controller, the X - // axis lines up with the robot's Y axis. And, the controller's positive X - // extends to the right. So, the robot's Y is the controller's inverted X. - // Finally, the other controller joystick is used for commanding rotation and - // things work out so that this is also an inverted X axis. - double x = -m_xbox.GetLeftY(); - double y = -m_xbox.GetLeftX(); - double z = -m_xbox.GetRightX(); - - // PlayStation controllers seem to do this strange thing with the rotation: - // double z = -m_xbox.GetLeftTriggerAxis(); - // Note: there is now a PS4Controller class. - - // Add some deadzone, so the robot doesn't drive when the joysticks are - // released and return to "zero". These implement a continuous deadband, one - // in which the full range of outputs may be generated, once joysticks move - // outside the deadband. - - // Also, cube the result, to provide more opertor control. Just cubing the - // raw value does a pretty good job with the deadband, but doing both is easy - // and guarantees no movement in the deadband. Cubing makes it easier to - // command smaller/slower movements, while still being able to command full - // power. The 'mixer` parameter is used to shape the `raw` input, some mix - // between out = in^3.0 and out = in. - auto shape = [](double raw, double mixer = 0.75) -> double + /* + PlayStation controllers seem to do this strange thing with the rotation: + double z = -m_xbox.GetLeftTriggerAxis(); + Note: there is now a PS4Controller class. + + Add some deadzone, so the robot doesn't drive when the joysticks are + released and return to "zero". These implement a continuous deadband, one + in which the full range of outputs may be generated, once joysticks move + outside the deadband. + + Also, cube the result, to provide more opertor control. Just cubing the + raw value does a pretty good job with the deadband, but doing both is easy + and guarantees no movement in the deadband. Cubing makes it easier to + command smaller/slower movements, while still being able to command full + power. The 'mixer` parameter is used to shape the `raw` input, some mix + between out = in^3.0 and out = in. + */ + + // Input deadband around 0.0 (+/- range). + constexpr double deadZoneVal = 0.05; + + constexpr double slope = 1.0 / (1.0 - deadZoneVal); + + if (RawJoystickVal >= -deadZoneVal && RawJoystickVal <= +deadZoneVal) { - // Input deadband around 0.0 (+/- range). - constexpr double range = 0.05; - - constexpr double slope = 1.0 / (1.0 - range); - - if (raw >= -range && raw <= +range) - { - raw = 0.0; - } - else if (raw < -range) - { - raw += range; - raw *= slope; - } - else if (raw > +range) - { - raw -= range; - raw *= slope; - } - - return mixer * std::pow(raw, 3.0) + (1.0 - mixer) * raw; - }; - - x = shape(x); - y = shape(y); - z = shape(z, 0.0); - - if (m_slow || m_buttonBoard.GetRawButton(9)) + RawJoystickVal = 0.0; + } + else if (RawJoystickVal < -deadZoneVal) { - x *= 0.50; - y *= 0.50; - z *= 0.40; + RawJoystickVal += deadZoneVal; + RawJoystickVal *= slope; } - else - { // XXX Still needed? - x *= 2.0; - y *= 2.0; - z *= 1.6; + else if (RawJoystickVal > +deadZoneVal) + { + RawJoystickVal -= deadZoneVal; + RawJoystickVal *= slope; } - return std::make_tuple(x, y, z, m_fieldOriented); + return mixer * std::pow(RawJoystickVal, 3.0) + (1.0 - mixer) * RawJoystickVal; } +void RobotContainer::ConfigureBindings() noexcept +{ + // TODO: define Keybindings here + m_xboxDrive.Start().OnTrue( + frc2::InstantCommand([&]() -> void + { triggerSpeedEnabled = !triggerSpeedEnabled; }, + {}) + .ToPtr()); + + m_xboxOperate.A().OnTrue(IntakeCommand(&m_intakeSubsystem).ToPtr()); + m_xboxOperate.B().OnTrue(IntakeEjectCommand(&m_intakeSubsystem).ToPtr()); + + // Runs shoot command to move arm into postion, start up the shooting motors and eject the note + //m_xboxOperate.Y().OnTrue(ShootCommands(&m_shooterSubsystem).ToPtr()); + + m_xboxOperate.Y().OnTrue(PIDPositionTransferArm(0_deg, &m_transferArmSubsystem).ToPtr().AndThen(ShootCommands(&m_shooterSubsystem).ToPtr()).AlongWith(IntakeEjectCommand(&m_intakeSubsystem).ToPtr())); + + m_xboxOperate.X().OnTrue(PIDPositionTransferArm(arm::kShooterToAmpAngle, &m_transferArmSubsystem).ToPtr()); // Example Only + m_xboxOperate.LeftBumper().OnTrue(PIDPositionTransferArm(arm::kShooterToIntakeAngle, &m_transferArmSubsystem).ToPtr()); // Intake + m_xboxOperate.RightBumper().OnTrue(PIDPositionTransferArm(0_deg, &m_transferArmSubsystem).ToPtr()); // Shooter + + m_xboxOperate.RightTrigger().OnTrue(ClimberRaiseCommand(&m_climberSubsystem).ToPtr()); // Raise the climber while button is pressed. + m_xboxOperate.RightTrigger().OnFalse(ClimberStopCommand(&m_climberSubsystem).ToPtr()); // on false stop the climber motor + + m_xboxOperate.LeftTrigger().OnTrue(ClimberLowerCommand(&m_climberSubsystem).ToPtr()); //Lower the climber while button is pressed + m_xboxOperate.LeftTrigger().OnFalse(ClimberStopCommand(&m_climberSubsystem).ToPtr()); // on false stop the climber motor +} +#pragma endregion + +#pragma region Test void RobotContainer::TestInit() noexcept { frc2::CommandScheduler::GetInstance().CancelAll(); m_driveSubsystem.ClearFaults(); - m_feederSubsystem.ClearFaults(); - m_shooterSubsystem.ClearFaults(); m_driveSubsystem.ResetEncoders(); m_driveSubsystem.TestInit(); - m_feederSubsystem.TestInit(); - m_shooterSubsystem.TestInit(); frc::SendableChooser> *chooser{m_driveSubsystem.TestModeChooser()}; - chooser->SetDefaultOption("Zero", std::bind(ZeroCommand::ZeroCommandFactory, &m_driveSubsystem)); - chooser->AddOption("Turning Max", std::bind(MaxVAndATurningCommand::MaxVAndATurningCommandFactory, &m_driveSubsystem)); - chooser->AddOption("Drive Max", std::bind(MaxVAndADriveCommand::MaxVAndADriveCommandFactory, &m_driveSubsystem)); - chooser->AddOption("Xs and Os", std::bind(XsAndOsCommand::XsAndOsCommandFactory, &m_driveSubsystem)); - chooser->AddOption("RotateModules", std::bind(RotateModulesCommand::RotateModulesCommandFactory, &m_driveSubsystem)); - chooser->AddOption("Point", std::bind(PointCommandFactory, this)); - chooser->AddOption("Square", std::bind(SquareCommand::SquareCommandFactory, &m_driveSubsystem)); - chooser->AddOption("Spirograph", std::bind(SpirographCommand::SpirographCommandFactory, &m_driveSubsystem)); - chooser->AddOption("Orbit", std::bind(OrbitCommand::OrbitCommandFactory, &m_driveSubsystem)); - chooser->AddOption("Pirouette", std::bind(PirouetteCommand::PirouetteCommandFactory, &m_driveSubsystem)); - chooser->AddOption("Drive", std::bind(DriveCommandFactory, this)); - chooser->AddOption("Spin", std::bind(SpinCommand::SpinCommandFactory, &m_driveSubsystem)); - frc2::CommandScheduler::GetInstance().Enable(); } +void RobotContainer::TestPeriodic() noexcept +{ + m_driveSubsystem.TestPeriodic(); +} + void RobotContainer::TestExit() noexcept { frc2::CommandScheduler::GetInstance().CancelAll(); m_driveSubsystem.ClearFaults(); - m_feederSubsystem.ClearFaults(); - m_shooterSubsystem.ClearFaults(); m_driveSubsystem.ResetEncoders(); m_driveSubsystem.TestExit(); - m_feederSubsystem.TestExit(); - m_shooterSubsystem.TestExit(); m_driveSubsystem.BurnConfig(); - m_feederSubsystem.BurnConfig(); - m_shooterSubsystem.BurnConfig(); } -void RobotContainer::TestPeriodic() noexcept +frc2::CommandPtr RobotContainer::PointCommandFactory(RobotContainer *container) noexcept { - m_driveSubsystem.TestPeriodic(); - m_feederSubsystem.TestPeriodic(); - m_shooterSubsystem.TestPeriodic(); + // Set up default drive command; non-owning pointer is passed by value. + auto driveRequirements = {dynamic_cast(&container->m_driveSubsystem)}; + + // Point swerve modules, but do not actually drive. + return frc2::CommandPtr{std::make_unique( + [container]() -> void + { + const auto controls = container->GetDriveTeleopControls(); + + units::angle::radian_t angle{std::atan2(std::get<0>(controls), std::get<1>(controls))}; + + // Ingnore return (bool); no need to check that commanded angle has + // been reached. + (void)container->m_driveSubsystem.SetTurningPosition(angle); + }, + driveRequirements)}; } +#pragma endregion +#pragma region Disabled void RobotContainer::DisabledInit() noexcept { frc2::CommandScheduler::GetInstance().CancelAll(); m_driveSubsystem.ClearFaults(); - m_feederSubsystem.ClearFaults(); - m_shooterSubsystem.ClearFaults(); m_driveSubsystem.ResetEncoders(); @@ -404,13 +357,14 @@ void RobotContainer::DisabledInit() noexcept m_driveSubsystem.DisabledInit(); } +void RobotContainer::DisabledPeriodic() noexcept {} + void RobotContainer::DisabledExit() noexcept { m_driveSubsystem.ClearFaults(); - m_feederSubsystem.ClearFaults(); - m_shooterSubsystem.ClearFaults(); m_driveSubsystem.ResetEncoders(); m_driveSubsystem.DisabledExit(); } +#pragma endregion diff --git a/src/main/cpp/commands/AutonomousCommands.cpp b/src/main/cpp/commands/AutonomousCommands.cpp index 7a197eb..4e2a15f 100644 --- a/src/main/cpp/commands/AutonomousCommands.cpp +++ b/src/main/cpp/commands/AutonomousCommands.cpp @@ -3,7 +3,6 @@ // the WPILib BSD license file in the root directory of this project. #include "commands/AutonomousCommands.h" - #include #include #include @@ -14,15 +13,10 @@ void TimedAutoBase::Initialize() noexcept m_drive->ResetDrive(); m_drive->ZeroHeading(); - m_feeder->Default(0.0); - m_feeder->LockIntake(); - m_feeder->RaiseIntake(); - - m_shooter->Stop(); + m_Intake->StopIntake(); - m_infrastructure->SetLEDPattern(95); + m_shooter->StopShooter(); - pressure_ = false; finished_ = false; FPGATime_ = frc::RobotController::GetFPGATime(); counter_ = 0; @@ -32,259 +26,15 @@ void TimedAutoBase::End(bool interrupted) noexcept { m_drive->Drive(0_mps, 0_mps, 0_deg_per_s, false); - m_feeder->Default(0.0); - - m_shooter->Stop(); + m_Intake->StopIntake(); - m_infrastructure->SetLEDPattern(95); + m_shooter->StopShooter(); } void TimedAutoBase::Execute() noexcept { const uint64_t FPGATime = frc::RobotController::GetFPGATime(); const uint deltaTime = (FPGATime - FPGATime_) / 1000; // ms - - if (deltaTime < 100) // 100ms - { - return; - } - - FPGATime_ = FPGATime; - - // Will arrive here every 100ms, for 15s autonomous period. - - const units::pressure::pounds_per_square_inch_t pressure = m_infrastructure->GetPressure(); - - if (pressure >= 75_psi) - { - pressure_ = true; - } - - if (!pressure_) - { - m_infrastructure->SetLEDPattern(78); - - return; - } - - // Assuming pneumatics were prefilled, counter runs 1 - ~150. - if (Iteration(++counter_)) - { - m_infrastructure->SetLEDPattern(95); - - finished_ = true; - } -} - -bool OneBallAuto::Iteration(const uint counter) noexcept -{ - // Sit still and run intake/elevator. - if (counter <= 5) // 0.0 - 0.5s - { - m_drive->Drive(0_mps, 0_mps, 0_deg_per_s, false); - - m_feeder->Default(1.0); - m_feeder->LockIntake(); - m_feeder->RaiseIntake(); - - m_shooter->Stop(); - - m_infrastructure->SetLEDPattern(79); - - return false; - } - - // Drop intake (first ball is preloaded). - if (counter <= 10) // 0.5 - 1.0s - { - m_feeder->DropIntake(); - - m_infrastructure->SetLEDPattern(80); - - return false; - } - - // Reextend drop catches, lower intake. - if (counter <= 15) // 1.0 - 1.5s - { - m_feeder->LockIntake(); - m_feeder->LowerIntake(); - - m_infrastructure->SetLEDPattern(81); - - return false; - } - - // Back up and spin up shooter. - if (counter <= 35) // 1.5 - 3.5s - { - m_drive->Drive(-0.55_mps, 0_mps, 0_deg_per_s, false); - - m_feeder->Default(0.0); - - m_shooter->Default(1.0, 1125.0); - - m_infrastructure->SetLEDPattern(82); - - return false; - } - - // Stop. - if (counter <= 40) // 3.5 - 4.0s - { - m_drive->Drive(0_mps, 0_mps, 0_deg_per_s, false); - - m_infrastructure->SetLEDPattern(83); - - return false; - } - - // Take the shot! - if (counter <= 60) // 4.0 - 6.0s - { - m_feeder->Fire(); - - m_infrastructure->SetLEDPattern(84); - - return false; - } - - return true; -} - -bool TwoBallAuto::Iteration(const uint counter) noexcept -{ - // Sit still. - if (counter <= 5) // 0.0 - 0.5s - { - m_drive->Drive(0_mps, 0_mps, 0_deg_per_s, false); - - m_feeder->Default(0.0); - m_feeder->LockIntake(); - m_feeder->RaiseIntake(); - - m_shooter->Stop(); - - m_infrastructure->SetLEDPattern(79); - - return false; - } - - // Drop intake (first ball is preloaded). - if (counter <= 10) // 0.5 - 1.0s - { - m_feeder->DropIntake(); - - m_infrastructure->SetLEDPattern(80); - - return false; - } - - // Reextend drop catches, lower intake. - if (counter <= 15) // 1.0 - 1.5s - { - m_feeder->LockIntake(); - m_feeder->LowerIntake(); - - m_infrastructure->SetLEDPattern(81); - - return false; - } - - // Drive forward, run intake. - if (counter <= 30) // 1.5 - 3.0s - { - m_drive->Drive(0.55_mps, 0_mps, 0_deg_per_s, false); - - m_feeder->Default(1.0); - - m_infrastructure->SetLEDPattern(82); - - return false; - } - - // Stop. - if (counter <= 35) // 3.0 - 3.5s - { - m_drive->Drive(0_mps, 0_mps, 0_deg_per_s, false); - - m_infrastructure->SetLEDPattern(83); - - return false; - } - - // Turn around and spin up shooter. - if (counter < 85) // 3.5 - 8.5s - { - if (m_drive->SetTurnToAngle(180_deg)) - { - m_infrastructure->SetLEDPattern(85); - } - else - { - m_infrastructure->SetLEDPattern(84); - } - - m_shooter->Default(1.0, 1125.0); - - return false; - } - else if (counter == 85) - { - m_drive->Drive(0_mps, 0_mps, 0_deg_per_s, false); - m_drive->ResetDrive(); - m_drive->ZeroHeading(); - - m_infrastructure->SetLEDPattern(86); - - m_shooter->Default(1.0, 1125.0); - - return false; - } - - // Spin up shooter. - if (counter <= 90) // 8.5 - 9.0s - { - m_shooter->Default(1.0, 1125.0); - - m_feeder->Default(0.0); - - m_infrastructure->SetLEDPattern(87); - - return false; - } - - // Take first shot! - if (counter <= 110) // 9.0 - 11.0s - { - m_feeder->Fire(); - - m_infrastructure->SetLEDPattern(88); - - return false; - } - - // Feed and hold second shot. - if (counter <= 120) // 11.0 - 12.0s - { - m_feeder->Default(1.0); - - m_infrastructure->SetLEDPattern(89); - - return false; - } - - // Take second, buzzer beater shot! - if (counter <= 150) // 13.0 - 15.0s - { - m_feeder->Fire(); - - m_infrastructure->SetLEDPattern(90); - - return false; - } - - return true; } frc2::CommandPtr TrajectoryAuto::TrajectoryAutoCommandFactory(DriveSubsystem *const drive, std::string_view name, frc::Trajectory &trajectory) noexcept @@ -293,8 +43,8 @@ frc2::CommandPtr TrajectoryAuto::TrajectoryAutoCommandFactory(DriveSubsystem *co trajectory, [drive]() -> frc::Pose2d { return drive->GetPose(); }, drive->kDriveKinematics, - frc2::PIDController{0.6, 0, 0}, - frc2::PIDController{0.6, 0, 0}, + frc::PIDController{0.6, 0, 0}, + frc::PIDController{0.6, 0, 0}, frc::ProfiledPIDController{pidf::kDriveThetaP, pidf::kDriveThetaI, pidf::kDriveThetaD, frc::TrapezoidProfile::Constraints{pidf::kDriveThetaMaxVelocity, pidf::kDriveThetaMaxAcceleration}}, // [drive]() -> frc::Rotation2d "desiredRotation" [drive](std::array states) -> void diff --git a/src/main/cpp/commands/ClimberLowerCommand.cpp b/src/main/cpp/commands/ClimberLowerCommand.cpp new file mode 100644 index 0000000..6d905b8 --- /dev/null +++ b/src/main/cpp/commands/ClimberLowerCommand.cpp @@ -0,0 +1,35 @@ +// 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. + +#include "commands/ClimberLowerCommand.h" + +/* +ClimberLowerCommand::ClimberLowerCommand() { + // Use addRequirements() here to declare subsystem dependencies. +} +*/ + +// Called when the command is initially scheduled. +void ClimberLowerCommand::Initialize() { + timer.Reset(); + timer.Start(); + finished = false; +} + +// Called repeatedly when this Command is scheduled to run +void ClimberLowerCommand::Execute() { + //Start the climber motor to move climbing hook up. + climberSubsystem->SetClimberMotorVoltagePercent(climber::kClimberMotorLowerVoltagePercent); + // End command after time defined in constants file. + if (timer.HasElapsed(climber::kClimberLowerTimer)) { finished = true; }} + +// Called once the command ends or is interrupted. +void ClimberLowerCommand::End(bool interrupted) { + climberSubsystem->StopClimber(); +} + +// Returns true when the command should end. +bool ClimberLowerCommand::IsFinished() { + return finished; +} diff --git a/src/main/cpp/commands/ClimberRaiseCommand.cpp b/src/main/cpp/commands/ClimberRaiseCommand.cpp new file mode 100644 index 0000000..f05ab1b --- /dev/null +++ b/src/main/cpp/commands/ClimberRaiseCommand.cpp @@ -0,0 +1,30 @@ +// 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. + +#include "commands/ClimberRaiseCommand.h" + + +// Called when the command is initially scheduled. +void ClimberRaiseCommand::Initialize() { + timer.Reset(); + timer.Start(); +} + +// Called repeatedly when this Command is scheduled to run +void ClimberRaiseCommand::Execute() { + //Start the climber motor to move climbing hook up. + climberSubsystem->SetClimberMotorVoltagePercent(climber::kClimberMotorRaiseVoltagePercent); + // End command after time defined in constants file. + if (timer.HasElapsed(climber::kClimberRaiseTimer)) { finished = true; } +} + +// Called once the command ends or is interrupted. +void ClimberRaiseCommand::End(bool interrupted) { + climberSubsystem->StopClimber(); +} + +// Returns true when the command should end. +bool ClimberRaiseCommand::IsFinished() { + return finished; +} diff --git a/src/main/cpp/commands/ClimberStopCommand.cpp b/src/main/cpp/commands/ClimberStopCommand.cpp new file mode 100644 index 0000000..c21cf83 --- /dev/null +++ b/src/main/cpp/commands/ClimberStopCommand.cpp @@ -0,0 +1,25 @@ +// 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. + +#include "commands/ClimberStopCommand.h" + + +// Called when the command is initially scheduled. +void ClimberStopCommand::Initialize() +{ + //Stop the motors + climberSubsystem->StopClimber(); + finished = true; +} + +// Called repeatedly when this Command is scheduled to run +void ClimberStopCommand::Execute() {} + +// Called once the command ends or is interrupted. +void ClimberStopCommand::End(bool interrupted) {} + +// Returns true when the command should end. +bool ClimberStopCommand::IsFinished() { + return finished; +} diff --git a/src/main/cpp/commands/DriveCommands.cpp b/src/main/cpp/commands/DriveCommands.cpp new file mode 100644 index 0000000..5b8753d --- /dev/null +++ b/src/main/cpp/commands/DriveCommands.cpp @@ -0,0 +1,27 @@ +// 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. + +#include "commands/DriveCommands.h" + +void ZeroTurningModules::Execute() noexcept { (void)driveSubsystem->ZeroModules(); } + +void DriveCommand::Initialize() noexcept +{ + driveSubsystem->Drive(0.0_mps, 0.0_mps, 0.0_rad_per_s, true); +} + +void DriveCommand::Execute() noexcept +{ + // TODO: call handle drive subsystem drive + // this will need to accept a parameter in the constructor + // and will be implemented similar to RobotContainer.cpp + // drivecommandfactory. + // Instead of reading the values from the controller input + // they will be passed in as parameters. +} + +void DriveCommand::End(bool interrupted) noexcept +{ + driveSubsystem->Drive(0.0_mps, 0.0_mps, 0.0_rad_per_s, true); +} diff --git a/src/main/cpp/commands/IntakeCommand.cpp b/src/main/cpp/commands/IntakeCommand.cpp new file mode 100644 index 0000000..0041cf6 --- /dev/null +++ b/src/main/cpp/commands/IntakeCommand.cpp @@ -0,0 +1,51 @@ +// 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. + +#include "commands/IntakeCommand.h" +#include +#include +#include +#include +#include +#include +#include +#include + +// Called when the command is initially scheduled. +void IntakeCommand::Initialize() { + /* Start the intake motors. + Set finished to false to allow multiple calls of this command. + */ + intakeSubsystem->SetSpinMotorVoltagePercent(intake::kIntakeSpinMotorVoltagePercent); + finished = false; + + frc::SmartDashboard::PutBoolean("Limit1 Boolean Value: ", limit1.Get()); + frc::SmartDashboard::PutBoolean("Limit2 Boolean Value: ", limit2.Get()); +} + +// Called repeatedly when this Command is scheduled to run +void IntakeCommand::Execute() { + /* Checks if the limitswitches have been activated. If so sets finished to true and intake to stop. + Need to add code for moving arm pack to home position. */ + frc::SmartDashboard::PutBoolean("Limit1 Boolean Value: ", limit1.Get()); + frc::SmartDashboard::PutBoolean("Limit2 Boolean Value: ", limit2.Get()); + + if (limit1.Get() or limit2.Get()){ + + finished = true; + intakeSubsystem->StopIntake(); + //Add code to raise the arm to the home position + } + +} + +// Called once the command ends or is interrupted. +void IntakeCommand::End(bool interrupted) { + +} + +// Returns true when the command should end. +bool IntakeCommand::IsFinished() { + return finished; +} diff --git a/src/main/cpp/commands/IntakeEjectCommand.cpp b/src/main/cpp/commands/IntakeEjectCommand.cpp new file mode 100644 index 0000000..1a57feb --- /dev/null +++ b/src/main/cpp/commands/IntakeEjectCommand.cpp @@ -0,0 +1,44 @@ +// 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. + +#include "commands/IntakeEjectCommand.h" + + +// Called when the command is initially scheduled. +void IntakeEjectCommand::Initialize() { + /* Start the intake motors in the reverse direction + Set finished to false to allow multiple calls of this command + */ + + + finished = false; + timer.Reset(); + timer.Start(); + +} + +// Called repeatedly when this Command is scheduled to run +void IntakeEjectCommand::Execute() { + + if (timer.HasElapsed(2_s)){ + intakeSubsystem->SetSpinMotorVoltagePercent(intake::kIntakeSpinMotorEjectVoltagePercent); + } + + //Run the intake motors in reverse for 2 seconds then stop the intake + if (timer.HasElapsed(4_s)){ + finished = true; + intakeSubsystem->StopIntake(); + } +} + +// Called once the command ends or is interrupted. +void IntakeEjectCommand::End(bool interrupted) { + //Stop the intake motors + intakeSubsystem->StopIntake(); +} + +// Returns true when the command should end. +bool IntakeEjectCommand::IsFinished() { + return finished; +} diff --git a/src/main/cpp/commands/PIDTransferArmCommand.cpp b/src/main/cpp/commands/PIDTransferArmCommand.cpp new file mode 100644 index 0000000..9bd367e --- /dev/null +++ b/src/main/cpp/commands/PIDTransferArmCommand.cpp @@ -0,0 +1,44 @@ +// 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. + +#include "commands/PIDTransferArmCommand.h" +#include + +PIDPositionTransferArm::PIDPositionTransferArm(units::turn_t targetPosition, TransferArmSubsystem* transferArmSubsystem) + : CommandHelper{frc::PIDController{arm::kArmPositionP, 0, arm::kArmPositionD}, + // Close loop on heading + [transferArmSubsystem] + { + frc::SmartDashboard::PutNumber("Arm Position ", transferArmSubsystem->GetTransferArmPosition().value()); + return transferArmSubsystem->GetTransferArmPosition().value(); + }, + // Set reference to targetPosition + targetPosition.value(), + // Pipe output to turn transfer arm + [transferArmSubsystem](double output) + { + frc::SmartDashboard::PutNumber("Arm PID Output ", output); + transferArmSubsystem->SetArmMotorVoltagePercent(output); + }, + // Require the transfer arm + {transferArmSubsystem}} { + // Set the controller to be continuous (because it is an angle controller) + m_controller.EnableContinuousInput(-1, 1); + // Set the controller tolerance - the delta tolerance ensures the robot is + // stationary at the setpoint before it is considered as having reached the + // reference + m_controller.SetTolerance(.01, .01); + + AddRequirements(transferArmSubsystem); + + frc::SmartDashboard::PutBoolean("PID Command Reset ", true); +} + +bool PIDPositionTransferArm::IsFinished() { + if(m_controller.AtSetpoint()) + { + frc::SmartDashboard::PutBoolean("PID Command Reset ", false); + } + return m_controller.AtSetpoint(); +} diff --git a/src/main/cpp/commands/PositionTransferArmCommand.cpp b/src/main/cpp/commands/PositionTransferArmCommand.cpp new file mode 100644 index 0000000..c6d13fd --- /dev/null +++ b/src/main/cpp/commands/PositionTransferArmCommand.cpp @@ -0,0 +1,28 @@ +// 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. + +#include "commands/PositionTransferArmCommand.h" +#include + +void PositionTransferArm::Initialize() noexcept +{ + finished = false; + + // transferArmSubsystem->SetTransferArmPosition(position); + timer.Reset(); + timer.Start(); +} + +void PositionTransferArm::Execute() noexcept +{ + frc::SmartDashboard::PutNumber("Arm Timer ", timer.Get().value()); + // transferArmSubsystem->UpdatePIDValues();s + transferArmSubsystem->SetArmMotorVoltagePercent(.05); + if (timer.HasElapsed(2_s)) { finished = true; } +} + +void PositionTransferArm::End(bool interrupted) noexcept +{ + transferArmSubsystem->StopTransferArm(); +} diff --git a/src/main/cpp/commands/ShootCommands.cpp b/src/main/cpp/commands/ShootCommands.cpp new file mode 100644 index 0000000..b4fb3fe --- /dev/null +++ b/src/main/cpp/commands/ShootCommands.cpp @@ -0,0 +1,37 @@ +// 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. + +#include "commands/ShootCommands.h" + +// Called when the command is initially scheduled. +void ShootCommands::Initialize() { + + //Start the shooter motors and timer + shooterSubsystem->SetShooterMotorVoltagePercent(shooter::kShooterMotorVoltagePercent); + finished = false; + timer.Reset(); + timer.Start(); + + +} + +// Called repeatedly when this Command is scheduled to run +void ShootCommands::Execute() { + //When the timer has run for 5 seconds set finished to true to stop the motors + if (timer.HasElapsed(5_s)) { + finished = true; + shooterSubsystem->StopShooter(); + } +} + +// Called once the command ends or is interrupted. +void ShootCommands::End(bool interrupted) { + //Stop the shooter motors + shooterSubsystem->StopShooter(); +} + +// Returns true when the command should end. +bool ShootCommands::IsFinished() { + return finished; +} diff --git a/src/main/cpp/commands/TestModeCommands.cpp b/src/main/cpp/commands/TestModeCommands.cpp deleted file mode 100644 index 69ffd5c..0000000 --- a/src/main/cpp/commands/TestModeCommands.cpp +++ /dev/null @@ -1,311 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "commands/TestModeCommands.h" - -#include - -void ZeroCommand::Execute() noexcept { (void)m_subsystem->ZeroModules(); } - -void MaxVAndATurningCommand::Initialize() noexcept -{ - m_iteration = 0; - - m_subsystem->TestModeTurningVoltage(0.0); - m_subsystem->TestModeDriveVoltage(0.0); -} - -void MaxVAndATurningCommand::End(bool interrupted) noexcept -{ - m_subsystem->TestModeTurningVoltage(0.0); - m_subsystem->TestModeDriveVoltage(0.0); -} - -void MaxVAndATurningCommand::Execute() noexcept -{ - // This is expected to run at ~20Hz. So 100 iterations is ~5s. - if (m_iteration < 50) - { - m_subsystem->TestModeTurningVoltage(-12.0); - } - else - { - m_subsystem->TestModeTurningVoltage(+12.0); - } - - if (++m_iteration >= 100) - { - m_iteration = 0; - } -} - -void MaxVAndADriveCommand::Initialize() noexcept -{ - m_iteration = 0; - - m_subsystem->TestModeTurningVoltage(0.0); - m_subsystem->TestModeDriveVoltage(0.0); -} - -void MaxVAndADriveCommand::End(bool interrupted) noexcept -{ - m_subsystem->TestModeTurningVoltage(0.0); - m_subsystem->TestModeDriveVoltage(0.0); -} - -void MaxVAndADriveCommand::Execute() noexcept -{ - // This is expected to run at ~20Hz. So 100 iterations is ~5s. - if (m_iteration < 50) - { - m_subsystem->TestModeDriveVoltage(-12.0); - } - else - { - m_subsystem->TestModeDriveVoltage(+12.0); - } - - if (++m_iteration >= 100) - { - m_iteration = 0; - } -} - -void XsAndOsCommand::Initialize() noexcept -{ - m_iteration = 0; - - m_subsystem->ResetDrive(); -} - -void XsAndOsCommand::End(bool interrupted) noexcept -{ - m_subsystem->ResetDrive(); -} - -void XsAndOsCommand::Execute() noexcept -{ - // This is expected to run at ~20Hz. So 100 iterations is ~5s. - if (m_iteration < 100) - { - (void)m_subsystem->SetLockWheelsX(); - } - else - { - (void)m_subsystem->SetTurnInPlace(); - } - - if (++m_iteration >= 200) - { - m_iteration = 0; - } -} - -void RotateModulesCommand::Initialize() noexcept -{ - m_iteration = 0; - - m_subsystem->ResetDrive(); -} - -void RotateModulesCommand::End(bool interrupted) noexcept -{ - m_subsystem->ResetDrive(); -} - -void RotateModulesCommand::Execute() noexcept -{ - // This is expected to run at ~20Hz. So 100 iterations is ~5s. - if (!m_subsystem->SetTurningPosition(1_deg * m_iteration)) - { - return; - } - - if (++m_iteration >= 360) - { - m_iteration = 0; - } -} - -void SquareCommand::Initialize() noexcept -{ - m_side = 0; - - m_subsystem->ResetDrive(); -} - -void SquareCommand::End(bool interrupted) noexcept -{ - m_subsystem->ResetDrive(); -} - -void SquareCommand::Execute() noexcept -{ - // First, command turning to the appropriate angle and go no further, until - // the wheels are oriented appropriately. - if (!m_subsystem->SetTurningPosition(90_deg * m_side)) - { - return; - } - - // Now, command driving a fixed distance and go no further, until it is - // reached. - if (!m_subsystem->SetDriveDistance(0.5_m)) - { - return; - } - - // Finished driving a side; now, set things up to start the next side, on - // the next iteration. After the fourth side, also reset the side counter. - m_subsystem->ResetDrive(); - - if (++m_side >= 4) - { - m_side = 0; - } -} - -void SpirographCommand::Initialize() noexcept -{ - m_angle = 0; - - m_subsystem->ResetDrive(); -} - -void SpirographCommand::End(bool interrupted) noexcept -{ - m_subsystem->ResetDrive(); -} - -void SpirographCommand::Execute() noexcept -{ - // First, command turning to the appropriate angle and go no further, until - // the wheels are oriented appropriately. - if (!m_subsystem->SetTurningPosition(1_deg * m_angle)) - { - return; - } - - // Now, command driving a fixed distance and go no further, until it is - // reached. - if (!m_subsystem->SetDriveDistance(0.5_m)) - { - return; - } - - // Finished driving a side; now, set things up to start the next side, on - // the next iteration. After the fourth side, also reset the side counter. - m_subsystem->ResetDrive(); - - m_angle += 75; - if (m_angle >= 360) - { - m_angle -= 360; - } -} - -void OrbitCommand::Execute() noexcept -{ - m_subsystem->Drive(0_mps, 0_mps, 10_deg_per_s, false, 1_m, 0_m); -} - -void PirouetteCommand::Initialize() noexcept -{ - // Arrange the origin and orientation of the coordinate system, so that the - // robot is on the unit circle, at (1,0) and facing positive X. (The robot - // does not move here, the coordinate system itself moves.) - m_subsystem->ResetOdometry(frc::Pose2d(1_m, 0_m, frc::Rotation2d(0_deg))); -} - -void PirouetteCommand::End(bool interrupted) noexcept -{ -} - -void PirouetteCommand::Execute() noexcept -{ - // Determine the direction the robot is facing, and where it is positioned. - // As it happens, the direction the robot is facing is not used here. - const frc::Pose2d pose = m_subsystem->GetPose(); - - // Now, work out the angle of where the robot is positioned on the unit - // circle, which is independent of where it is facing. It may drift either - // inside or outside of the unit circle. - const double X = pose.X().to(); - const double Y = pose.Y().to(); - frc::Rotation2d major_angle(X, Y); - - // The robot should translate tangentially to the unit circle, in a - // counter-clockwise fashion. So, add 90 degrees. - major_angle = major_angle + frc::Rotation2d(90_deg); - - // Work out the radius of the circle where robot is actually sitting, so it - // may be used to make an adjustment to try to get back onto the unit - // circle. Positive error means robot is outside target circle, negative - // means inside. Outside correction is to increase the computed angle; - // inside is to decrease. - double major_error = std::hypot(X, Y) - 1.0; - - // Limit the maximum correction to apply and then apply it. - if (major_error > 0.5) - { - major_error = 0.5; - } - else if (major_error < -0.5) - { - major_error = -0.5; - } - major_angle = major_angle + frc::Rotation2d(major_error * 20_deg); - - // Update X and Y drive command inputs based on above computations, while - // always commanding rotation. The commanded drive needs to be adjusted to - // account for the actual orientation of the robot, thus field oriented - // applies. - m_subsystem->Drive(major_angle.Sin() * 1_mps, major_angle.Cos() * 1_mps, 10_deg_per_s, true); -} - -void SpinCommand::Initialize() noexcept -{ - m_angle = 0; - m_delay = 0; - - m_subsystem->ResetDrive(); -} - -void SpinCommand::Execute() noexcept -{ - if (!m_subsystem->SetTurnInPlace()) - { - return; - } - - if (!m_subsystem->SetTurnToAngle(1_deg * m_angle)) - { - return; - } - - // Reached desired angle; delay for 2.5s. - if (++m_delay < 25) - { - return; - } - m_delay = 0; - - if (m_angle == 0) - { - m_angle = 90; - } - else if (m_angle == 90) - { - m_angle = 270; - } - else if (m_angle == 270) - { - m_angle = 0; - } -} - -void SpinCommand::End(bool interrupted) noexcept -{ - m_subsystem->ResetDrive(); -} diff --git a/src/main/cpp/infrastructure/PWMAngleSensor.cpp b/src/main/cpp/infrastructure/PWMAngleSensor.cpp index db19970..d0b4f0b 100644 --- a/src/main/cpp/infrastructure/PWMAngleSensor.cpp +++ b/src/main/cpp/infrastructure/PWMAngleSensor.cpp @@ -1,11 +1,23 @@ #include "infrastructure/PWMAngleSensor.h" #include "infrastructure/ShuffleboardWidgets.h" - -AngleSensor::AngleSensor(const int channel, const int alignment) noexcept : alignment_{alignment} -{ - digitalInput_ = std::make_unique(channel); - dutyCycle_ = std::make_unique(*digitalInput_); +#include +#include +#include +#include + +AngleSensor::AngleSensor(int deviceID, units::turn_t alignment) noexcept + : canCoder_(deviceID) { + // Configure the sensor range and other settings + canCoder_.GetConfigurator().Apply( + ctre::phoenix6::configs::MagnetSensorConfigs() + .WithAbsoluteSensorRange(ctre::phoenix6::signals::AbsoluteSensorRangeValue::Signed_PlusMinusHalf) + .WithMagnetOffset(alignment.value()) + // .WithSensorDirection(ctre::phoenix6::signals::SensorDirectionValue::Clockwise_Positive) + ); + int deviceId = canCoder_.GetDeviceID(); // Obtain the CANCoder device ID + std::string deviceIdStr = std::to_string(deviceId); // Convert the device ID to string + frc::SmartDashboard::PutNumber("Alignment Device ID: " + deviceIdStr, alignment.value()); } void AngleSensor::Periodic() noexcept @@ -27,27 +39,17 @@ void AngleSensor::Periodic() noexcept encoderPosition = std::get<1>(pair); } - const int frequency = dutyCycle_->GetFrequency(); - const double output = dutyCycle_->GetOutput(); - const auto position = GetAbsolutePosition(frequency, output, false); + const auto position = GetAbsolutePositionWithoutAlignment(); const bool status = position.has_value(); - int reportPosition{0}; + const units::turn_t alignment = GetAlignment(); + units::degree_t reportPosition{0}; if (status) { - reportPosition = position.value() + alignment_; - - if (reportPosition > 2047) - { - reportPosition = reportPosition - 4096; - } - if (reportPosition < -2048) - { - reportPosition = reportPosition + 4096; - } + reportPosition = position.value() + alignment; } - const double heading = static_cast(reportPosition) * 360.0 / 4096.0; + const double heading = reportPosition.value(); headingGyro_.Set(heading); @@ -73,27 +75,23 @@ void AngleSensor::Periodic() noexcept encoderError -= 360.0; } - frequencyUI_->GetEntry()->SetInteger(frequency); commandDiscrepancyUI_->GetEntry()->SetDouble(commandedError); statusUI_->GetEntry()->SetBoolean(status); commandedUI_->GetEntry()->SetDouble(commandedPosition.to()); - positionUI_->GetEntry()->SetInteger(reportPosition); - outputUI_->GetEntry()->SetDouble(output); + positionUI_->GetEntry()->SetDouble(reportPosition.to()); encoderDiscrepancyUI_->GetEntry()->SetDouble(encoderError); - alignmentUI_->GetEntry()->SetInteger(alignment_); + alignmentUI_->GetEntry()->SetDouble(alignment.to()); } void AngleSensor::ShuffleboardCreate(frc::ShuffleboardContainer &container, std::function()> getCommandedAndEncoderPositionsF) noexcept { - frequencyUI_ = &container.Add("Frequency", 0) - .WithPosition(0, 0); commandDiscrepancyUI_ = &container.Add("PID Error", 0) - .WithPosition(0, 1); + .WithPosition(0, 0); statusUI_ = &container.Add("Status", false) - .WithPosition(0, 2) + .WithPosition(0, 1) .WithWidget(frc::BuiltInWidgets::kBooleanBox); headingUI_ = &container.Add("Heading", headingGyro_) @@ -108,108 +106,65 @@ void AngleSensor::ShuffleboardCreate(frc::ShuffleboardContainer &container, positionUI_ = &container.Add("Position", 0) .WithPosition(1, 2); - outputUI_ = &container.Add("Output", 0.0) - .WithPosition(2, 0); - encoderDiscrepancyUI_ = &container.Add("Motor Discrepancy", 0) - .WithPosition(2, 1); + .WithPosition(2, 0); alignmentUI_ = &container.Add("Alignment", 0) - .WithPosition(2, 2); + .WithPosition(2, 1); getCommandedAndEncoderPositionsF_ = getCommandedAndEncoderPositionsF; shuffleboard_ = true; } -std::optional AngleSensor::GetAbsolutePosition() noexcept -{ - const auto position = GetAbsolutePosition(dutyCycle_->GetFrequency(), dutyCycle_->GetOutput(), true); +units::turn_t AngleSensor::GetAlignment() noexcept { + ctre::phoenix6::configs::MagnetSensorConfigs sensorConfig = ctre::phoenix6::configs::MagnetSensorConfigs(); + canCoder_.GetConfigurator().Refresh(sensorConfig); + return (units::turn_t)sensorConfig.MagnetOffset; +}; + +void AngleSensor::SetAlignment(const units::turn_t alignment) noexcept { + canCoder_.GetConfigurator().Apply( + ctre::phoenix6::configs::MagnetSensorConfigs() + .WithMagnetOffset(alignment.value()) + ); +}; +/* +std::optional AngleSensor::GetAbsolutePosition() noexcept { + units::degree_t position = canCoder_.GetPosition().GetValue(); + // Position will already within [-180 to 180) degree range, offset to match magnet position + frc::SmartDashboard::PutNumber("AngleSensor::GetAbsolutePosition Value", position.value()); + return position; +} +*/ +std::optional AngleSensor::GetAbsolutePosition() noexcept { + units::degree_t position = canCoder_.GetPosition().GetValue(); // Assuming this returns units::angle::degree_t + int deviceId = canCoder_.GetDeviceID(); // Obtain the CANCoder device ID + std::string deviceIdStr = std::to_string(deviceId); // Convert the device ID to string - if (!position.has_value()) - { - return std::nullopt; - } + // Use the device ID in the SmartDashboard key + frc::SmartDashboard::PutNumber("AngleSensor::GetAbsolutePosition Absolute ID: " + deviceIdStr, position.value()); - return units::angle::turn_t{static_cast(position.value()) / 4096.0}; -} + // Convert to radians and back to use MathUtil AngleModulus + units::degree_t normalizedPosition = frc::AngleModulus(position); -std::optional AngleSensor::GetAbsolutePositionWithoutAlignment() noexcept -{ - return GetAbsolutePosition(dutyCycle_->GetFrequency(), dutyCycle_->GetOutput(), false); + // Use the device ID in the adjusted position SmartDashboard key as well + frc::SmartDashboard::PutNumber("AngleSensor::GetAbsolutePosition Adjusted ID: " + deviceIdStr, normalizedPosition.value()); + return normalizedPosition; } -// Calulate absolute turning position in the range [-2048, +2048), from the raw -// absolute PWM encoder data. No value is returned in the event the absolute -// position sensor itself is not returning valid data. The alignment offset is -// optionally used to establish the zero position. - -// This is a low-level routine, meant to be used only by the version of -// GetAbsolutePosition() with no arguments (above) or, from test mode. It does -// not use standardized units and leaks knowledge of the sensor output range. -std::optional AngleSensor::GetAbsolutePosition(const int frequency, const double output, const bool applyOffset) noexcept +std::optional AngleSensor::GetAbsolutePositionWithoutAlignment() noexcept { - // SRX MAG ENCODER chip seems to be AS5045B; from the datasheet, position - // is given by: - // ((t_on * 4098) / (t_on + t_off)) - 1; - // if this result is 4096, set it to 4095. - // This computation results in a position in the range [0, 4096). - - // REV Through Bore Encoder chip is AEAT-8800-Q24, which has configurable - // PWM parameters. However, it seems to be configured to match AS5045B, so - // it works without any changes. A factory preset zero offset may allow no - // alignment (simply specify an alignment of zero). - - // If the frequency isn't within the range specified in the data sheet, - // return an error. This range is [220, 268], with a nominal value of 244. - // A tolerance of 12 (~5% of nominal) is provided for any measurment error. - bool absoluteSensorGood = frequency >= 208 && frequency <= 280; - - if (!absoluteSensorGood) - { - return std::nullopt; - } + units::degree_t position = canCoder_.GetPosition().GetValue(); + int deviceId = canCoder_.GetDeviceID(); // Obtain the CANCoder device ID + std::string deviceIdStr = std::to_string(deviceId); // Convert the device ID to string - // GetOutput() is (t_on / (t_on + t_off)); this is all that we have; note - // that this is sampled at a high frequency and the sampling window may not - // be perfectly aligned with the signal, although the frequency should be - // employed to ensure the sampling window is sized to a multiple of the - // period of the signal. So, t_on and t_off have a different scale in this - // context, which is OK. We are using the duty cycle (ratio) only. - - // Multiply by 4098 and subtract 1; should yield 0 - 4094, and also 4096. - // Conditionals cover special case of 4096 and enforce the specified range. - int position = std::lround(output * 4098.0) - 1; - if (position < 0) - { - position = 0; - } - else if (position > 4095) - { - position = 4095; - } + frc::SmartDashboard::PutNumber("AngleSensor::GetAbsolutePositionWithoutAlignment Absolute ID: " + deviceIdStr, position.value()); - // Now, shift the range from [0, 4096) to [-2048, +2048). - position -= 2048; - - if (!applyOffset) - { - return position; - } + position -= GetAlignment(); - // There is a mechanical offset reflecting the alignment of the magnet with - // repect to the axis of rotation of the wheel (and also any variance in - // the alignment of the sensor). Add this in here to reference position to - // the desired zero position. - position += alignment_; - if (position > 2047) - { - position -= 4096; - } - if (position < -2048) - { - position += 4096; - } - - return position; + // Convert to radians and back to use MathUtil AngleModulus + units::degree_t normalizedPosition = frc::AngleModulus(position); + frc::SmartDashboard::PutNumber("AngleSensor::GetAbsolutePositionWithoutAlignment Adjusted ID: " + deviceIdStr, normalizedPosition.value()); + return normalizedPosition; } diff --git a/src/main/cpp/infrastructure/SparkMax.cpp b/src/main/cpp/infrastructure/SparkMax.cpp index e7ad4b1..8c3986e 100644 --- a/src/main/cpp/infrastructure/SparkMax.cpp +++ b/src/main/cpp/infrastructure/SparkMax.cpp @@ -9,8 +9,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -132,12 +132,12 @@ namespace // Underlying REV object holders. std::unique_ptr motor_; std::unique_ptr encoder_; - std::unique_ptr controller_; + std::unique_ptr controller_; // REV object holders, only used when not using an external // ("alternate") encoder. - std::unique_ptr forward_; - std::unique_ptr reverse_; + std::unique_ptr forward_; + std::unique_ptr reverse_; // Shuffleboard UI elements, used by ShuffleboardCreate() and // ShuffleboardPeriodic() only. @@ -167,22 +167,22 @@ namespace std::string updateConfigReporting_; // Config parameters which are not individually settable. - double outputRangeMin0_ = std::get(SparkMaxFactory::configDefaults.at("kOutputMin_0")); - double outputRangeMax0_ = std::get(SparkMaxFactory::configDefaults.at("kOutputMax_0")); - double outputRangeMin1_ = std::get(SparkMaxFactory::configDefaults.at("kOutputMin_1")); - double outputRangeMax1_ = std::get(SparkMaxFactory::configDefaults.at("kOutputMax_1")); - uint followerID_ = std::get(SparkMaxFactory::configDefaults.at("kFollowerID")); - uint followerConfig_ = std::get(SparkMaxFactory::configDefaults.at("kFollowerConfig")); - double currentChop_ = std::get(SparkMaxFactory::configDefaults.at("kCurrentChop")); - uint currentChopCycles_ = std::get(SparkMaxFactory::configDefaults.at("kCurrentChopCycles")); - uint smartCurrentStallLimit_ = std::get(SparkMaxFactory::configDefaults.at("kSmartCurrentStallLimit")); - uint smartCurrentFreeLimit_ = std::get(SparkMaxFactory::configDefaults.at("kSmartCurrentFreeLimit")); - uint smartCurrentConfig_ = std::get(SparkMaxFactory::configDefaults.at("kSmartCurrentConfig")); + double outputRangeMin0_ = std::get(SparkFactory::configDefaults.at("kOutputMin_0")); + double outputRangeMax0_ = std::get(SparkFactory::configDefaults.at("kOutputMax_0")); + double outputRangeMin1_ = std::get(SparkFactory::configDefaults.at("kOutputMin_1")); + double outputRangeMax1_ = std::get(SparkFactory::configDefaults.at("kOutputMax_1")); + uint followerID_ = std::get(SparkFactory::configDefaults.at("kFollowerID")); + uint followerConfig_ = std::get(SparkFactory::configDefaults.at("kFollowerConfig")); + double currentChop_ = std::get(SparkFactory::configDefaults.at("kCurrentChop")); + uint currentChopCycles_ = std::get(SparkFactory::configDefaults.at("kCurrentChopCycles")); + uint smartCurrentStallLimit_ = std::get(SparkFactory::configDefaults.at("kSmartCurrentStallLimit")); + uint smartCurrentFreeLimit_ = std::get(SparkFactory::configDefaults.at("kSmartCurrentFreeLimit")); + uint smartCurrentConfig_ = std::get(SparkFactory::configDefaults.at("kSmartCurrentConfig")); // Config parameters which are needed before SetConfig()/AddConfig(). - uint status0_ = std::get(SparkMaxFactory::configDefaults.at("kStatus0")); - uint status1_ = std::get(SparkMaxFactory::configDefaults.at("kStatus1")); - uint status2_ = std::get(SparkMaxFactory::configDefaults.at("kStatus2")); + uint status0_ = std::get(SparkFactory::configDefaults.at("kStatus0")); + uint status1_ = std::get(SparkFactory::configDefaults.at("kStatus1")); + uint status2_ = std::get(SparkFactory::configDefaults.at("kStatus2")); // The next two data members are used to implement pacing of the config // state machines. @@ -305,7 +305,7 @@ namespace } } -void SparkMaxFactory::ConfigIndex() noexcept +void SparkFactory::ConfigIndex() noexcept { for (const auto &elem : configDefaults) { @@ -317,7 +317,7 @@ void SparkMaxFactory::ConfigIndex() noexcept } } -std::unique_ptr SparkMaxFactory::CreateSparkMax(const std::string_view name, const int canId, const bool inverted, const int encoderCounts) noexcept +std::unique_ptr SparkFactory::CreateSparkMax(const std::string_view name, const int canId, const bool inverted, const int encoderCounts) noexcept { return std::make_unique(name, canId, inverted, encoderCounts); } @@ -341,8 +341,8 @@ void SparkMax::SetConfig(const ConfigMap config) noexcept // Ensure at least these two config parameters are managed. config_.clear(); - config_["Firmware Version"] = std::get(SparkMaxFactory::configDefaults.at("Firmware Version")); - config_["kIdleMode"] = std::get(SparkMaxFactory::configDefaults.at("kIdleMode")); + config_["Firmware Version"] = std::get(SparkFactory::configDefaults.at("Firmware Version")); + config_["kIdleMode"] = std::get(SparkFactory::configDefaults.at("kIdleMode")); // The order matters here, hence the somewhat convoluted logic. tmp.merge(config_); @@ -691,7 +691,7 @@ void SparkMax::ConfigPeriodic() noexcept { DoSafely("motor_", [&]() -> void { - motor_ = std::make_unique(canId_, rev::CANSparkMaxLowLevel::MotorType::kBrushless); + motor_ = std::make_unique(canId_, rev::CANSparkLowLevel::MotorType::kBrushless); if (!motor_) { ++throws_; @@ -736,7 +736,10 @@ void SparkMax::ConfigPeriodic() noexcept { if (encoderCounts_ == 0) { - encoder_ = std::make_unique(motor_->GetEncoder()); + // FIXME: Once Rev updates the library to not include + // conflicting API definitions, the parameter to Get Encoder + // can be removed. + encoder_ = std::make_unique(motor_->GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor)); } else { @@ -756,7 +759,7 @@ void SparkMax::ConfigPeriodic() noexcept { DoSafely("controller_", [&]() -> void { - controller_ = std::make_unique(motor_->GetPIDController()); + controller_ = std::make_unique(motor_->GetPIDController()); if (!controller_) { ++throws_; @@ -838,17 +841,17 @@ void SparkMax::ConfigPeriodic() noexcept } else { - uint tmp = std::get(SparkMaxFactory::configDefaults.at("kLimitSwitchFwdPolarity")); + uint tmp = std::get(SparkFactory::configDefaults.at("kLimitSwitchFwdPolarity")); if (config_.count("kLimitSwitchFwdPolarity") != 0) { tmp = std::get(config_.at("kLimitSwitchFwdPolarity")); } - const rev::SparkMaxLimitSwitch::Type polarity = tmp == 0 ? rev::SparkMaxLimitSwitch::Type::kNormallyOpen : rev::SparkMaxLimitSwitch::Type::kNormallyClosed; + const rev::SparkLimitSwitch::Type polarity = tmp == 0 ? rev::SparkLimitSwitch::Type::kNormallyOpen : rev::SparkLimitSwitch::Type::kNormallyClosed; DoSafely("forward_", [&]() -> void { - forward_ = std::make_unique(motor_->GetForwardLimitSwitch(polarity)); + forward_ = std::make_unique(motor_->GetForwardLimitSwitch(polarity)); if (!forward_) { ++throws_; @@ -873,17 +876,17 @@ void SparkMax::ConfigPeriodic() noexcept } else { - uint tmp = std::get(SparkMaxFactory::configDefaults.at("kLimitSwitchRevPolarity")); + uint tmp = std::get(SparkFactory::configDefaults.at("kLimitSwitchRevPolarity")); if (config_.count("kLimitSwitchRevPolarity") != 0) { tmp = std::get(config_.at("kLimitSwitchRevPolarity")); } - const rev::SparkMaxLimitSwitch::Type polarity = tmp == 0 ? rev::SparkMaxLimitSwitch::Type::kNormallyOpen : rev::SparkMaxLimitSwitch::Type::kNormallyClosed; + const rev::SparkLimitSwitch::Type polarity = tmp == 0 ? rev::SparkLimitSwitch::Type::kNormallyOpen : rev::SparkLimitSwitch::Type::kNormallyClosed; DoSafely("reverse_", [&]() -> void { - reverse_ = std::make_unique(motor_->GetReverseLimitSwitch(polarity)); + reverse_ = std::make_unique(motor_->GetReverseLimitSwitch(polarity)); if (!reverse_) { ++throws_; @@ -1378,9 +1381,9 @@ void SparkMax::DoSafely(const char *const what, std::function work) noex // latter function. std::tuple SparkMax::VerifyConfig(const std::string_view key, const ConfigValue &value) noexcept { - const auto kv = SparkMaxFactory::configDefaults.find(std::string(key)); + const auto kv = SparkFactory::configDefaults.find(std::string(key)); - if (kv == SparkMaxFactory::configDefaults.end()) + if (kv == SparkFactory::configDefaults.end()) { return std::make_tuple(false, false, "Invalid"); } @@ -1393,7 +1396,7 @@ std::tuple SparkMax::VerifyConfig(const std::string_vie // In order to avoid switching on a string, obtain the index into defaults. // The complication here is that maps are sorted, so the indices are not in // an easy-to-maintain order. - const auto ndx = std::distance(kv, SparkMaxFactory::configDefaults.end()); + const auto ndx = std::distance(kv, SparkFactory::configDefaults.end()); // Some settings only apply to one encoder type or the other. const bool altMode = (encoderCounts_ != 0); @@ -1909,9 +1912,9 @@ std::tuple SparkMax::VerifyConfig(const std::string_vie // if there was some problem (or empty string, if not). std::string SparkMax::ApplyConfig(const std::string_view key, const ConfigValue &value) noexcept { - const auto kv = SparkMaxFactory::configDefaults.find(std::string(key)); + const auto kv = SparkFactory::configDefaults.find(std::string(key)); - if (kv == SparkMaxFactory::configDefaults.end()) + if (kv == SparkFactory::configDefaults.end()) { return std::string("Invalid"); } @@ -1919,7 +1922,7 @@ std::string SparkMax::ApplyConfig(const std::string_view key, const ConfigValue // In order to avoid switching on a string, obtain the index into defaults. // The complication here is that maps are sorted, so the indices are not in // an easy-to-maintain order. - const auto ndx = std::distance(kv, SparkMaxFactory::configDefaults.end()); + const auto ndx = std::distance(kv, SparkFactory::configDefaults.end()); // Some settings only apply to one encoder type or the other. const bool altMode = (encoderCounts_ != 0); diff --git a/src/main/cpp/infrastructure/SwerveModule.cpp b/src/main/cpp/infrastructure/SwerveModule.cpp index a50f283..030e817 100644 --- a/src/main/cpp/infrastructure/SwerveModule.cpp +++ b/src/main/cpp/infrastructure/SwerveModule.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -38,7 +39,7 @@ SwerveModule::SwerveModule( const int driveMotorCanID, const int turningMotorCanID, const int turningEncoderPort, - const int alignmentOffset) noexcept : m_name{name} + const units::turn_t alignmentOffset) noexcept : m_name{name} { // Set up onboard printf-style logging. std::string logName{"/SwerveModule/"}; @@ -55,7 +56,7 @@ SwerveModule::SwerveModule( pidf::kTurningPositionD, std::move(frc::TrapezoidProfile::Constraints{ pidf::kTurningPositionMaxVelocity, - pidf::kTurningPositionMaxAcceleration})); + pidf::kTurningPositionMaxAcceleration}), 100_ms); m_rioPIDController->EnableContinuousInput(-180.0_deg, +180.0_deg); @@ -65,12 +66,17 @@ SwerveModule::SwerveModule( // and it's incremental encoder in order to make things work out/line up. m_turningPositionPWM = std::make_unique(turningEncoderPort, alignmentOffset); + // test printout for alignment calibration + if (m_turningPositionPWM->GetAbsolutePosition().has_value()) { + frc::SmartDashboard::PutNumber("Initial Alignment " + std::string(name), m_turningPositionPWM->GetAbsolutePosition().value().value()); + } + // Motor controller configurations are only checked (or saved) in test mode // but a minimal amount is set up in these methods. - m_turningMotorBase = SparkMaxFactory::CreateSparkMax(m_name + std::string(" Turning"), turningMotorCanID, m_turningMotorInverted); + m_turningMotorBase = SparkFactory::CreateSparkMax(m_name + std::string(" Turning"), turningMotorCanID, m_turningMotorInverted); m_turningMotor = std::make_unique>(*m_turningMotorBase); - m_driveMotorBase = SparkMaxFactory::CreateSparkMax(m_name + std::string(" Drive"), driveMotorCanID, m_driveMotorInverted); + m_driveMotorBase = SparkFactory::CreateSparkMax(m_name + std::string(" Drive"), driveMotorCanID, m_driveMotorInverted); m_driveMotor = std::make_unique>(*m_driveMotorBase); // kStatus1 includes velocity; kStatus2 includes position -- these are made @@ -82,6 +88,7 @@ SwerveModule::SwerveModule( {"kStatus2", uint{250}}, // ms {"kPositionConversionFactor", double{360.0}}, {"kVelocityConversionFactor", double{360.0 / 60.0}}, + {"kSmartCurrentStallLimit", uint{40}}, // Amps }); m_turningMotor->ApplyConfig(false); @@ -91,6 +98,7 @@ SwerveModule::SwerveModule( {"kStatus2", uint{250}}, // ms {"kPositionConversionFactor", double{physical::kDriveMetersPerRotation}}, {"kVelocityConversionFactor", double{physical::kDriveMetersPerRotation / 60.0}}, + {"kSmartCurrentStallLimit", uint{40}}, // Amps }); m_driveMotor->ApplyConfig(false); @@ -208,6 +216,14 @@ void SwerveModule::Periodic() noexcept return; } + // If no drive speed is applied, don't bother turning + if (m_turningStopped) + { + m_turningMotor->Set(0); + m_turningStopped = false; + return; + } + // Update (and apply below) turning position PID. double calculated = m_rioPIDController->Calculate(m_turningPosition); @@ -225,6 +241,8 @@ void SwerveModule::Periodic() noexcept calculated -= m_rioPID_F; } + frc::SmartDashboard::PutNumber(std::string(m_name) + "_TurningOutput", calculated); + // Use voltage compensation, to offset low battery voltage. m_turningMotor->SetVoltage(calculated * 12.0_V); } @@ -306,6 +324,7 @@ void SwerveModule::SetTurningPosition(const units::angle::degree_t position) noe m_commandedHeading = adjustedPosition; m_rioPIDController->SetGoal(adjustedPosition); + frc::SmartDashboard::PutNumber(m_name + " Turning PID Setpoint Angle: ", adjustedPosition.value()); if (m_rio || m_testModeControl || m_testModeTurningVoltage != 0.0) { @@ -331,6 +350,11 @@ bool SwerveModule::CheckTurningPosition(const units::angle::degree_t tolerance) return error >= -tolerance && error < tolerance; } +void SwerveModule::StopTurning() noexcept +{ + m_turningStopped = true; +} + // Drive position and velocity are in rotations and rotations/second, // respectively. Brake/coast also applies here; brake only in distance mode, // once distance has been reached or in velocity mode, if commanded velocity is @@ -403,6 +427,8 @@ void SwerveModule::SetDriveVelocity(units::velocity::meters_per_second_t velocit const units::angle::degree_t angleError = m_turningPosition - m_commandedHeading; const double vectorAlignment = std::cos(units::angle::radian_t{angleError}.to()); + frc::SmartDashboard::PutNumber(std::string(m_name) + "_VectorAlignment", vectorAlignment); + #if 0 m_driveMotor->SeekVelocity(velocity * vectorAlignment); #else @@ -441,6 +467,7 @@ void SwerveModule::SetDesiredState(const frc::SwerveModuleState &referenceState) { m_turningPosition = position.value(); state = frc::SwerveModuleState::Optimize(referenceState, frc::Rotation2d(m_turningPosition)); + frc::SmartDashboard::PutNumber(std::string(m_name) + " Angle Optimization Delta", referenceState.angle.Degrees().value() - state.angle.Degrees().value()); } SetTurningPosition(state.angle.Degrees()); @@ -570,8 +597,8 @@ void SwerveModule::TestPeriodic() noexcept m_driveMotor->CheckConfig(); } - // [-2048, +2048) - std::optional position = m_turningPositionPWM->GetAbsolutePositionWithoutAlignment(); + // [-180, +180) + std::optional position = m_turningPositionPWM->GetAbsolutePositionWithoutAlignment(); // This provides a rough means of zeroing the turning position. if (position.has_value()) @@ -579,29 +606,29 @@ void SwerveModule::TestPeriodic() noexcept if (zeroTurning) { // Work out new alignment so position becomes zero. - int alignmentOffset = -position.value(); - if (alignmentOffset == +2048) + units::turn_t alignmentOffset = -position.value(); + if (alignmentOffset == 180_deg) { - alignmentOffset = -2048; + alignmentOffset = -180_deg; } m_turningPositionPWM->SetAlignment(alignmentOffset); - position = 0; + position = 0.0_deg; m_turningPosition = 0.0_deg; } else { // Alignment is [-2048, +2048). position = position.value() + m_turningPositionPWM->GetAlignment(); - if (position > 2047) + if (position > 180_deg) { - position = position.value() - 4096; + position = position.value() - 360_deg; } - if (position < -2048) + if (position < -180_deg) { - position = position.value() + 4096; + position = position.value() + 360_deg; } - m_turningPosition = position.value() / 2048.0 * 180.0_deg; + m_turningPosition = position.value(); } } @@ -771,8 +798,8 @@ void SwerveModule::TurningPositionPID(double P, double I, double IZ, double IM, m_turningPosition_D = D; m_turningPosition_DF = DF; m_turningPosition_F = F; - m_turningPosition_V = V; - m_turningPosition_A = A; + m_turningPosition_V = units::degrees_per_second_t{V}; + m_turningPosition_A = units::degrees_per_second_squared_t{A}; SetTurningPositionPID(); } diff --git a/src/main/cpp/subsystems/ClimberSubsystem.cpp b/src/main/cpp/subsystems/ClimberSubsystem.cpp new file mode 100644 index 0000000..d46a4e4 --- /dev/null +++ b/src/main/cpp/subsystems/ClimberSubsystem.cpp @@ -0,0 +1,20 @@ +// 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. + +#include "subsystems/ClimberSubsystem.h" + +ClimberSubsystem::ClimberSubsystem() noexcept +{ + m_ClimberMotor.SetInverted(climber::kClimberMotorIsInverted); +} + +void ClimberSubsystem::StopClimber() noexcept +{ + m_ClimberMotor.StopMotor(); +} + +void ClimberSubsystem::SetClimberMotorVoltagePercent(const double percent) noexcept +{ + m_ClimberMotor.SetVoltage(percent * 12.00_V); +} diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index f2c393b..ab32fb0 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -10,13 +10,14 @@ #include "infrastructure/SwerveModule.h" #include +#include #include #include #include #include #include #include -#include +#include #include #include #include @@ -162,7 +163,7 @@ void DriveSubsystem::Periodic() noexcept { if (m_ahrs) { - botRot = -m_ahrs->GetRotation2d(); + botRot = m_ahrs->GetRotation2d(); } else { @@ -222,7 +223,7 @@ void DriveSubsystem::ResetOdometry(frc::Pose2d pose) noexcept { if (m_ahrs) { - botRot = -m_ahrs->GetRotation2d(); + botRot = m_ahrs->GetRotation2d(); } }); m_odometry->ResetPosition(botRot, GetModulePositions(), pose); @@ -991,14 +992,14 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, m_rotation = rot / physical::kMaxTurnRate; m_x = xSpeed / physical::kMaxDriveSpeed; m_y = ySpeed / physical::kMaxDriveSpeed; - frc::Rotation2d botRot; DoSafeIMU("GetRotation2d()", [&]() -> void { if (m_ahrs) { - botRot = -m_ahrs->GetRotation2d(); + botRot = m_ahrs->GetRotation2d(); + frc::SmartDashboard::PutNumber("BotRot", botRot.Degrees().value()); } }); if (!m_ahrs) @@ -1006,6 +1007,8 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, fieldRelative = false; } + frc::SmartDashboard::PutBoolean("Field Relative", fieldRelative); + // Center of rotation argument is defaulted to the center of the robot above, // but it is also possible to rotate about a different point. wpi::array states = kDriveKinematics.ToSwerveModuleStates( @@ -1029,6 +1032,7 @@ void DriveSubsystem::ResetEncoders() noexcept void DriveSubsystem::SetModuleStates(std::array &desiredStates) noexcept { + auto [frontLeft, frontRight, rearLeft, rearRight] = desiredStates; m_commandedStateFrontLeft = frontLeft; @@ -1036,6 +1040,16 @@ void DriveSubsystem::SetModuleStates(std::array &desi m_commandedStateRearLeft = rearLeft; m_commandedStateRearRight = rearRight; + frc::SmartDashboard::PutNumber("LeftFront Desired Speed", frontLeft.speed.value()); + frc::SmartDashboard::PutNumber("RightFront Desired Speed", frontRight.speed.value()); + frc::SmartDashboard::PutNumber("LeftRear Desired Speed", rearLeft.speed.value()); + frc::SmartDashboard::PutNumber("RightRear Desired Speed", rearRight.speed.value()); + + frc::SmartDashboard::PutNumber("LeftFront Desired Angle", frontLeft.angle.Degrees().value()); + frc::SmartDashboard::PutNumber("RightFront Desired Angle", frontRight.angle.Degrees().value()); + frc::SmartDashboard::PutNumber("LeftRear Desired Angle", rearLeft.angle.Degrees().value()); + frc::SmartDashboard::PutNumber("RightRear Desired Angle", rearRight.angle.Degrees().value()); + // Don't command turning if there is no drive; this is used from Drive(), and // it winds up causing the modules to all home to zero any time there is no // joystick input. This check causes them to stay where they are, which is @@ -1046,14 +1060,21 @@ void DriveSubsystem::SetModuleStates(std::array &desi rearLeft.speed == 0.0_mps && rearRight.speed == 0.0_mps) { + frc::SmartDashboard::PutBoolean("Stopped", true); m_frontLeftSwerveModule->SetDriveVelocity(0.0_mps); m_frontRightSwerveModule->SetDriveVelocity(0.0_mps); m_rearLeftSwerveModule->SetDriveVelocity(0.0_mps); m_rearRightSwerveModule->SetDriveVelocity(0.0_mps); + m_frontLeftSwerveModule->StopTurning(); + m_frontRightSwerveModule->StopTurning(); + m_rearLeftSwerveModule->StopTurning(); + m_rearRightSwerveModule->StopTurning(); return; } + frc::SmartDashboard::PutBoolean("Stopped", false); + // m_limit is always unity, except in Test Mode. So, by default, it does not // modify anything here. In Test Mode, it can be used to slow things down. frontLeft.speed *= m_limit; @@ -1093,7 +1114,7 @@ units::degree_t DriveSubsystem::GetHeading() noexcept { if (m_ahrs) { - heading = units::degree_t{-m_ahrs->GetAngle()}; // In degrees already. + heading = units::degree_t{m_ahrs->GetAngle()}; // In degrees already. } }); return heading; @@ -1117,7 +1138,7 @@ double DriveSubsystem::GetTurnRate() noexcept { if (m_ahrs) { - rate = -m_ahrs->GetRate(); // In degrees/second (units not used in WPILib). + rate = m_ahrs->GetRate(); // In degrees/second (units not used in WPILib). } }); return rate; diff --git a/src/main/cpp/subsystems/FeederSubsystem.cpp b/src/main/cpp/subsystems/FeederSubsystem.cpp deleted file mode 100644 index 442c278..0000000 --- a/src/main/cpp/subsystems/FeederSubsystem.cpp +++ /dev/null @@ -1,192 +0,0 @@ -#include "subsystems/FeederSubsystem.h" - -#include "Constants.h" - -#include - -FeederSubsystem::FeederSubsystem() noexcept -{ - // NEO 550. - const SmartMotorBase::ConfigMap config = { - {"kStatus1", uint{250}}, - {"kStatus2", uint{250}}, - {"kIdleMode", uint{0}}, - {"kRampRate", double{0.1}}, - {"kSmartCurrentStallLimit", uint{25}}, // Amps - {"kSmartCurrentFreeLimit", uint{10}}, // Amps - }; - - const SmartMotorBase::ConfigMap climberConfig = { - {"kStatus1", uint{250}}, - {"kStatus2", uint{250}}, - {"kRampRate", double{0.5}}, - {"kSmartCurrentStallLimit", uint{40}}, // Amps - // {"kSoftLimitFwd", double{0.0}}, - // {"kSoftLimitRev", double{0.0}}, - }; - - const SmartMotorBase::ConfigMap moreTorque = { - {"kSmartCurrentStallLimit", uint{30}}, // Amps - }; - - m_intakeMotorBase = SparkMaxFactory::CreateSparkMax("Intake", 9, false); - m_elevatorMotorBase = SparkMaxFactory::CreateSparkMax("Elevator", 10, true); - m_feederMotorBase = SparkMaxFactory::CreateSparkMax("Feeder", 11, true); - m_climberMotorBase = SparkMaxFactory::CreateSparkMax("Climber", 12, false); - m_intakeMotor = std::make_unique>(*m_intakeMotorBase); - m_elevatorMotor = std::make_unique>(*m_elevatorMotorBase); - m_feederMotor = std::make_unique>(*m_feederMotorBase); - m_climberMotor = std::make_unique>(*m_climberMotorBase); - - m_intakeRaise = std::make_unique(1, frc::PneumaticsModuleType::REVPH, 0, 1); - m_intakeRelease = std::make_unique(1, frc::PneumaticsModuleType::REVPH, 13, 12); - - m_intakeMotor->SetConfig(config); - m_elevatorMotor->SetConfig(config); - m_feederMotor->SetConfig(config); - m_climberMotor->SetConfig(climberConfig); - m_elevatorMotor->AddConfig(moreTorque); - - m_intakeMotor->ApplyConfig(false); - m_elevatorMotor->ApplyConfig(false); - m_feederMotor->ApplyConfig(false); - m_climberMotor->ApplyConfig(false); - - Default(0.0); -} - -void FeederSubsystem::Periodic() noexcept -{ - m_intakeMotor->Periodic(); - m_elevatorMotor->Periodic(); - m_feederMotor->Periodic(); - m_climberMotor->Periodic(); -} - -bool FeederSubsystem::GetStatus() const noexcept -{ - return m_intakeMotor->GetStatus() || - m_elevatorMotor->GetStatus() || - m_feederMotor->GetStatus() || - m_climberMotor->GetStatus(); -} - -void FeederSubsystem::TestInit() noexcept {} -void FeederSubsystem::TestExit() noexcept {} - -void FeederSubsystem::TestPeriodic() noexcept -{ - const std::chrono::time_point now = std::chrono::steady_clock::now(); - if (now >= m_verifyMotorControllersWhen) - { - using namespace std::chrono_literals; - - m_verifyMotorControllersWhen = now + 15s; - - m_intakeMotor->CheckConfig(); - m_elevatorMotor->CheckConfig(); - m_feederMotor->CheckConfig(); - m_climberMotor->CheckConfig(); - } -} - -void FeederSubsystem::Default(const double percent) noexcept -{ - m_intakeMotor->SetVoltage(percent * 12_V); - m_elevatorMotor->SetVoltage(percent * 12_V); - - m_climberMotor->Stop(); - - m_intakeRelease->Set(frc::DoubleSolenoid::Value::kOff); - m_intakeRaise->Set(frc::DoubleSolenoid::Value::kOff); -} - -void FeederSubsystem::NoFeed() noexcept -{ - m_feederMotor->Stop(); -} - -void FeederSubsystem::Eject() noexcept -{ - m_intakeMotor->SetVoltage(-12.0_V); - m_elevatorMotor->SetVoltage(-12.0_V); - m_feederMotor->SetVoltage(-12.0_V); - - m_climberMotor->Stop(); - - m_intakeRelease->Set(frc::DoubleSolenoid::Value::kOff); - m_intakeRaise->Set(frc::DoubleSolenoid::Value::kOff); -} - -void FeederSubsystem::Fire() noexcept -{ - m_intakeMotor->Stop(); - m_elevatorMotor->Stop(); - - m_feederMotor->SetVoltage(12_V); - - m_climberMotor->Stop(); - - m_intakeRelease->Set(frc::DoubleSolenoid::Value::kOff); - m_intakeRaise->Set(frc::DoubleSolenoid::Value::kOff); -} - -void FeederSubsystem::Raise() noexcept -{ - m_intakeMotor->Stop(); - m_elevatorMotor->Stop(); - m_feederMotor->Stop(); - - m_climberMotor->SetVoltage(-10.0_V); - - m_intakeRelease->Set(frc::DoubleSolenoid::Value::kOff); - m_intakeRaise->Set(frc::DoubleSolenoid::Value::kOff); -} - -void FeederSubsystem::Lower() noexcept -{ - m_intakeMotor->Stop(); - m_elevatorMotor->Stop(); - m_feederMotor->Stop(); - - m_climberMotor->SetVoltage(10.0_V); - - m_intakeRelease->Set(frc::DoubleSolenoid::Value::kOff); - m_intakeRaise->Set(frc::DoubleSolenoid::Value::kOff); -} - -void FeederSubsystem::LockIntake() noexcept -{ - m_intakeRelease->Set(frc::DoubleSolenoid::Value::kForward); -} - -void FeederSubsystem::DropIntake() noexcept -{ - m_intakeRelease->Set(frc::DoubleSolenoid::Value::kReverse); -} - -void FeederSubsystem::RaiseIntake() noexcept -{ - m_intakeRaise->Set(frc::DoubleSolenoid::Value::kForward); -} - -void FeederSubsystem::LowerIntake() noexcept -{ - m_intakeRaise->Set(frc::DoubleSolenoid::Value::kReverse); -} - -void FeederSubsystem::BurnConfig() noexcept -{ - m_intakeMotor->ApplyConfig(true); - m_elevatorMotor->ApplyConfig(true); - m_feederMotor->ApplyConfig(true); - m_climberMotor->ApplyConfig(true); -} - -void FeederSubsystem::ClearFaults() noexcept -{ - m_intakeMotor->ClearFaults(); - m_elevatorMotor->ClearFaults(); - m_feederMotor->ClearFaults(); - m_climberMotor->ClearFaults(); -} diff --git a/src/main/cpp/subsystems/IntakeSubsystem.cpp b/src/main/cpp/subsystems/IntakeSubsystem.cpp new file mode 100644 index 0000000..6a35853 --- /dev/null +++ b/src/main/cpp/subsystems/IntakeSubsystem.cpp @@ -0,0 +1,18 @@ +#include "subsystems/IntakeSubsystem.h" +#include + +IntakeSubsystem::IntakeSubsystem() noexcept +{ + m_IntakeMotor.SetInverted(intake::kIntakeSpinMotorIsInverted); + StopIntake(); +} + +void IntakeSubsystem::StopIntake() noexcept +{ + m_IntakeMotor.StopMotor(); +} + +void IntakeSubsystem::SetSpinMotorVoltagePercent(const double percent) noexcept +{ + m_IntakeMotor.SetVoltage(percent * 12_V); +} diff --git a/src/main/cpp/subsystems/ShooterSubsystem.cpp b/src/main/cpp/subsystems/ShooterSubsystem.cpp index 9dbc58b..a0db7da 100644 --- a/src/main/cpp/subsystems/ShooterSubsystem.cpp +++ b/src/main/cpp/subsystems/ShooterSubsystem.cpp @@ -1,117 +1,21 @@ #include "subsystems/ShooterSubsystem.h" - -#include "Constants.h" - #include ShooterSubsystem::ShooterSubsystem() noexcept { - // These motors use bang-bang control, thus an increased velocity reporting - // rate (kStatus1). - const SmartMotorBase::ConfigMap config = { - {"kStatus1", uint{250}}, - {"kStatus2", uint{250}}, - {"kIdleMode", uint{0}}, - }; - - m_shooterMotorBase = SparkMaxFactory::CreateSparkMax("Shooter", 13, false); - m_backspinMotorBase = SparkMaxFactory::CreateSparkMax("Backspin", 14, true); - m_shooterMotor = std::make_unique>(*m_shooterMotorBase); - m_backspinMotor = std::make_unique>(*m_backspinMotorBase); - - m_shooterMotor->SetConfig(config); - m_backspinMotor->SetConfig(config); - - m_shooterMotor->ApplyConfig(false); - m_backspinMotor->ApplyConfig(false); -} - -void ShooterSubsystem::Periodic() noexcept -{ - m_shooterMotor->Periodic(); - m_backspinMotor->Periodic(); -} - -bool ShooterSubsystem::GetStatus() const noexcept -{ - return m_shooterMotor->GetStatus() || - m_backspinMotor->GetStatus(); -} - -void ShooterSubsystem::TestInit() noexcept {} -void ShooterSubsystem::TestExit() noexcept {} -void ShooterSubsystem::TestPeriodic() noexcept -{ - const std::chrono::time_point now = std::chrono::steady_clock::now(); - if (now >= m_verifyMotorControllersWhen) - { - using namespace std::chrono_literals; - - m_verifyMotorControllersWhen = now + 15s; - - m_shooterMotor->CheckConfig(); - m_backspinMotor->CheckConfig(); - } -} - -void ShooterSubsystem::Default(const double percent, const double velocity) noexcept -{ - // Manual control (backup). - if (velocity == 0.0) - { - m_shooterMotor->SetVoltage(percent * 12.00_V); - m_backspinMotor->SetVoltage(percent * 11.25_V); - - return; - } - - // On/off trigger control (off here). - if (percent < 0.25) - { - m_shooterMotor->Stop(); - m_backspinMotor->Stop(); - - return; - } - -// Unfortunately, SPARK MAX has too much velocity measurement latency for this. -#if 0 - // RPM. - const double shooterVelocity = m_shooterMotor->GetVelocityRaw(); - // const double backspinVelocity = m_backspinMotor->GetVelocityRaw(); - - // Bang-bang velocity control (simple). - if (shooterVelocity < velocity) - { - m_shooterMotor->Set(1.0); - } - else - { - m_shooterMotor->Stop(); - } - -#else - - m_shooterMotor->SetVoltage(velocity / 1500.0 * 12.00_V); - m_backspinMotor->SetVoltage(velocity / 1500.0 * 11.25_V); - -#endif -} - -void ShooterSubsystem::Stop() noexcept -{ - m_shooterMotor->Stop(); - m_backspinMotor->Stop(); + m_LeftShooterMotor.SetInverted(shooter::kLeftShooterMotorIsInverted); + m_RightShooterMotor.SetInverted(shooter::kRightShooterMotorIsInverted); + StopShooter(); } -void ShooterSubsystem::BurnConfig() noexcept +void ShooterSubsystem::StopShooter() noexcept { - m_shooterMotor->ApplyConfig(true); - m_backspinMotor->ApplyConfig(true); + m_LeftShooterMotor.StopMotor(); + m_RightShooterMotor.StopMotor(); } -void ShooterSubsystem::ClearFaults() noexcept +void ShooterSubsystem::SetShooterMotorVoltagePercent(const double percent) noexcept { - m_shooterMotor->ClearFaults(); - m_backspinMotor->ClearFaults(); + m_LeftShooterMotor.SetVoltage(percent * 12.00_V); + m_RightShooterMotor.SetVoltage(percent * 12.00_V); } diff --git a/src/main/cpp/subsystems/TransferArmSubsystem.cpp b/src/main/cpp/subsystems/TransferArmSubsystem.cpp new file mode 100644 index 0000000..5cb3cd8 --- /dev/null +++ b/src/main/cpp/subsystems/TransferArmSubsystem.cpp @@ -0,0 +1,62 @@ +#include "subsystems/TransferArmSubsystem.h" +#include +#include + +TransferArmSubsystem::TransferArmSubsystem() noexcept +{ + m_TransferArmMotor.SetInverted(arm::kTransferArmMotorIsInverted); + + m_encoder.SetMeasurementPeriod(10); + + // set PID coefficients + // m_pidController.SetP(kP); + // m_pidController.SetI(kI); + // m_pidController.SetD(kD); + // m_pidController.SetIZone(kIz); + // m_pidController.SetFF(kFF); + // m_pidController.SetOutputRange(kMinOutput, kMaxOutput); + + // display PID coefficients on SmartDashboard + // frc::SmartDashboard::PutNumber("Arm P Gain ", kP); + // frc::SmartDashboard::PutNumber("Arm D Gain ", kD); + // frc::SmartDashboard::PutNumber("Arm F ", kFF); + + frc::SmartDashboard::PutNumber("Arm Init Position ", m_encoder.GetPosition()); + + StopTransferArm(); +} + +void TransferArmSubsystem::StopTransferArm() noexcept +{ + m_TransferArmMotor.StopMotor(); +} + +void TransferArmSubsystem::SetArmMotorVoltagePercent(const double percent) noexcept +{ + m_TransferArmMotor.SetVoltage(percent * 12_V); +} + +// void TransferArmSubsystem::SetTransferArmPosition(const units::turn_t position) noexcept +// { +// m_pidController.SetReference(position.value(), rev::CANSparkMax::ControlType::kPosition); +// frc::SmartDashboard::PutNumber("SetPoint", position.value()); +// frc::SmartDashboard::PutNumber("ProcessVariable", m_encoder.GetPosition()); +// } + +units::turn_t TransferArmSubsystem::GetTransferArmPosition() noexcept +{ + frc::SmartDashboard::PutNumber("Arm Position ", m_encoder.GetPosition()); + return units::turn_t(m_encoder.GetPosition()); +} + +// void TransferArmSubsystem::UpdatePIDValues() noexcept +// { +// double p = frc::SmartDashboard::GetNumber("Arm P Gain ", 0); +// double d = frc::SmartDashboard::GetNumber("Arm D Gain ", 0); +// double ff = frc::SmartDashboard::GetNumber("Arm F ", 0); + +// // if PID coefficients on SmartDashboard have changed, write new values to controller +// if((p != kP)) { m_pidController.SetP(p); kP = p; } +// if((d != kD)) { m_pidController.SetD(d); kD = d; } +// if((ff != kFF)) { m_pidController.SetFF(ff); kFF = ff; } +// } diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 551fa80..0b19191 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -8,12 +8,12 @@ namespace physical { - // Alignment constants, for each swerve module. Specified on [-2048, 2048) - // "count" scale, in (dimensionless) angular units. - constexpr int kFrontLeftAlignmentOffset = +2064; - constexpr int kFrontRightAlignmentOffset = +445; - constexpr int kRearLeftAlignmentOffset = -16; - constexpr int kRearRightAlignmentOffset = -201; + // Alignment constants, for each swerve module. Specified on [-180, 180) + // "count" scale, in deg. From driver station view: +clockwise -counterclockwise. + constexpr units::degree_t kFrontLeftAlignmentOffset = -92_deg; + constexpr units::degree_t kFrontRightAlignmentOffset = 93_deg; + constexpr units::degree_t kRearLeftAlignmentOffset = 26_deg; + constexpr units::degree_t kRearRightAlignmentOffset = 131_deg; // SDS Mk3 Standard (or Fast) Gear Ratio: 8.16:1 (or 6.86:1); // Nominal Wheel Diameter (4"): =0.1016m; @@ -59,48 +59,48 @@ namespace physical // Drivebase geometry: distance between centers of right and left wheels on // robot; distance between centers of front and back wheels on robot. - constexpr units::meter_t kTrackWidth = 22.5_in; - constexpr units::meter_t kWheelBase = 22.5_in; + constexpr units::meter_t kTrackWidth = 24.5_in; + constexpr units::meter_t kWheelBase = 24.5_in; // CAN ID and Digital I/O Port assignments. - constexpr int kFrontLeftTurningMotorCanID = 1; - constexpr int kFrontLeftDriveMotorCanID = 2; - constexpr int kFrontRightTurningMotorCanID = 3; - constexpr int kFrontRightDriveMotorCanID = 4; - constexpr int kRearLeftTurningMotorCanID = 5; - constexpr int kRearLeftDriveMotorCanID = 6; - constexpr int kRearRightTurningMotorCanID = 7; - constexpr int kRearRightDriveMotorCanID = 8; - constexpr int kFrontLeftTurningEncoderPort = 0; - constexpr int kFrontRightTurningEncoderPort = 1; - constexpr int kRearLeftTurningEncoderPort = 2; - constexpr int kRearRightTurningEncoderPort = 3; + constexpr int kFrontRightDriveMotorCanID = 1; + constexpr int kFrontRightTurningMotorCanID = 2; + constexpr int kRearRightDriveMotorCanID = 3; + constexpr int kRearRightTurningMotorCanID = 4; + constexpr int kRearLeftDriveMotorCanID = 5; + constexpr int kRearLeftTurningMotorCanID = 6; + constexpr int kFrontLeftDriveMotorCanID = 7; + constexpr int kFrontLeftTurningMotorCanID = 8; + constexpr int kFrontRightTurningEncoderPort = 10; + constexpr int kRearRightTurningEncoderPort = 11; + constexpr int kRearLeftTurningEncoderPort = 12; + constexpr int kFrontLeftTurningEncoderPort = 13; // These can flip because of gearing. constexpr bool kDriveMotorInverted = false; - constexpr bool kTurningMotorInverted = false; + constexpr bool kTurningMotorInverted = true; } namespace firmware { - constexpr int kSparkMaxFirmwareVersion = 0x01060003; // Version 1.6.3. + constexpr int kSparkFirmwareVersion = 0x01060003; // Version 1.6.3. } namespace pidf { constexpr units::degrees_per_second_t kTurningPositionMaxVelocity = 2750.0_deg_per_s; constexpr units::degrees_per_second_squared_t kTurningPositionMaxAcceleration = 20000.0_deg_per_s_sq; - constexpr double kTurningPositionP = 0.006; - constexpr double kTurningPositionF = 0.003; + constexpr double kTurningPositionP = 0.00395; // originally 0.006 concrete 0.00395 + constexpr double kTurningPositionF = 0.0; // originally 0.003 concrete 0.003 constexpr double kTurningPositionI = 0.0; constexpr double kTurningPositionIZ = 0.0; constexpr double kTurningPositionIM = 0.0; - constexpr double kTurningPositionD = 0.0; + constexpr double kTurningPositionD = 0.000395; constexpr double kTurningPositionDF = 0.0; constexpr double kDrivePositionMaxVelocity = 5700.0; // Rotations per minute. constexpr double kDrivePositionMaxAcceleration = 1000.0; // Rotations per minute per second. - constexpr double kDrivePositionP = 0.004; + constexpr double kDrivePositionP = 0.000; constexpr double kDrivePositionF = 0.0; constexpr double kDrivePositionI = 0.0; constexpr double kDrivePositionIZ = 0.0; @@ -127,10 +127,73 @@ namespace pidf constexpr double kDriveThetaD = 0.0; } -namespace nonDrive +namespace shooter { - constexpr int kFeederOneCanID = 11; - constexpr int kFeederTwoCanID = 12; - constexpr int kShooterOneCanID = 9; - constexpr int kShooterTwoCanID = 10; + // Left Motor Parameters + constexpr int kLeftShooterMotorCanID = 14; + constexpr bool kLeftShooterMotorIsInverted = true; + + // Right Motor Parameters + constexpr int kRightShooterMotorCanID = 15; + constexpr bool kRightShooterMotorIsInverted = true; + + constexpr int kShooterMotorVoltagePercent = .80; +} + +namespace intake +{ + // Intake Motor Parameters + constexpr int kIntakeSpinMotorCanID = 16; + constexpr bool kIntakeSpinMotorIsInverted = true; + + /* NOTE!!!: The intake arm values are NOT final, are subject to change + after testing*/ + constexpr units::degree_t kIntakeArmHome = 90.0_deg; + constexpr units::degree_t kIntakeArmPickup = 180.0_deg; + constexpr units::degree_t kIntakeArmLoad = 0.0_deg; + + constexpr double kIntakeSpinMotorVoltagePercent = .80; + constexpr double kIntakeSpinMotorEjectVoltagePercent = -.90; +} + +namespace arm +{ + // Arm Motor Parameters + constexpr int kTransferArmMotorCanID = 17; + constexpr bool kTransferArmMotorIsInverted = true; + + // Arm Controller + constexpr double kArmPositionP = 10.0; + constexpr double kArmPositionD = .10; + constexpr double kArmPositionF = 0.0; + + constexpr units::turn_t kShooterToAmpAngle = -50_deg; + constexpr units::turn_t kShooterToIntakeAngle = -200_deg; +} + +namespace climber +{ + //Climber parameters + constexpr int kClimberMotorCanID = 18; + constexpr bool kClimberMotorIsInverted = false; + constexpr double kClimberMotorRaiseVoltagePercent = .15; + constexpr units::second_t kClimberRaiseTimer = 2_s; + constexpr double kClimberMotorLowerVoltagePercent = -.15; + constexpr units::second_t kClimberLowerTimer = 2_s; + + constexpr int kClimberSolenoidCanID = 19; + +} + +namespace amp +{ + //Bosch motor for extending out and back + constexpr int kAmpExtendMotorCanID = 20; + constexpr bool kAmpExtendMotorIsInverted = false; + constexpr double kAmpExtendMotorVoltagePercent = .15; + + //Johnson Electric for raising and lowering + constexpr int kAmpRaiseMotorCanID = 21; + constexpr bool kAmpRaiseMotorIsInverted = false; + constexpr double kAmpRaiseMotorVoltagePercent = .15; } diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 929325c..49d40fb 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -9,11 +9,13 @@ #include #include "commands/AutonomousCommands.h" -#include "commands/TestModeCommands.h" #include "subsystems/DriveSubsystem.h" -#include "subsystems/FeederSubsystem.h" +#include "subsystems/IntakeSubsystem.h" +#include "subsystems/TransferArmSubsystem.h" #include "subsystems/Infrastructure.h" +#include "subsystems/ClimberSubsystem.h" #include "subsystems/ShooterSubsystem.h" +#include #include #include @@ -33,38 +35,64 @@ class RobotContainer RobotContainer(const RobotContainer &) = delete; RobotContainer &operator=(const RobotContainer &) = delete; +#pragma region Autonomous +public: + void AutonomousInit() noexcept; + void AutonomousPeriodic() noexcept; + void AutonomousExit() noexcept; std::optional GetAutonomousCommand() noexcept; +#pragma endregion - void TestInit() noexcept; - void TestExit() noexcept; - void TestPeriodic() noexcept; - - void DisabledInit() noexcept; - void DisabledExit() noexcept; - void AutonomousInit() noexcept; +#pragma region Teleop +public: void TeleopInit() noexcept; + void TeleopPeriodic() noexcept; + void TeleopExit() noexcept; private: static frc2::CommandPtr DriveCommandFactory(RobotContainer *container) noexcept; - static frc2::CommandPtr PointCommandFactory(RobotContainer *container) noexcept; - std::tuple GetDriveTeleopControls() noexcept; - + double ConditionRawTriggerInput(double RawTrigVal) noexcept; + double ConditionRawJoystickInput(double RawJoystickVal, double mixer = 0.75) noexcept; void ConfigureBindings() noexcept; - bool m_fieldOriented{false}; + bool m_fieldOriented{true}; bool m_lock{false}; - bool m_slow{false}; - double m_shooterVelocity{0.0}; - uint m_LEDPattern{29}; - uint m_LEDPatternCount{0}; + bool triggerSpeedEnabled{false}; + + frc2::CommandXboxController m_xboxDrive{0}; + frc2::CommandXboxController m_xboxOperate{1}; + // frc2::CommandXboxController m_xbox{1}; TODO: update for second controller +#pragma endregion - // The robot's subsystems and commands are defined here... +#pragma region Test +public: + void TestInit() noexcept; + void TestPeriodic() noexcept; + void TestExit() noexcept; +private: + static frc2::CommandPtr PointCommandFactory(RobotContainer *container) noexcept; +#pragma endregion + +#pragma region Disabled +public: + void DisabledInit() noexcept; + void DisabledPeriodic() noexcept; + void DisabledExit() noexcept; +#pragma endregion + +#pragma region Subsystem Declare +private: + // The robot's subsystems and commands are declared here DriveSubsystem m_driveSubsystem; - FeederSubsystem m_feederSubsystem; InfrastructureSubsystem m_infrastructureSubsystem; + IntakeSubsystem m_intakeSubsystem; + TransferArmSubsystem m_transferArmSubsystem; ShooterSubsystem m_shooterSubsystem; + ClimberSubsystem m_climberSubsystem; - frc2::CommandXboxController m_xbox{0}; - frc2::CommandGenericHID m_buttonBoard{1}; + // declared for the infrastructure subsystem + uint m_LEDPattern{29}; + uint m_LEDPatternCount{0}; +#pragma endregion }; diff --git a/src/main/include/commands/AutonomousCommands.h b/src/main/include/commands/AutonomousCommands.h index 35c25e8..05987d2 100644 --- a/src/main/include/commands/AutonomousCommands.h +++ b/src/main/include/commands/AutonomousCommands.h @@ -4,25 +4,21 @@ #pragma once -#include +#include #include #include #include #include - #include "subsystems/DriveSubsystem.h" -#include "subsystems/FeederSubsystem.h" +#include "subsystems/IntakeSubsystem.h" #include "subsystems/Infrastructure.h" #include "subsystems/ShooterSubsystem.h" -#include -#include - -class TimedAutoBase : public frc2::CommandHelper +class TimedAutoBase : public frc2::CommandHelper { protected: - TimedAutoBase(DriveSubsystem *const drive, FeederSubsystem *const feeder, InfrastructureSubsystem *const infrastructure, ShooterSubsystem *const shooter, std::string_view name) noexcept - : m_drive{drive}, m_feeder{feeder}, m_infrastructure{infrastructure}, m_shooter{shooter} { SetName(name); } + TimedAutoBase(DriveSubsystem *const drive, IntakeSubsystem *const Intake, ShooterSubsystem *const shooter, std::string_view name) noexcept + : m_drive{drive}, m_Intake{Intake}, m_shooter{shooter} { SetName(name); } public: virtual ~TimedAutoBase() noexcept = default; @@ -39,45 +35,15 @@ class TimedAutoBase : public frc2::CommandHelper(drive, feeder, infrastructure, shooter)}; - } -}; - -class TwoBallAuto : public TimedAutoBase -{ -public: - TwoBallAuto(DriveSubsystem *const drive, FeederSubsystem *const feeder, InfrastructureSubsystem *const infrastructure, ShooterSubsystem *const shooter) noexcept - : TimedAutoBase(drive, feeder, infrastructure, shooter, "TwoBallAuto") {} - - bool Iteration(const uint counter) noexcept override; - - static frc2::CommandPtr TwoBallAutoCommandFactory(DriveSubsystem *const drive, FeederSubsystem *const feeder, InfrastructureSubsystem *const infrastructure, ShooterSubsystem *const shooter) noexcept - { - return frc2::CommandPtr{std::make_unique(drive, feeder, infrastructure, shooter)}; - } -}; - namespace TrajectoryAuto { frc2::CommandPtr TrajectoryAutoCommandFactory(DriveSubsystem *const drive, std::string_view name, frc::Trajectory &trajectory) noexcept; diff --git a/src/main/include/commands/ClimberLowerCommand.h b/src/main/include/commands/ClimberLowerCommand.h new file mode 100644 index 0000000..e1af39f --- /dev/null +++ b/src/main/include/commands/ClimberLowerCommand.h @@ -0,0 +1,38 @@ +// 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. + +#pragma once + +#include +#include +#include "subsystems/ClimberSubsystem.h" +#include "Constants.h" +#include + +/** + * An example command. + * + *

Note that this extends CommandHelper, rather extending Command + * directly; this is crucially important, or else the decorator functions in + * Command will *not* work! + */ +class ClimberLowerCommand + : public frc2::CommandHelper { + public: + explicit ClimberLowerCommand(ClimberSubsystem *climberSubsystem) + : climberSubsystem{climberSubsystem} + { + AddRequirements(climberSubsystem); + } + + void Initialize() override; + void Execute() override; + void End(bool interrupted) override; + bool IsFinished() override; + + private: + ClimberSubsystem *climberSubsystem{nullptr}; + frc::Timer timer{}; + bool finished{false}; +}; diff --git a/src/main/include/commands/ClimberRaiseCommand.h b/src/main/include/commands/ClimberRaiseCommand.h new file mode 100644 index 0000000..a45c531 --- /dev/null +++ b/src/main/include/commands/ClimberRaiseCommand.h @@ -0,0 +1,38 @@ +// 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. + +#pragma once + +#include +#include +#include "subsystems/ClimberSubsystem.h" +#include "Constants.h" +#include + +/** + * An example command. + * + *

Note that this extends CommandHelper, rather extending Command + * directly; this is crucially important, or else the decorator functions in + * Command will *not* work! + */ +class ClimberRaiseCommand + : public frc2::CommandHelper { + public: + explicit ClimberRaiseCommand(ClimberSubsystem *climberSubsystem) + : climberSubsystem{climberSubsystem} + { + AddRequirements(climberSubsystem); + } + + void Initialize() override; + void Execute() override; + void End(bool interrupted) override; + bool IsFinished() override; + + private: + ClimberSubsystem *climberSubsystem{nullptr}; + frc::Timer timer{}; + bool finished{false}; +}; diff --git a/src/main/include/commands/ClimberStopCommand.h b/src/main/include/commands/ClimberStopCommand.h new file mode 100644 index 0000000..b94753a --- /dev/null +++ b/src/main/include/commands/ClimberStopCommand.h @@ -0,0 +1,36 @@ +// 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. + +#pragma once + +#include +#include +#include "subsystems/ClimberSubsystem.h" +#include "Constants.h" + +/** + * An example command. + * + *

Note that this extends CommandHelper, rather extending Command + * directly; this is crucially important, or else the decorator functions in + * Command will *not* work! + */ +class ClimberStopCommand + : public frc2::CommandHelper { + public: + explicit ClimberStopCommand(ClimberSubsystem *climberSubsystem) + : climberSubsystem{climberSubsystem} + { + AddRequirements(climberSubsystem); + } + + void Initialize() override; + void Execute() override; + void End(bool interrupted) override; + bool IsFinished() override; + + private: + ClimberSubsystem *climberSubsystem{nullptr}; + bool finished{false}; +}; diff --git a/src/main/include/commands/DriveCommands.h b/src/main/include/commands/DriveCommands.h new file mode 100644 index 0000000..fdcb3ff --- /dev/null +++ b/src/main/include/commands/DriveCommands.h @@ -0,0 +1,56 @@ +// 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. + +#pragma once + +#include +#include +#include +#include "subsystems/DriveSubsystem.h" + +// Home all swerve modules to zero heading. +class ZeroTurningModules + : public frc2::CommandHelper +{ +public: + explicit ZeroTurningModules(DriveSubsystem *driveSubsystem) noexcept + : driveSubsystem{driveSubsystem} { SetName("Zero"); } + + void Initialize() noexcept override {} + void Execute() noexcept override; + void End(bool interrupted) noexcept override {} + bool IsFinished() noexcept override { return finished; } + + static frc2::CommandPtr ZeroCommandFactory(DriveSubsystem *driveSubsystem) noexcept + { + return frc2::CommandPtr{std::make_unique(driveSubsystem)}; + } + +private: + DriveSubsystem *driveSubsystem{nullptr}; + bool finished{false}; +}; + +// TODO: decide what inputs this will accept +class DriveCommand + : public frc2::CommandHelper +{ +public: + explicit DriveCommand(DriveSubsystem *driveSubsystem) noexcept + : driveSubsystem{driveSubsystem} { SetName("DriveCommand"); } + + void Initialize() noexcept override; + void Execute() noexcept override; + void End(bool interrupted) noexcept override; + bool IsFinished() noexcept override { return finished; } + + static frc2::CommandPtr DriveCommandFactory(DriveSubsystem *driveSubsystem) noexcept + { + return frc2::CommandPtr{std::make_unique(driveSubsystem)}; + } + +private: + DriveSubsystem *driveSubsystem{nullptr}; + bool finished{false}; +}; diff --git a/src/main/include/commands/IntakeCommand.h b/src/main/include/commands/IntakeCommand.h new file mode 100644 index 0000000..6e2af3f --- /dev/null +++ b/src/main/include/commands/IntakeCommand.h @@ -0,0 +1,37 @@ +// 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. + +#pragma once + +#include +#include +#include "frc/DigitalInput.h" +#include "subsystems/IntakeSubsystem.h" +#include "Constants.h" +#include "subsystems/TransferArmSubsystem.h" + +/** + * This command is for picking up a note and moving to the home position with the arm + */ +class IntakeCommand + : public frc2::CommandHelper { + public: + explicit IntakeCommand(IntakeSubsystem *intakeSubsystem) + : intakeSubsystem{intakeSubsystem} + { + AddRequirements(intakeSubsystem); + } + + void Initialize() override; + void Execute() override; + void End(bool interrupted) override; + bool IsFinished() override; + + private: + IntakeSubsystem *intakeSubsystem{nullptr}; + bool finished{false}; + frc::DigitalInput limit1{8}; + frc::DigitalInput limit2{9}; + +}; diff --git a/src/main/include/commands/IntakeEjectCommand.h b/src/main/include/commands/IntakeEjectCommand.h new file mode 100644 index 0000000..79d5c43 --- /dev/null +++ b/src/main/include/commands/IntakeEjectCommand.h @@ -0,0 +1,38 @@ +// 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. + +#pragma once + +#include +#include +#include "subsystems/IntakeSubsystem.h" +#include "Constants.h" +#include + + + +/** + * This command is for ejecting the note from the intake. + * This can be called by the shootcommand to feed into the shooter + * Or used to eject a note onto the field + */ +class IntakeEjectCommand + : public frc2::CommandHelper { + public: + explicit IntakeEjectCommand(IntakeSubsystem *intakeSubsystem) + : intakeSubsystem{intakeSubsystem} + { + AddRequirements(intakeSubsystem); + } + + void Initialize() override; + void Execute() override; + void End(bool interrupted) override; + bool IsFinished() override; + + private: + IntakeSubsystem *intakeSubsystem{nullptr}; + bool finished{false}; + frc::Timer timer{}; +}; diff --git a/src/main/include/commands/PIDTransferArmCommand.h b/src/main/include/commands/PIDTransferArmCommand.h new file mode 100644 index 0000000..753def0 --- /dev/null +++ b/src/main/include/commands/PIDTransferArmCommand.h @@ -0,0 +1,30 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include "subsystems/TransferArmSubsystem.h" +#include "Constants.h" + +/** + * A command that will turn the transfer arm to the specified angle. + */ +class PIDPositionTransferArm : public frc2::CommandHelper { + public: + /** + * Turns the transfer arm to robot to the specified angle. + * + * @param targetAngleDegrees The angle to turn to + * @param transferArmSubsystem The transferArmSubsystem subsystem to use + */ + PIDPositionTransferArm(units::turn_t targetPosition, TransferArmSubsystem* transferArmSubsystem); + + bool IsFinished() override; +}; diff --git a/src/main/include/commands/PositionTransferArmCommand.h b/src/main/include/commands/PositionTransferArmCommand.h new file mode 100644 index 0000000..411d2f0 --- /dev/null +++ b/src/main/include/commands/PositionTransferArmCommand.h @@ -0,0 +1,40 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include "subsystems/TransferArmSubsystem.h" + +class PositionTransferArm + : public frc2::CommandHelper +{ +public: + explicit PositionTransferArm(TransferArmSubsystem *transferArmSubsystem, units::degree_t position) noexcept + : transferArmSubsystem{transferArmSubsystem}, position{position} + { + SetName("PositionTransferArm"); + AddRequirements(transferArmSubsystem); + } + + void Initialize() noexcept override; + void Execute() noexcept override; + void End(bool interrupted) noexcept override; + bool IsFinished() noexcept override { return finished; } + + static frc2::CommandPtr PositionTransferArmCommandFactory(TransferArmSubsystem *transferArmSubsystem, units::degree_t position) noexcept + { + return frc2::CommandPtr{std::make_unique(transferArmSubsystem, position)}; + } + +private: + TransferArmSubsystem *transferArmSubsystem{nullptr}; + units::degree_t position; + frc::Timer timer{}; + bool finished{false}; +}; diff --git a/src/main/include/commands/ShootCommands.h b/src/main/include/commands/ShootCommands.h new file mode 100644 index 0000000..56e3c21 --- /dev/null +++ b/src/main/include/commands/ShootCommands.h @@ -0,0 +1,38 @@ +// 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. + +#pragma once + +#include +#include +#include +#include "subsystems/ShooterSubsystem.h" + +/* + * An example command. + * + *

Note that this extends CommandHelper, rather extending Command + * directly; this is crucially important, or else the decorator functions in + * Command will *not* work! + */ +class ShootCommands + : public frc2::CommandHelper +{ +public: + explicit ShootCommands(ShooterSubsystem *shooterSubsystem) + : shooterSubsystem{shooterSubsystem} + { + AddRequirements(shooterSubsystem); + } + + void Initialize() override; + void Execute() override; + void End(bool interrupted) override; + bool IsFinished() override; + +private: + ShooterSubsystem *shooterSubsystem{nullptr}; + frc::Timer timer{}; + bool finished{false}; +}; diff --git a/src/main/include/commands/TestModeCommands.h b/src/main/include/commands/TestModeCommands.h deleted file mode 100644 index 6440d4d..0000000 --- a/src/main/include/commands/TestModeCommands.h +++ /dev/null @@ -1,258 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -#include -#include -#include - -#include "subsystems/DriveSubsystem.h" - -#include - -// Home all swerve modules to zero heading. -class ZeroCommand - : public frc2::CommandHelper -{ -public: - explicit ZeroCommand(DriveSubsystem *subsystem) noexcept - : m_subsystem{subsystem} { SetName("Zero"); } - - void Initialize() noexcept override {} - - void Execute() noexcept override; - - void End(bool interrupted) noexcept override {} - - static frc2::CommandPtr ZeroCommandFactory(DriveSubsystem *subsystem) noexcept - { - return frc2::CommandPtr{std::make_unique(subsystem)}; - } - -private: - DriveSubsystem *m_subsystem{nullptr}; -}; - -// Expose turning maximum velocity and acceleration. -class MaxVAndATurningCommand - : public frc2::CommandHelper -{ -public: - explicit MaxVAndATurningCommand(DriveSubsystem *subsystem) noexcept - : m_subsystem{subsystem} { SetName("MaxVAndATurning"); } - - void Initialize() noexcept override; - - void Execute() noexcept override; - - void End(bool interrupted) noexcept override; - - static frc2::CommandPtr MaxVAndATurningCommandFactory(DriveSubsystem *subsystem) noexcept - { - return frc2::CommandPtr{std::make_unique(subsystem)}; - } - -private: - DriveSubsystem *m_subsystem{nullptr}; - - unsigned m_iteration{0}; -}; - -// Expose drive maximum velocity and acceleration. -class MaxVAndADriveCommand - : public frc2::CommandHelper -{ -public: - explicit MaxVAndADriveCommand(DriveSubsystem *subsystem) noexcept - : m_subsystem{subsystem} { SetName("MaxVAndADrive"); } - - void Initialize() noexcept override; - - void Execute() noexcept override; - - void End(bool interrupted) noexcept override; - - static frc2::CommandPtr MaxVAndADriveCommandFactory(DriveSubsystem *subsystem) noexcept - { - return frc2::CommandPtr{std::make_unique(subsystem)}; - } - -private: - DriveSubsystem *m_subsystem{nullptr}; - - unsigned m_iteration{0}; -}; - -// Alternately command swerve modules to form X and O patterns. -class XsAndOsCommand - : public frc2::CommandHelper -{ -public: - explicit XsAndOsCommand(DriveSubsystem *subsystem) noexcept - : m_subsystem{subsystem} { SetName("Xs and Os"); } - - void Initialize() noexcept override; - - void Execute() noexcept override; - - void End(bool interrupted) noexcept override; - - static frc2::CommandPtr XsAndOsCommandFactory(DriveSubsystem *subsystem) noexcept - { - return frc2::CommandPtr{std::make_unique(subsystem)}; - } - -private: - DriveSubsystem *m_subsystem{nullptr}; - - unsigned m_iteration{0}; -}; - -// Command swerve modules to slowly rotate together (spin). -class RotateModulesCommand - : public frc2::CommandHelper -{ -public: - explicit RotateModulesCommand(DriveSubsystem *subsystem) noexcept - : m_subsystem{subsystem} { SetName("Rotate Modules"); } - - void Initialize() noexcept override; - - void Execute() noexcept override; - - void End(bool interrupted) noexcept override; - - static frc2::CommandPtr RotateModulesCommandFactory(DriveSubsystem *subsystem) noexcept - { - return frc2::CommandPtr{std::make_unique(subsystem)}; - } - -private: - DriveSubsystem *m_subsystem{nullptr}; - - unsigned m_iteration{0}; -}; - -// Drive in a square. -class SquareCommand - : public frc2::CommandHelper -{ -public: - explicit SquareCommand(DriveSubsystem *subsystem) noexcept - : m_subsystem{subsystem} { SetName("Square"); } - - void Initialize() noexcept override; - - void Execute() noexcept override; - - void End(bool interrupted) noexcept override; - - static frc2::CommandPtr SquareCommandFactory(DriveSubsystem *subsystem) noexcept - { - return frc2::CommandPtr{std::make_unique(subsystem)}; - } - -private: - DriveSubsystem *m_subsystem{nullptr}; - - unsigned m_side{0}; -}; - -// Drive in a spirograph pattern. -class SpirographCommand - : public frc2::CommandHelper -{ -public: - explicit SpirographCommand(DriveSubsystem *subsystem) noexcept - : m_subsystem{subsystem} { SetName("Spirograph"); } - - void Initialize() noexcept override; - - void Execute() noexcept override; - - void End(bool interrupted) noexcept override; - - static frc2::CommandPtr SpirographCommandFactory(DriveSubsystem *subsystem) noexcept - { - return frc2::CommandPtr{std::make_unique(subsystem)}; - } - -private: - DriveSubsystem *m_subsystem{nullptr}; - - unsigned m_angle{0}; -}; - -// Drive in an orbit. -class OrbitCommand - : public frc2::CommandHelper -{ -public: - explicit OrbitCommand(DriveSubsystem *subsystem) noexcept - : m_subsystem{subsystem} { SetName("Orbit"); } - - void Initialize() noexcept override {} - - void Execute() noexcept override; - - void End(bool interrupted) noexcept override {} - - static frc2::CommandPtr OrbitCommandFactory(DriveSubsystem *subsystem) noexcept - { - return frc2::CommandPtr{std::make_unique(subsystem)}; - } - -private: - DriveSubsystem *m_subsystem{nullptr}; -}; - -// Drive in a fancy pirouette. -class PirouetteCommand - : public frc2::CommandHelper -{ -public: - explicit PirouetteCommand(DriveSubsystem *subsystem) noexcept - : m_subsystem{subsystem} { SetName("Pirouette"); } - - void Initialize() noexcept override; - - void Execute() noexcept override; - - void End(bool interrupted) noexcept override; - - static frc2::CommandPtr PirouetteCommandFactory(DriveSubsystem *subsystem) noexcept - { - return frc2::CommandPtr{std::make_unique(subsystem)}; - } - -private: - DriveSubsystem *m_subsystem{nullptr}; -}; - -// Excersize theta (spin) controller. -class SpinCommand - : public frc2::CommandHelper -{ -public: - explicit SpinCommand(DriveSubsystem *subsystem) noexcept - : m_subsystem{subsystem} { SetName("Spin"); } - - void Initialize() noexcept override; - - void Execute() noexcept override; - - void End(bool interrupted) noexcept override; - - static frc2::CommandPtr SpinCommandFactory(DriveSubsystem *subsystem) noexcept - { - return frc2::CommandPtr{std::make_unique(subsystem)}; - } - -private: - DriveSubsystem *m_subsystem{nullptr}; - - unsigned m_angle{0}; - unsigned m_delay{0}; -}; diff --git a/src/main/include/infrastructure/PWMAngleSensor.h b/src/main/include/infrastructure/PWMAngleSensor.h index ffaf82c..026fcbd 100644 --- a/src/main/include/infrastructure/PWMAngleSensor.h +++ b/src/main/include/infrastructure/PWMAngleSensor.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include @@ -15,7 +16,7 @@ class AngleSensor { public: - AngleSensor(const int channel, const int alignment) noexcept; + AngleSensor(int deviceID, units::turn_t alignment) noexcept; AngleSensor(const AngleSensor &) = delete; AngleSensor &operator=(const AngleSensor &) = delete; @@ -25,22 +26,19 @@ class AngleSensor void ShuffleboardCreate(frc::ShuffleboardContainer &container, std::function()> getCommandedAndEncoderPositions = nullptr) noexcept; - int GetAlignment() noexcept { return alignment_; } + units::turn_t GetAlignment() noexcept; - void SetAlignment(const int alignment) noexcept { alignment_ = alignment; } + void SetAlignment(const units::turn_t alignment) noexcept; std::optional GetAbsolutePosition() noexcept; - std::optional GetAbsolutePositionWithoutAlignment() noexcept; + std::optional GetAbsolutePositionWithoutAlignment() noexcept; private: // Range is [-2048, +2048). - std::optional GetAbsolutePosition(const int frequency, const double output, const bool applyOffset) noexcept; + std::optional GetAbsolutePosition(const int frequency, const double output, const bool applyOffset) noexcept; - int alignment_{0}; - - std::unique_ptr digitalInput_; - std::unique_ptr dutyCycle_; + ctre::phoenix6::hardware::CANcoder canCoder_; std::function()> getCommandedAndEncoderPositionsF_{nullptr}; HeadingGyro headingGyro_; diff --git a/src/main/include/infrastructure/SparkMax.h b/src/main/include/infrastructure/SparkMax.h index f2593a2..93897e2 100644 --- a/src/main/include/infrastructure/SparkMax.h +++ b/src/main/include/infrastructure/SparkMax.h @@ -4,7 +4,7 @@ #include "infrastructure/SmartMotor.h" -namespace SparkMaxFactory +namespace SparkFactory { // Print out string to index mapping (for updating switch/cases). void ConfigIndex() noexcept; @@ -109,8 +109,8 @@ namespace SparkMaxFactory // kDataPortConfig; 0 = Default (limit switches are enabled), 1 = Alternate // Encoder Mode (limit switches are disabled / alternate encoder is enabled) // This configuration parameter also has no explicit set and no get at all. -// By constructing a SparkMax, either a rev::SparkMaxRelativeEncoder or a -// rev::SparkMaxAlternateEncoder is constructed, setting this as a side +// By constructing a SparkMax, either a rev::SparkRelativeEncoder or a +// rev::SparkAlternateEncoder is constructed, setting this as a side // effect. This is otherwise the same as the two prior parameters. This // is the fourth of the four parameters to be manually set and saved, and the // fifth of the six parameters that persist. @@ -205,7 +205,7 @@ namespace SparkMaxFactory // Limit switches (hard limits) only work when not in Alternate Encoder Mode. // For polarity, there are similar issues with get (none) and set (side // effect of calling GetForwardLimitSwitch() or GetReverseLimitSwitch()) as -// with analog feedback. Enable get and set are via SparkMaxLimitSwitch. By +// with analog feedback. Enable get and set are via SparkLimitSwitch. By // definition, there isn't much code to handle a hard limit switch, leaving // this mainly to configuration. It may be useful to be able to report the // status, and to enable/disable the limits. Changing the polarity via @@ -378,7 +378,7 @@ namespace SparkMaxFactory // Also note that SmartMotionAccelStrategy does not appear to be implemented; // if it is set to kSCurve, this succeeds -- but it reads back as kTrapezoidal. -namespace SparkMaxFactory +namespace SparkFactory { // Version 1.5.2; all configuration parameters are current at this release. // **** DO NOT ALTER THIS LIST WITHOUT UNDERSTANDING THE IMPLICATIONS! **** @@ -412,7 +412,7 @@ namespace SparkMaxFactory {"kAltEncoderVelocityFactor", double{1.0}}, {"kCurrentChop", double{115.0}}, // Amps {"kCurrentChopCycles", uint{0}}, - {"kSmartCurrentStallLimit", uint{80}}, // Amps + {"kSmartCurrentStallLimit", uint{40}}, // Amps {"kSmartCurrentFreeLimit", uint{20}}, // Amps {"kSmartCurrentConfig", uint{10000}}, // RPM {"kP_0", double{0.0}}, diff --git a/src/main/include/infrastructure/SwerveModule.h b/src/main/include/infrastructure/SwerveModule.h index 7e37040..c9c1927 100644 --- a/src/main/include/infrastructure/SwerveModule.h +++ b/src/main/include/infrastructure/SwerveModule.h @@ -140,7 +140,7 @@ class SwerveModule const int driveMotorCanID, const int turningMotorCanID, const int turningEncoderPort, - const int alignmentOffset) noexcept; + const units::turn_t alignmentOffset) noexcept; // No copy/assign. SwerveModule(const SwerveModule &) = delete; @@ -177,7 +177,10 @@ class SwerveModule // Determine if commanded turning position has been achieved, to within // specified tolerance. - bool CheckTurningPosition(const units::angle::degree_t tolerance = 2.5_deg) noexcept; + bool CheckTurningPosition(const units::angle::degree_t tolerance = 5_deg) noexcept; + + // Stop all output to turning motors to save power when stationary. + void StopTurning() noexcept; // Drive is normally oriented around velocity, but distance enables odometry, // simple dead reckoning, etc. Possibly useful for autonomous driving. @@ -274,6 +277,7 @@ class SwerveModule // roboRIO. Setting m_rio false allows testing turning PID on the SPARK MAX. const bool m_rio{true}; bool m_brakeApplied{true}; + bool m_turningStopped{false}; double m_rioPID_F{pidf::kTurningPositionF}; std::unique_ptr> m_rioPIDController; @@ -285,8 +289,8 @@ class SwerveModule double m_turningPosition_D{pidf::kTurningPositionD}; double m_turningPosition_DF{pidf::kTurningPositionDF}; double m_turningPosition_F{pidf::kTurningPositionF}; - double m_turningPosition_V{pidf::kTurningPositionMaxVelocity}; - double m_turningPosition_A{pidf::kTurningPositionMaxAcceleration}; + units::degrees_per_second_t m_turningPosition_V{pidf::kTurningPositionMaxVelocity}; + units::degrees_per_second_squared_t m_turningPosition_A{pidf::kTurningPositionMaxAcceleration}; // Drive position PID double m_drivePosition_P{pidf::kDrivePositionP}; diff --git a/src/main/include/subsystems/ClimberSubsystem.h b/src/main/include/subsystems/ClimberSubsystem.h new file mode 100644 index 0000000..5c32037 --- /dev/null +++ b/src/main/include/subsystems/ClimberSubsystem.h @@ -0,0 +1,28 @@ +// 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. + +#pragma once + +#include +#include "Constants.h" +#include "rev/CANSparkMax.h" + + +class ClimberSubsystem : public frc2::SubsystemBase +{ + public: + ClimberSubsystem() noexcept; + + ClimberSubsystem(const ClimberSubsystem &) = delete; + ClimberSubsystem &operator=(const ClimberSubsystem &) = delete; + + void StopClimber() noexcept; + void SetClimberMotorVoltagePercent(const double percent) noexcept; + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + + rev::CANSparkMax m_ClimberMotor{climber::kClimberMotorCanID, rev::CANSparkMax::MotorType::kBrushed}; +}; diff --git a/src/main/include/subsystems/FeederSubsystem.h b/src/main/include/subsystems/FeederSubsystem.h deleted file mode 100644 index b5cc955..0000000 --- a/src/main/include/subsystems/FeederSubsystem.h +++ /dev/null @@ -1,65 +0,0 @@ -#pragma once - -#include "infrastructure/SparkMax.h" - -#include -#include - -#include -#include - -class FeederSubsystem : public frc2::SubsystemBase -{ -public: - // The only ctor of the FeederSubsystem class. - FeederSubsystem() noexcept; - - FeederSubsystem(const FeederSubsystem &) = delete; - FeederSubsystem &operator=(const FeederSubsystem &) = delete; - - void Periodic() noexcept override; - - bool GetStatus() const noexcept; - - void TestInit() noexcept; - void TestExit() noexcept; - void TestPeriodic() noexcept; - - void Default(const double percent) noexcept; - void NoFeed() noexcept; - - void Eject() noexcept; - - void Fire() noexcept; - - void Raise() noexcept; - - void Lower() noexcept; - - void LockIntake() noexcept; - - void DropIntake() noexcept; - - void RaiseIntake() noexcept; - - void LowerIntake() noexcept; - - void BurnConfig() noexcept; - - void ClearFaults() noexcept; - -private: - std::unique_ptr m_intakeMotorBase; - std::unique_ptr m_elevatorMotorBase; - std::unique_ptr m_feederMotorBase; - std::unique_ptr m_climberMotorBase; - std::unique_ptr> m_intakeMotor; - std::unique_ptr> m_elevatorMotor; - std::unique_ptr> m_feederMotor; - std::unique_ptr> m_climberMotor; - - std::unique_ptr m_intakeRelease; - std::unique_ptr m_intakeRaise; - - std::chrono::steady_clock::time_point m_verifyMotorControllersWhen; -}; diff --git a/src/main/include/subsystems/IntakeSubsystem.h b/src/main/include/subsystems/IntakeSubsystem.h new file mode 100644 index 0000000..e388763 --- /dev/null +++ b/src/main/include/subsystems/IntakeSubsystem.h @@ -0,0 +1,20 @@ +#pragma once + +#include "Constants.h" +#include "rev/CANSparkMax.h" +#include + +class IntakeSubsystem : public frc2::SubsystemBase +{ +public: + IntakeSubsystem() noexcept; + + IntakeSubsystem(const IntakeSubsystem &) = delete; + IntakeSubsystem &operator=(const IntakeSubsystem &) = delete; + + void StopIntake() noexcept; + void SetSpinMotorVoltagePercent(const double percent) noexcept; + +private: + rev::CANSparkMax m_IntakeMotor{intake::kIntakeSpinMotorCanID, rev::CANSparkMax::MotorType::kBrushed}; +}; diff --git a/src/main/include/subsystems/ShooterSubsystem.h b/src/main/include/subsystems/ShooterSubsystem.h index 9d3df4a..a43f68f 100644 --- a/src/main/include/subsystems/ShooterSubsystem.h +++ b/src/main/include/subsystems/ShooterSubsystem.h @@ -1,42 +1,21 @@ #pragma once -#include "infrastructure/SparkMax.h" - +#include "Constants.h" +#include "rev/CANSparkMax.h" #include -#include -#include - class ShooterSubsystem : public frc2::SubsystemBase { public: - // The only ctor of the FeederSubsystem class. ShooterSubsystem() noexcept; ShooterSubsystem(const ShooterSubsystem &) = delete; ShooterSubsystem &operator=(const ShooterSubsystem &) = delete; - void Periodic() noexcept override; - - bool GetStatus() const noexcept; - - void TestInit() noexcept; - void TestExit() noexcept; - void TestPeriodic() noexcept; - - void Default(const double percent, const double velocity) noexcept; - - void Stop() noexcept; - - void BurnConfig() noexcept; - - void ClearFaults() noexcept; + void StopShooter() noexcept; + void SetShooterMotorVoltagePercent(const double percent) noexcept; private: - std::unique_ptr m_shooterMotorBase; - std::unique_ptr m_backspinMotorBase; - std::unique_ptr> m_shooterMotor; - std::unique_ptr> m_backspinMotor; - - std::chrono::steady_clock::time_point m_verifyMotorControllersWhen; + rev::CANSparkMax m_LeftShooterMotor{shooter::kLeftShooterMotorCanID, rev::CANSparkMax::MotorType::kBrushed}; + rev::CANSparkMax m_RightShooterMotor{shooter::kRightShooterMotorCanID, rev::CANSparkMax::MotorType::kBrushed}; }; diff --git a/src/main/include/subsystems/TransferArmSubsystem.h b/src/main/include/subsystems/TransferArmSubsystem.h new file mode 100644 index 0000000..b74f814 --- /dev/null +++ b/src/main/include/subsystems/TransferArmSubsystem.h @@ -0,0 +1,27 @@ +#pragma once + +#include "Constants.h" +#include "rev/CANSparkMax.h" +#include +#include + +class TransferArmSubsystem : public frc2::SubsystemBase +{ +public: + TransferArmSubsystem() noexcept; + + TransferArmSubsystem(const TransferArmSubsystem &) = delete; + TransferArmSubsystem &operator=(const TransferArmSubsystem &) = delete; + + void StopTransferArm() noexcept; + void SetArmMotorVoltagePercent(const double percent) noexcept; + // void SetTransferArmPosition(const units::turn_t position) noexcept; + units::turn_t GetTransferArmPosition() noexcept; + // void UpdatePIDValues() noexcept; + +private: + rev::CANSparkMax m_TransferArmMotor{arm::kTransferArmMotorCanID, rev::CANSparkMax::MotorType::kBrushless}; + rev::SparkMaxAlternateEncoder m_encoder = m_TransferArmMotor.GetAlternateEncoder(8192); + // rev::SparkPIDController m_pidController = m_TransferArmMotor.GetPIDController(); + // double kP = arm::kArmPositionP, kI = 0, kD = arm::kArmPositionD, kIz = 0, kFF = arm::kArmPositionF, kMaxOutput = 1, kMinOutput = -1; +}; diff --git a/src/test/cpp/main.cpp b/src/test/cpp/main.cpp index 3640cef..b8b23d2 100644 --- a/src/test/cpp/main.cpp +++ b/src/test/cpp/main.cpp @@ -2,8 +2,7 @@ #include "gtest/gtest.h" -int main(int argc, char **argv) -{ +int main(int argc, char** argv) { HAL_Initialize(500, 0); ::testing::InitGoogleTest(&argc, argv); int ret = RUN_ALL_TESTS(); diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json index 29ec93a..e978a5f 100644 --- a/vendordeps/NavX.json +++ b/vendordeps/NavX.json @@ -1,17 +1,18 @@ { "fileName": "NavX.json", - "name": "KauaiLabs_navX_FRC", - "version": "2023.0.3", + "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.3" + "version": "2024.1.0" } ], "jniDependencies": [], @@ -19,7 +20,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-cpp", - "version": "2023.0.3", + "version": "2024.1.0", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false, @@ -30,7 +31,7 @@ "linuxraspbian", "linuxarm32", "linuxarm64", - "linux86-64", + "linuxx86-64", "osxuniversal", "windowsx86-64" ] diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 0000000..88a68dd --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.0", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.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.33.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.33.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.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.33.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.33.0", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix6.json similarity index 61% rename from vendordeps/Phoenix.json rename to vendordeps/Phoenix6.json index 282d536..69a4079 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix6.json @@ -1,56 +1,32 @@ { - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.30.4+23.0.12", - "frcYear": 2023, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.1.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", - "javaDependencies": [ + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "conflictsWith": [ { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.30.4" - }, + "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", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "5.30.4" + "version": "24.1.0" } ], "jniDependencies": [ { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.4", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.4", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.12", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -61,9 +37,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.12", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -74,9 +50,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.12", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -87,9 +63,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.12", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -100,9 +76,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.12", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -113,9 +89,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.12", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -126,9 +102,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.12", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -139,9 +115,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.12", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,9 +128,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.12", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -165,9 +141,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.12", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,10 +156,10 @@ ], "cppDependencies": [ { - "groupId": "com.ctre.phoenix", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "5.30.4", - "libName": "CTRE_Phoenix_WPI", + "version": "24.1.0", + "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -195,39 +171,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.30.4", - "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.30.4", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.12", + "version": "24.1.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.30.4", - "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.30.4", - "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.30.4", - "libName": "CTRE_PhoenixCCISim", + "version": "24.1.0", + "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -285,9 +201,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.12", + "version": "24.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -300,9 +216,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.12", + "version": "24.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -315,9 +231,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.12", + "version": "24.1.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -330,9 +246,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.12", + "version": "24.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -345,9 +261,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.12", + "version": "24.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -360,9 +276,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.12", + "version": "24.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -375,9 +291,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.12", + "version": "24.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -390,9 +306,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.12", + "version": "24.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -405,9 +321,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.12", + "version": "24.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index f2d0b7d..0f3520e 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,24 +1,25 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2023.1.3", + "version": "2024.2.0", + "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2023.1.3" + "version": "2024.2.0" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2023.1.3", + "version": "2024.2.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2023.1.3", + "version": "2024.2.0", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -54,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2023.1.3", + "version": "2024.2.0", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 65dcc03..67bf389 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,35 +3,36 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2024", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-java", - "version": "wpilib" - } + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } ], "jniDependencies": [], "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-cpp", - "version": "wpilib", - "libName": "wpilibNewCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxuniversal" - ] - } + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } ] }