├── .gitmodules ├── .gitignore ├── gradle └── wrapper │ ├── gradle-wrapper.jar │ └── gradle-wrapper.properties ├── init ├── src └── main │ └── java │ └── org │ └── frcteam2910 │ └── c2022 │ ├── Main.java │ ├── util │ ├── Utilities.java │ ├── ClimbChooser.java │ ├── SysIdGeneralMechanismLogger.java │ ├── DriverReadout.java │ ├── SysIdLogger.java │ ├── AutonomousTrajectories.java │ └── AutonomousChooser.java │ ├── commands │ ├── EnableCompressorCommand.java │ ├── DefaultIntakeCommand.java │ ├── ManualFeedToShooterCommand.java │ ├── DefaultFeederCommand.java │ ├── SetFlywheelVoltageCommand.java │ ├── DefaultShooterCommand.java │ ├── FollowTrajectoryCommand.java │ ├── ClimberToPointCommand.java │ ├── SetHoodAngleCommand.java │ ├── ShootWhenReadyCommand.java │ ├── SimpleIntakeCommand.java │ ├── FenderShootCommand.java │ ├── CharacterizeDrivetrainCommand.java │ ├── ResetFeederCommand.java │ ├── DefaultDriveCommand.java │ ├── ZeroClimberCommand.java │ ├── AlignRobotToShootCommand.java │ ├── ZeroHoodCommand.java │ ├── TargetWithShooterCommand.java │ └── AutoClimbCommand.java │ ├── Superstructure.java │ ├── Robot.java │ ├── subsystems │ ├── IntakeSubsystem.java │ ├── FeederSubsystem.java │ ├── VisionSubsystem.java │ ├── DrivetrainSubsystem.java │ ├── ClimberSubsystem.java │ └── ShooterSubsystem.java │ ├── Constants.java │ └── RobotContainer.java ├── git-hooks └── pre-commit ├── vendordeps ├── SdsSwerveLib.json ├── WPILibNewCommands.json ├── PhotonLib-json-1.0.json ├── REVLib.json └── Phoenix-frc2022-latest.json ├── settings.gradle ├── .github └── workflows │ └── master.yml ├── gradlew.bat └── gradlew /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "common"] 2 | path = common 3 | url = https://github.com/FRCTeam2910/Common.git 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore Gradle output folders 2 | /.gradle/ 3 | /build/ 4 | 5 | # Ignore Intellij configuration 6 | /.idea/ 7 | -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FRCTeam2910/2022CompetitionRobot/HEAD/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /init: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | echo "Installing git hooks" 4 | rm .git/hooks/pre-commit 5 | ln -s -r git-hooks/pre-commit .git/hooks/pre-commit 6 | -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=permwrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip 4 | zipStoreBase=GRADLE_USER_HOME 5 | zipStorePath=permwrapper/dists 6 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/Main.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022; 2 | 3 | import edu.wpi.first.wpilibj.RobotBase; 4 | 5 | public class Main { 6 | public static void main(String[] args) { 7 | RobotBase.startRobot(Robot::new); 8 | } 9 | } 10 | -------------------------------------------------------------------------------- /git-hooks/pre-commit: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | echo "Checking if code is properly formatted" 4 | if ! ./gradlew spotlessCheck &> /dev/null; then 5 | echo "Code is not formatted correctly, formatting code" 6 | ./gradlew spotlessApply &> /dev/null 7 | echo "Finished formatting code" 8 | echo "Please review code changes and commit again" 9 | exit 1 10 | else 11 | echo "Code is properly formatted" 12 | fi 13 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/util/Utilities.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.util; 2 | 3 | import edu.wpi.first.math.geometry.Pose2d; 4 | import org.frcteam2910.common.math.RigidTransform2; 5 | import org.frcteam2910.common.math.Rotation2; 6 | import org.frcteam2910.common.math.Vector2; 7 | 8 | public class Utilities { 9 | public static RigidTransform2 poseToRigidTransform(Pose2d pose) { 10 | return new RigidTransform2(new Vector2(pose.getX(), pose.getY()), 11 | Rotation2.fromDegrees(pose.getRotation().getDegrees())); 12 | } 13 | } 14 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/EnableCompressorCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj.PneumaticHub; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | 6 | public class EnableCompressorCommand extends CommandBase { 7 | private final PneumaticHub pneumatics; 8 | 9 | public EnableCompressorCommand(PneumaticHub pneumatics) { 10 | this.pneumatics = pneumatics; 11 | } 12 | 13 | @Override 14 | public void initialize() { 15 | pneumatics.enableCompressorAnalog(115, 120); 16 | } 17 | } 18 | -------------------------------------------------------------------------------- /vendordeps/SdsSwerveLib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "SdsSwerveLib.json", 3 | "name": "swerve-lib", 4 | "version": "1.1.0", 5 | "uuid": "9619F7EA-7F96-4236-9D94-02338DFED592", 6 | "mavenUrls": [ 7 | "https://jitpack.io" 8 | ], 9 | "jsonUrl": "https://raw.githubusercontent.com/SwerveDriveSpecialties/swerve-lib/master/SdsSwerveLib.json", 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.github.SwerveDriveSpecialties", 13 | "artifactId": "swerve-lib", 14 | "version": "1.1.0" 15 | } 16 | ], 17 | "jniDependencies": [], 18 | "cppDependencies": [] 19 | } 20 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/DefaultIntakeCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import org.frcteam2910.c2022.subsystems.IntakeSubsystem; 5 | 6 | public class DefaultIntakeCommand extends CommandBase { 7 | private final IntakeSubsystem intake; 8 | 9 | public DefaultIntakeCommand(IntakeSubsystem intake) { 10 | this.intake = intake; 11 | 12 | addRequirements(intake); 13 | } 14 | 15 | @Override 16 | public void initialize() { 17 | intake.setExtended(false); 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/Superstructure.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022; 2 | 3 | import edu.wpi.first.wpilibj.PneumaticHub; 4 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 5 | import org.frcteam2910.c2022.commands.EnableCompressorCommand; 6 | 7 | public class Superstructure { 8 | private final PneumaticHub pneumatics = new PneumaticHub(); 9 | 10 | public Superstructure() { 11 | pneumatics.enableCompressorAnalog(115, 120); 12 | SmartDashboard.putData(new EnableCompressorCommand(pneumatics)); 13 | } 14 | 15 | public double getCurrentPressure() { 16 | return pneumatics.getPressure(0); 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/ManualFeedToShooterCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import org.frcteam2910.c2022.subsystems.FeederSubsystem; 5 | 6 | public class ManualFeedToShooterCommand extends CommandBase { 7 | private static final double FEEDER_SPEED = 0.5; 8 | 9 | private final FeederSubsystem feeder; 10 | 11 | public ManualFeedToShooterCommand(FeederSubsystem feeder) { 12 | this.feeder = feeder; 13 | 14 | addRequirements(feeder); 15 | } 16 | 17 | @Override 18 | public void execute() { 19 | feeder.setFeederSpeed(FEEDER_SPEED); 20 | } 21 | 22 | @Override 23 | public void end(boolean interrupted) { 24 | feeder.setFeederSpeed(0.0); 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/DefaultFeederCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import org.frcteam2910.c2022.subsystems.FeederSubsystem; 5 | 6 | public class DefaultFeederCommand extends CommandBase { 7 | private static final double MAX_FEEDER_SPEED = 0.75; 8 | 9 | private final FeederSubsystem feeder; 10 | 11 | public DefaultFeederCommand(FeederSubsystem feeder) { 12 | this.feeder = feeder; 13 | addRequirements(feeder); 14 | } 15 | 16 | @Override 17 | public void execute() { 18 | if (!feeder.isFull() && feeder.isBallAtEntry()) { 19 | feeder.setFeederSpeed(MAX_FEEDER_SPEED); 20 | } else { 21 | feeder.setFeederSpeed(0.0); 22 | } 23 | } 24 | } 25 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/SetFlywheelVoltageCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import org.frcteam2910.c2022.subsystems.ShooterSubsystem; 5 | 6 | public class SetFlywheelVoltageCommand extends CommandBase { 7 | private final ShooterSubsystem shooter; 8 | private final double voltage; 9 | 10 | public SetFlywheelVoltageCommand(ShooterSubsystem shooter, double voltage) { 11 | this.shooter = shooter; 12 | this.voltage = voltage; 13 | 14 | addRequirements(shooter); 15 | } 16 | 17 | @Override 18 | public void execute() { 19 | shooter.setFlywheelVoltage(voltage); 20 | } 21 | 22 | @Override 23 | public void end(boolean interrupted) { 24 | shooter.setFlywheelVoltage(0.0); 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/DefaultShooterCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import org.frcteam2910.c2022.subsystems.ShooterSubsystem; 5 | 6 | public class DefaultShooterCommand extends CommandBase { 7 | private final ShooterSubsystem shooter; 8 | 9 | public DefaultShooterCommand(ShooterSubsystem shooter) { 10 | this.shooter = shooter; 11 | addRequirements(shooter); 12 | } 13 | 14 | public void initialize() { 15 | shooter.setHoodTargetPosition(Math.toRadians(0.0)); 16 | shooter.setTargetFlywheelSpeed(ShooterSubsystem.FLYWHEEL_IDLE_SPEED); 17 | shooter.enableCurrentLimits(true); 18 | } 19 | 20 | @Override 21 | public void end(boolean interrupted) { 22 | shooter.enableCurrentLimits(false); 23 | } 24 | } 25 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/util/ClimbChooser.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.util; 2 | 3 | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; 4 | 5 | public class ClimbChooser { 6 | private final SendableChooser climbChooser = new SendableChooser<>(); 7 | 8 | public ClimbChooser() { 9 | climbChooser.setDefaultOption("Traversal Partway", ClimbType.TRAVERSAL_PARTWAY); 10 | climbChooser.addOption("Traversal Hook", ClimbType.TRAVERSAL_HOOK); 11 | climbChooser.addOption("Traversal Partway", ClimbType.TRAVERSAL_PARTWAY); 12 | climbChooser.addOption("High Hook", ClimbType.HIGH_HOOK); 13 | climbChooser.addOption("High Partway", ClimbType.HIGH_PARTWAY); 14 | climbChooser.addOption("Mid / Low", ClimbType.MID); 15 | } 16 | 17 | public SendableChooser getClimbChooser() { 18 | return climbChooser; 19 | } 20 | 21 | public enum ClimbType { 22 | TRAVERSAL_HOOK, TRAVERSAL_PARTWAY, HIGH_HOOK, HIGH_PARTWAY, MID 23 | } 24 | } 25 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/util/SysIdGeneralMechanismLogger.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.util; 2 | 3 | import java.util.Set; 4 | 5 | public class SysIdGeneralMechanismLogger extends SysIdLogger { 6 | private static final Set MECHANISMS = Set.of("Arm", "Elevator", "Simple"); 7 | private double primaryMotorVoltage = 0.0; 8 | 9 | public double getMotorVoltage() { 10 | return primaryMotorVoltage; 11 | } 12 | 13 | public void log(double measuredPosition, double measuredVelocity) { 14 | updateData(); 15 | if (data.size() < DATA_BUFFER_SIZE) { 16 | data.add(timestamp); 17 | data.add(primaryMotorVoltage); 18 | data.add(measuredPosition); 19 | data.add(measuredVelocity); 20 | } 21 | 22 | primaryMotorVoltage = motorVoltage; 23 | } 24 | 25 | public void reset() { 26 | super.reset(); 27 | primaryMotorVoltage = 0.0; 28 | } 29 | 30 | public boolean isWrongMechanism() { 31 | return !MECHANISMS.contains(mechanism); 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/FollowTrajectoryCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import org.frcteam2910.c2022.subsystems.DrivetrainSubsystem; 5 | import org.frcteam2910.common.control.Trajectory; 6 | 7 | public class FollowTrajectoryCommand extends CommandBase { 8 | private final DrivetrainSubsystem drivetrain; 9 | 10 | private final Trajectory trajectory; 11 | 12 | public FollowTrajectoryCommand(DrivetrainSubsystem drivetrain, Trajectory trajectory) { 13 | this.drivetrain = drivetrain; 14 | this.trajectory = trajectory; 15 | 16 | addRequirements(drivetrain); 17 | } 18 | 19 | @Override 20 | public void initialize() { 21 | drivetrain.getFollower().follow(trajectory); 22 | } 23 | 24 | @Override 25 | public void end(boolean interrupted) { 26 | drivetrain.getFollower().cancel(); 27 | } 28 | 29 | @Override 30 | public boolean isFinished() { 31 | return drivetrain.getFollower().getCurrentTrajectory().isEmpty(); 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/util/DriverReadout.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.util; 2 | 3 | import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; 4 | import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; 5 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 6 | import org.frcteam2910.c2022.Constants; 7 | import org.frcteam2910.c2022.RobotContainer; 8 | 9 | public class DriverReadout { 10 | 11 | public DriverReadout(RobotContainer container) { 12 | ShuffleboardTab tab = Shuffleboard.getTab(Constants.DRIVER_READOUT_TAB_NAME); 13 | 14 | tab.addNumber("Pressure", () -> container.getSuperstructure().getCurrentPressure()).withSize(2, 2) 15 | .withPosition(0, 0).withWidget(BuiltInWidgets.kDial); 16 | tab.add("Autonomous Mode", container.getAutonomousChooser().getModeChooser()).withSize(2, 1).withPosition(2, 0); 17 | tab.add("Climb Mode", container.getClimbChooser().getClimbChooser()).withSize(2, 1).withPosition(0, 2); 18 | tab.addCamera("Camera", "Camera", "http://limelight.local:5800", "http://10.29.10.11:5800").withSize(3, 3) 19 | .withPosition(4, 0); 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /vendordeps/WPILibNewCommands.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "WPILibNewCommands.json", 3 | "name": "WPILib-New-Commands", 4 | "version": "2020.0.0", 5 | "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", 6 | "mavenUrls": [], 7 | "jsonUrl": "", 8 | "javaDependencies": [ 9 | { 10 | "groupId": "edu.wpi.first.wpilibNewCommands", 11 | "artifactId": "wpilibNewCommands-java", 12 | "version": "wpilib" 13 | } 14 | ], 15 | "jniDependencies": [], 16 | "cppDependencies": [ 17 | { 18 | "groupId": "edu.wpi.first.wpilibNewCommands", 19 | "artifactId": "wpilibNewCommands-cpp", 20 | "version": "wpilib", 21 | "libName": "wpilibNewCommands", 22 | "headerClassifier": "headers", 23 | "sourcesClassifier": "sources", 24 | "sharedLibrary": true, 25 | "skipInvalidPlatforms": true, 26 | "binaryPlatforms": [ 27 | "linuxathena", 28 | "linuxraspbian", 29 | "linuxaarch64bionic", 30 | "windowsx86-64", 31 | "windowsx86", 32 | "linuxx86-64", 33 | "osxx86-64" 34 | ] 35 | } 36 | ] 37 | } 38 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/ClimberToPointCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import org.frcteam2910.c2022.subsystems.ClimberSubsystem; 5 | 6 | public class ClimberToPointCommand extends CommandBase { 7 | public final ClimberSubsystem climber; 8 | 9 | public double height; 10 | public boolean fast; 11 | public boolean evenFaster; 12 | 13 | public ClimberToPointCommand(ClimberSubsystem climber, double height) { 14 | this(climber, height, true, false); 15 | } 16 | 17 | public ClimberToPointCommand(ClimberSubsystem climber, double height, boolean fast, boolean evenFaster) { 18 | this.climber = climber; 19 | this.height = height; 20 | this.fast = fast; 21 | this.evenFaster = evenFaster; 22 | } 23 | 24 | @Override 25 | public void initialize() { 26 | climber.setTargetHeight(height); 27 | if (evenFaster) { 28 | climber.setEvenFasterMotionConstraints(); 29 | } else { 30 | climber.setFastClimberConstraints(fast); 31 | } 32 | } 33 | 34 | @Override 35 | public boolean isFinished() { 36 | return climber.isAtTargetPosition(); 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /vendordeps/PhotonLib-json-1.0.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "photonlib.json", 3 | "name": "photonlib", 4 | "version": "v2022.1.4", 5 | "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", 6 | "mavenUrls": [ 7 | "https://maven.photonvision.org/repository/internal", 8 | "https://maven.photonvision.org/repository/snapshots" 9 | ], 10 | "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", 11 | "jniDependencies": [], 12 | "cppDependencies": [ 13 | { 14 | "groupId": "org.photonvision", 15 | "artifactId": "PhotonLib-cpp", 16 | "version": "v2022.1.4", 17 | "libName": "Photon", 18 | "headerClassifier": "headers", 19 | "sharedLibrary": true, 20 | "skipInvalidPlatforms": true, 21 | "binaryPlatforms": [ 22 | "windowsx86-64", 23 | "linuxathena", 24 | "linuxx86-64", 25 | "osxx86-64" 26 | ] 27 | } 28 | ], 29 | "javaDependencies": [ 30 | { 31 | "groupId": "org.photonvision", 32 | "artifactId": "PhotonLib-java", 33 | "version": "v2022.1.4" 34 | }, 35 | { 36 | "groupId": "org.photonvision", 37 | "artifactId": "PhotonTargeting-java", 38 | "version": "v2022.1.4" 39 | } 40 | ] 41 | } 42 | -------------------------------------------------------------------------------- /settings.gradle: -------------------------------------------------------------------------------- 1 | import org.gradle.internal.os.OperatingSystem 2 | 3 | pluginManagement { 4 | plugins { 5 | id "edu.wpi.first.GradleRIO" version "2022.4.1" 6 | id "net.ltgt.errorprone" version "2.0.2" 7 | id "com.github.johnrengelman.shadow" version "7.1.2" 8 | id "com.diffplug.spotless" version "6.1.0" 9 | } 10 | repositories { 11 | mavenLocal() 12 | gradlePluginPortal() 13 | String frcYear = '2022' 14 | File frcHome 15 | if (OperatingSystem.current().isWindows()) { 16 | String publicFolder = System.getenv('PUBLIC') 17 | if (publicFolder == null) { 18 | publicFolder = "C:\\Users\\Public" 19 | } 20 | def homeRoot = new File(publicFolder, "wpilib") 21 | frcHome = new File(homeRoot, frcYear) 22 | } else { 23 | def userFolder = System.getProperty("user.home") 24 | def homeRoot = new File(userFolder, "wpilib") 25 | frcHome = new File(homeRoot, frcYear) 26 | } 27 | def frcHomeMaven = new File(frcHome, 'maven') 28 | maven { 29 | name 'frcHome' 30 | url frcHomeMaven 31 | } 32 | } 33 | } 34 | 35 | rootProject.name = "2022CompetitionRobot" 36 | 37 | include(":common", ":common:robot") -------------------------------------------------------------------------------- /.github/workflows/master.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | build: 11 | # The type of runner that the job will run on 12 | runs-on: ubuntu-latest 13 | # This grabs the WPILib docker container 14 | container: wpilib/roborio-cross-ubuntu:2022-18.04 15 | # Steps represent a sequence of tasks that will be executed as part of the job 16 | steps: 17 | # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it 18 | - uses: actions/checkout@v2 19 | with: 20 | submodules: 'recursive' 21 | # Grant execute permission for gradlew 22 | - name: Grant execute permission for gradlew 23 | run: chmod +x gradlew 24 | # Runs a single command using the runners shell 25 | - name: Compile and run tests on robot code 26 | run: ./gradlew build 27 | spotless: 28 | runs-on: ubuntu-latest 29 | container: wpilib/roborio-cross-ubuntu:2022-18.04 30 | steps: 31 | - uses: actions/checkout@v2 32 | with: 33 | submodules: 'recursive' 34 | - name: Grant execute permission for gradlew 35 | run: chmod +x gradlew 36 | - name: Compile and run tests on robot code 37 | run: ./gradlew spotlessCheck 38 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/SetHoodAngleCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.CommandBase; 4 | import org.frcteam2910.c2022.subsystems.ShooterSubsystem; 5 | 6 | public class SetHoodAngleCommand extends CommandBase { 7 | private final ShooterSubsystem shooter; 8 | 9 | private final double targetAngle; 10 | private final boolean climbing; 11 | private final boolean fast; 12 | 13 | public SetHoodAngleCommand(ShooterSubsystem shooter, double targetAngle) { 14 | this(shooter, targetAngle, true, false); 15 | } 16 | 17 | public SetHoodAngleCommand(ShooterSubsystem shooter, double targetAngle, boolean fast) { 18 | this(shooter, targetAngle, fast, false); 19 | } 20 | 21 | public SetHoodAngleCommand(ShooterSubsystem shooter, double targetAngle, boolean fast, boolean climbing) { 22 | this.shooter = shooter; 23 | this.targetAngle = targetAngle; 24 | this.climbing = climbing; 25 | this.fast = fast; 26 | 27 | addRequirements(shooter); 28 | } 29 | 30 | @Override 31 | public void initialize() { 32 | shooter.setFastHoodConfig(fast); 33 | shooter.setHoodTargetPosition(targetAngle); 34 | } 35 | 36 | @Override 37 | public boolean isFinished() { 38 | return shooter.isHoodAtTargetAngle(climbing); 39 | } 40 | } -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/ShootWhenReadyCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | import org.frcteam2910.c2022.subsystems.FeederSubsystem; 6 | import org.frcteam2910.c2022.subsystems.ShooterSubsystem; 7 | import org.frcteam2910.c2022.subsystems.VisionSubsystem; 8 | 9 | public class ShootWhenReadyCommand extends CommandBase { 10 | private static final double FEEDER_SPEED = 1.0; 11 | 12 | private final FeederSubsystem feeder; 13 | private final ShooterSubsystem shooter; 14 | private final VisionSubsystem vision; 15 | 16 | private final Timer timer = new Timer(); 17 | 18 | public ShootWhenReadyCommand(FeederSubsystem feeder, ShooterSubsystem shooter, VisionSubsystem vision) { 19 | this.feeder = feeder; 20 | this.shooter = shooter; 21 | this.vision = vision; 22 | 23 | addRequirements(feeder); 24 | } 25 | 26 | @Override 27 | public void initialize() { 28 | timer.start(); 29 | timer.reset(); 30 | } 31 | 32 | @Override 33 | public void execute() { 34 | if (vision.isOnTarget() & shooter.isFlywheelAtTargetSpeed() & shooter.isHoodAtTargetAngle()) { 35 | feeder.setFeederSpeed(FEEDER_SPEED); 36 | timer.reset(); 37 | } else { 38 | feeder.setFeederSpeed(0.0); 39 | } 40 | } 41 | 42 | @Override 43 | public void end(boolean interrupted) { 44 | shooter.setTargetFlywheelSpeed(0.0); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/SimpleIntakeCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj.GenericHID; 4 | import edu.wpi.first.wpilibj.XboxController; 5 | import edu.wpi.first.wpilibj2.command.CommandBase; 6 | import org.frcteam2910.c2022.subsystems.FeederSubsystem; 7 | import org.frcteam2910.c2022.subsystems.IntakeSubsystem; 8 | 9 | public class SimpleIntakeCommand extends CommandBase { 10 | private static final double INTAKE_SPEED = 1.0; 11 | 12 | private final IntakeSubsystem intake; 13 | private final FeederSubsystem feeder; 14 | 15 | private final XboxController controller; 16 | 17 | public SimpleIntakeCommand(IntakeSubsystem intake, FeederSubsystem feeder, XboxController controller) { 18 | this.intake = intake; 19 | this.feeder = feeder; 20 | this.controller = controller; 21 | 22 | addRequirements(intake); 23 | } 24 | 25 | @Override 26 | public void initialize() { 27 | intake.setExtended(true); 28 | } 29 | 30 | @Override 31 | public void execute() { 32 | intake.setIntakeSpeed(INTAKE_SPEED); 33 | if (feeder.isBallAtEntry()) { 34 | controller.setRumble(GenericHID.RumbleType.kLeftRumble, 1.0); 35 | } else { 36 | controller.setRumble(GenericHID.RumbleType.kLeftRumble, 0.0); 37 | } 38 | } 39 | 40 | @Override 41 | public void end(boolean interrupted) { 42 | intake.setIntakeSpeed(0.0); 43 | controller.setRumble(GenericHID.RumbleType.kLeftRumble, 0.0); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/FenderShootCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.math.util.Units; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | import org.frcteam2910.c2022.subsystems.FeederSubsystem; 6 | import org.frcteam2910.c2022.subsystems.ShooterSubsystem; 7 | 8 | public class FenderShootCommand extends CommandBase { 9 | private final static double SHOOTER_ANGLE = Math.toRadians(0.0); 10 | private final static double FLYWHEEL_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(1800); 11 | private final static double FEEDER_SPEED = 1.0; 12 | 13 | private final ShooterSubsystem shooter; 14 | private final FeederSubsystem feeder; 15 | 16 | public FenderShootCommand(FeederSubsystem feeder, ShooterSubsystem shooter) { 17 | this.shooter = shooter; 18 | this.feeder = feeder; 19 | 20 | addRequirements(feeder, shooter); 21 | } 22 | 23 | @Override 24 | public void initialize() { 25 | shooter.setHoodTargetPosition(SHOOTER_ANGLE); 26 | shooter.setTargetFlywheelSpeed(FLYWHEEL_SPEED); 27 | } 28 | 29 | @Override 30 | public void execute() { 31 | // To aim when bumpers are flat against the hub wall 32 | if (shooter.isHoodAtTargetAngle() && shooter.isFlywheelAtTargetSpeed()) { 33 | feeder.setFeederSpeed(FEEDER_SPEED); 34 | } else { 35 | feeder.setFeederSpeed(0.0); 36 | } 37 | } 38 | 39 | @Override 40 | public void end(boolean interrupted) { 41 | feeder.setFeederSpeed(0.0); 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/CharacterizeDrivetrainCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.math.geometry.Pose2d; 4 | import edu.wpi.first.math.kinematics.ChassisSpeeds; 5 | import edu.wpi.first.wpilibj2.command.CommandBase; 6 | import org.frcteam2910.c2022.subsystems.DrivetrainSubsystem; 7 | import org.frcteam2910.c2022.util.SysIdGeneralMechanismLogger; 8 | 9 | public class CharacterizeDrivetrainCommand extends CommandBase { 10 | private final DrivetrainSubsystem drivetrain; 11 | 12 | private final SysIdGeneralMechanismLogger logger = new SysIdGeneralMechanismLogger(); 13 | 14 | public CharacterizeDrivetrainCommand(DrivetrainSubsystem drivetrain) { 15 | this.drivetrain = drivetrain; 16 | addRequirements(drivetrain); 17 | } 18 | 19 | @Override 20 | public void initialize() { 21 | drivetrain.setPose(new Pose2d()); // Reset the drivetrain position 22 | 23 | logger.initLogging(); 24 | } 25 | 26 | @Override 27 | public void execute() { 28 | // Update the data 29 | double position = drivetrain.getPose().getX(); 30 | double velocity = drivetrain.getCurrentVelocity().vxMetersPerSecond; 31 | logger.log(position, velocity); 32 | 33 | // Get the voltage 34 | double voltage = logger.getMotorVoltage(); 35 | 36 | // Drive at new voltage 37 | drivetrain.drive( 38 | new ChassisSpeeds(voltage / 12.0 * DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND, 0.0, 0.0)); 39 | } 40 | 41 | @Override 42 | public void end(boolean interrupted) { 43 | logger.sendData(); 44 | logger.reset(); 45 | drivetrain.drive(new ChassisSpeeds(0.0, 0.0, 0.0)); 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/ResetFeederCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.math.util.Units; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | import org.frcteam2910.c2022.subsystems.FeederSubsystem; 6 | import org.frcteam2910.c2022.subsystems.IntakeSubsystem; 7 | 8 | public class ResetFeederCommand extends CommandBase { 9 | private static final double REVERSE_FEEDER_SPEED = -1.0; 10 | private static final double ONE_BALL_EJECTION_DISTANCE = Units.inchesToMeters(14.0); 11 | private static final double TWO_BALL_EJECTION_DISTANCE = Units.inchesToMeters(6.0); 12 | 13 | private final FeederSubsystem feeder; 14 | private final IntakeSubsystem intake; 15 | 16 | private double feederStartingPosition = 0.0; 17 | private double ejectionDistance = 0.0; 18 | 19 | public ResetFeederCommand(FeederSubsystem feeder, IntakeSubsystem intake) { 20 | this.feeder = feeder; 21 | this.intake = intake; 22 | 23 | addRequirements(feeder); 24 | addRequirements(intake); 25 | } 26 | 27 | @Override 28 | public void initialize() { 29 | if (feeder.isFull()) { 30 | ejectionDistance = TWO_BALL_EJECTION_DISTANCE; 31 | } else { 32 | ejectionDistance = ONE_BALL_EJECTION_DISTANCE; 33 | } 34 | feederStartingPosition = feeder.getPosition(); 35 | } 36 | 37 | @Override 38 | public void execute() { 39 | feeder.setFeederSpeed(REVERSE_FEEDER_SPEED); 40 | intake.setIntakeSpeed(-1.0); 41 | } 42 | 43 | @Override 44 | public boolean isFinished() { 45 | return Math.abs(feeder.getPosition() - feederStartingPosition) > ejectionDistance; 46 | } 47 | 48 | @Override 49 | public void end(boolean interrupted) { 50 | feeder.setFeederSpeed(0.0); 51 | intake.setIntakeSpeed(0.0); 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/DefaultDriveCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import java.util.function.DoubleSupplier; 4 | 5 | import edu.wpi.first.math.kinematics.ChassisSpeeds; 6 | import edu.wpi.first.wpilibj2.command.CommandBase; 7 | import org.frcteam2910.c2022.subsystems.DrivetrainSubsystem; 8 | 9 | public class DefaultDriveCommand extends CommandBase { 10 | private final DrivetrainSubsystem m_drivetrainSubsystem; 11 | 12 | private final DoubleSupplier m_translationXSupplier; 13 | private final DoubleSupplier m_translationYSupplier; 14 | private final DoubleSupplier m_rotationSupplier; 15 | 16 | public DefaultDriveCommand(DrivetrainSubsystem drivetrainSubsystem, DoubleSupplier translationXSupplier, 17 | DoubleSupplier translationYSupplier, DoubleSupplier rotationSupplier) { 18 | this.m_drivetrainSubsystem = drivetrainSubsystem; 19 | this.m_translationXSupplier = translationXSupplier; 20 | this.m_translationYSupplier = translationYSupplier; 21 | this.m_rotationSupplier = rotationSupplier; 22 | 23 | addRequirements(drivetrainSubsystem); 24 | } 25 | 26 | @Override 27 | public void execute() { 28 | // You can use `new ChassisSpeeds(...)` for robot-oriented movement instead of 29 | // field-oriented movement 30 | m_drivetrainSubsystem.drive(ChassisSpeeds.fromFieldRelativeSpeeds( 31 | m_translationXSupplier.getAsDouble() * m_drivetrainSubsystem.getMotorOutputLimiter(), 32 | m_translationYSupplier.getAsDouble() * m_drivetrainSubsystem.getMotorOutputLimiter(), 33 | m_rotationSupplier.getAsDouble() * m_drivetrainSubsystem.getMotorOutputLimiter(), 34 | m_drivetrainSubsystem.getPose().getRotation())); 35 | } 36 | 37 | @Override 38 | public void end(boolean interrupted) { 39 | m_drivetrainSubsystem.drive(new ChassisSpeeds(0.0, 0.0, 0.0)); 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/ZeroClimberCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.math.util.Units; 4 | import edu.wpi.first.wpilibj.Timer; 5 | import edu.wpi.first.wpilibj2.command.CommandBase; 6 | import org.frcteam2910.c2022.subsystems.ClimberSubsystem; 7 | 8 | public class ZeroClimberCommand extends CommandBase { 9 | private static final double CLIMBER_ZERO_VELOCITY_TIME_PERIOD = 0.5; 10 | private static final double REVERSE_VOLTAGE = -0.75; 11 | private static final double VELOCITY_THRESHOLD = Units.inchesToMeters(0.1); 12 | 13 | private final ClimberSubsystem climber; 14 | 15 | private double zeroVelocityTimestamp; 16 | 17 | public ZeroClimberCommand(ClimberSubsystem climber) { 18 | this.climber = climber; 19 | 20 | addRequirements(climber); 21 | } 22 | 23 | @Override 24 | public void initialize() { 25 | zeroVelocityTimestamp = Double.NaN; 26 | climber.setZeroed(false); 27 | } 28 | 29 | @Override 30 | public void execute() { 31 | climber.setTargetVoltage(REVERSE_VOLTAGE); 32 | if (Math.abs(climber.getCurrentVelocity()) < VELOCITY_THRESHOLD) { 33 | if (!Double.isFinite(zeroVelocityTimestamp)) { 34 | zeroVelocityTimestamp = Timer.getFPGATimestamp(); 35 | } 36 | } else { 37 | zeroVelocityTimestamp = Double.NaN; 38 | } 39 | } 40 | 41 | @Override 42 | public boolean isFinished() { 43 | if (Double.isFinite(zeroVelocityTimestamp)) { 44 | return Timer.getFPGATimestamp() - zeroVelocityTimestamp >= CLIMBER_ZERO_VELOCITY_TIME_PERIOD; 45 | } 46 | return false; 47 | } 48 | 49 | @Override 50 | public void end(boolean interrupted) { 51 | climber.setTargetVoltage(0.0); 52 | if (!interrupted) { 53 | climber.setZeroPosition(); 54 | climber.setTargetHeight(0); 55 | climber.setZeroed(true); 56 | } 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/Robot.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022; 2 | 3 | import edu.wpi.first.wpilibj.TimedRobot; 4 | import edu.wpi.first.wpilibj2.command.CommandScheduler; 5 | import edu.wpi.first.wpilibj2.command.InstantCommand; 6 | import org.frcteam2910.c2022.commands.CharacterizeDrivetrainCommand; 7 | import org.frcteam2910.c2022.commands.ZeroClimberCommand; 8 | import org.frcteam2910.c2022.commands.ZeroHoodCommand; 9 | import org.frcteam2910.c2022.util.DriverReadout; 10 | 11 | public class Robot extends TimedRobot { 12 | private final RobotContainer robotContainer = new RobotContainer(); 13 | 14 | private final CharacterizeDrivetrainCommand characterizeCommand = new CharacterizeDrivetrainCommand( 15 | robotContainer.getDrivetrain()); 16 | 17 | @SuppressWarnings("unused") 18 | private final DriverReadout driverReadout = new DriverReadout(robotContainer); 19 | 20 | @Override 21 | public void robotPeriodic() { 22 | CommandScheduler.getInstance().run(); 23 | } 24 | 25 | @Override 26 | public void disabledInit() { 27 | robotContainer.getShooter().setHoodBrakeMode(false); 28 | } 29 | 30 | @Override 31 | public void disabledExit() { 32 | robotContainer.getShooter().setHoodBrakeMode(false); 33 | } 34 | 35 | @Override 36 | public void teleopInit() { 37 | if (!robotContainer.getClimber().isClimberZeroed()) { 38 | new ZeroClimberCommand(robotContainer.getClimber()).schedule(); 39 | } 40 | if (!robotContainer.getShooter().isHoodZeroed()) { 41 | new ZeroHoodCommand(robotContainer.getShooter(), true).schedule(); 42 | } 43 | } 44 | 45 | @Override 46 | public void testInit() { 47 | new InstantCommand(robotContainer.getShooter()::disableFlywheel); 48 | } 49 | 50 | @Override 51 | public void autonomousInit() { 52 | robotContainer.getAutonomousChooser().getCommand(robotContainer).schedule(); 53 | // characterizeCommand.schedule(); 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/subsystems/IntakeSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.subsystems; 2 | 3 | import com.ctre.phoenix.motorcontrol.StatusFrame; 4 | import com.ctre.phoenix.motorcontrol.TalonFXControlMode; 5 | import com.ctre.phoenix.motorcontrol.can.TalonFX; 6 | import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; 7 | import edu.wpi.first.wpilibj.PneumaticsModuleType; 8 | import edu.wpi.first.wpilibj.Solenoid; 9 | import edu.wpi.first.wpilibj2.command.Subsystem; 10 | import org.frcteam2910.c2022.Constants; 11 | 12 | public class IntakeSubsystem implements Subsystem { 13 | private final TalonFX leftMotor = new TalonFX(Constants.INTAKE_LEFT_MOTOR_PORT); 14 | private final TalonFX rightMotor = new TalonFX(Constants.INTAKE_RIGHT_MOTOR_PORT); 15 | private final Solenoid extensionSolenoid = new Solenoid(PneumaticsModuleType.REVPH, Constants.INTAKE_SOLENOID_PORT); 16 | 17 | private double motorSpeed = 0.0; 18 | private boolean extended = false; 19 | 20 | public IntakeSubsystem() { 21 | TalonFXConfiguration configuration = new TalonFXConfiguration(); 22 | configuration.supplyCurrLimit.currentLimit = 30; 23 | configuration.supplyCurrLimit.enable = true; 24 | 25 | rightMotor.configAllSettings(configuration); 26 | leftMotor.configAllSettings(configuration); 27 | 28 | rightMotor.setStatusFramePeriod(StatusFrame.Status_1_General, 255); 29 | rightMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 255); 30 | leftMotor.setStatusFramePeriod(StatusFrame.Status_1_General, 255); 31 | leftMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 255); 32 | rightMotor.setInverted(true); 33 | } 34 | 35 | public void setIntakeSpeed(double motorSpeed) { 36 | this.motorSpeed = motorSpeed; 37 | } 38 | 39 | public void setExtended(boolean extended) { 40 | this.extended = extended; 41 | } 42 | 43 | @Override 44 | public void periodic() { 45 | leftMotor.set(TalonFXControlMode.PercentOutput, motorSpeed); 46 | rightMotor.set(TalonFXControlMode.PercentOutput, motorSpeed); 47 | extensionSolenoid.set(extended); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /vendordeps/REVLib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "REVLib.json", 3 | "name": "REVLib", 4 | "version": "2022.1.1", 5 | "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", 6 | "mavenUrls": [ 7 | "https://maven.revrobotics.com/" 8 | ], 9 | "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.revrobotics.frc", 13 | "artifactId": "REVLib-java", 14 | "version": "2022.1.1" 15 | } 16 | ], 17 | "jniDependencies": [ 18 | { 19 | "groupId": "com.revrobotics.frc", 20 | "artifactId": "REVLib-driver", 21 | "version": "2022.1.1", 22 | "skipInvalidPlatforms": true, 23 | "isJar": false, 24 | "validPlatforms": [ 25 | "windowsx86-64", 26 | "windowsx86", 27 | "linuxaarch64bionic", 28 | "linuxx86-64", 29 | "linuxathena", 30 | "linuxraspbian", 31 | "osxx86-64" 32 | ] 33 | } 34 | ], 35 | "cppDependencies": [ 36 | { 37 | "groupId": "com.revrobotics.frc", 38 | "artifactId": "REVLib-cpp", 39 | "version": "2022.1.1", 40 | "libName": "REVLib", 41 | "headerClassifier": "headers", 42 | "sharedLibrary": false, 43 | "skipInvalidPlatforms": true, 44 | "binaryPlatforms": [ 45 | "windowsx86-64", 46 | "windowsx86", 47 | "linuxaarch64bionic", 48 | "linuxx86-64", 49 | "linuxathena", 50 | "linuxraspbian", 51 | "osxx86-64" 52 | ] 53 | }, 54 | { 55 | "groupId": "com.revrobotics.frc", 56 | "artifactId": "REVLib-driver", 57 | "version": "2022.1.1", 58 | "libName": "REVLibDriver", 59 | "headerClassifier": "headers", 60 | "sharedLibrary": false, 61 | "skipInvalidPlatforms": true, 62 | "binaryPlatforms": [ 63 | "windowsx86-64", 64 | "windowsx86", 65 | "linuxaarch64bionic", 66 | "linuxx86-64", 67 | "linuxathena", 68 | "linuxraspbian", 69 | "osxx86-64" 70 | ] 71 | } 72 | ] 73 | } 74 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/subsystems/FeederSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.subsystems; 2 | 3 | import com.ctre.phoenix.motorcontrol.NeutralMode; 4 | import com.ctre.phoenix.motorcontrol.StatusFrame; 5 | import com.ctre.phoenix.motorcontrol.TalonFXControlMode; 6 | import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; 7 | import com.ctre.phoenix.motorcontrol.can.TalonFX; 8 | import edu.wpi.first.math.util.Units; 9 | import edu.wpi.first.wpilibj.DigitalInput; 10 | import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; 11 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 12 | import edu.wpi.first.wpilibj2.command.Subsystem; 13 | import org.frcteam2910.c2022.Constants; 14 | 15 | public class FeederSubsystem implements Subsystem { 16 | private final static double FEEDER_GEAR_RATIO = 1.0 / 4.0; 17 | private final static double FEEDER_PULLEY_RADIUS = Units.inchesToMeters(0.5); 18 | 19 | private final static double BALL_TRAVEL_COEFFICIENT = 0.5; 20 | private final static double SENSOR_POSITION_COEFFICIENT = (1.0 / 2048.0) * FEEDER_GEAR_RATIO * 2.0 21 | * FEEDER_PULLEY_RADIUS * Math.PI * BALL_TRAVEL_COEFFICIENT; 22 | 23 | private final TalonFX motor = new TalonFX(Constants.FEEDER_MOTOR_PORT); 24 | private final DigitalInput fullSensor = new DigitalInput(Constants.FEEDER_SENSOR_FULL_PORT); 25 | private final DigitalInput entrySensor = new DigitalInput(Constants.FEEDER_SENSOR_ENTRY_PORT); 26 | 27 | private double motorSpeed; 28 | 29 | public FeederSubsystem() { 30 | motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 0); 31 | motor.setStatusFramePeriod(StatusFrame.Status_1_General, 255); 32 | motor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 20); 33 | motor.setNeutralMode(NeutralMode.Brake); 34 | 35 | ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Feeder"); 36 | shuffleboardTab.addBoolean("Full Sensor", this::isFull); 37 | shuffleboardTab.addBoolean("Entry Sensor", this::isBallAtEntry); 38 | } 39 | 40 | public void setFeederSpeed(double motorSpeed) { 41 | this.motorSpeed = motorSpeed; 42 | } 43 | 44 | public double getPosition() { 45 | return motor.getSelectedSensorPosition() * SENSOR_POSITION_COEFFICIENT; 46 | } 47 | 48 | public boolean isBallAtEntry() { 49 | return !entrySensor.get(); 50 | } 51 | 52 | public boolean isFull() { 53 | return !fullSensor.get(); 54 | } 55 | 56 | @Override 57 | public void periodic() { 58 | motor.set(TalonFXControlMode.PercentOutput, motorSpeed); 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/AlignRobotToShootCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import java.util.function.DoubleSupplier; 4 | 5 | import edu.wpi.first.math.geometry.Rotation2d; 6 | import edu.wpi.first.math.kinematics.ChassisSpeeds; 7 | import edu.wpi.first.wpilibj2.command.CommandBase; 8 | import org.frcteam2910.c2022.Robot; 9 | import org.frcteam2910.c2022.subsystems.DrivetrainSubsystem; 10 | import org.frcteam2910.c2022.subsystems.VisionSubsystem; 11 | import org.frcteam2910.common.control.PidConstants; 12 | import org.frcteam2910.common.control.PidController; 13 | 14 | public class AlignRobotToShootCommand extends CommandBase { 15 | private static final double ROTATION_STATIC_CONSTANT = 0.3; 16 | 17 | private final DrivetrainSubsystem drivetrain; 18 | private final VisionSubsystem vision; 19 | 20 | private final PidController controller = new PidController(new PidConstants(5.0, 0.0, 0.3)); 21 | 22 | private boolean targetSeen = false; 23 | 24 | private final DoubleSupplier xAxis; 25 | private final DoubleSupplier yAxis; 26 | 27 | public AlignRobotToShootCommand(DrivetrainSubsystem drivetrain, VisionSubsystem vision, DoubleSupplier xAxis, 28 | DoubleSupplier yAxis) { 29 | this.drivetrain = drivetrain; 30 | this.vision = vision; 31 | this.xAxis = xAxis; 32 | this.yAxis = yAxis; 33 | 34 | controller.setInputRange(0, 2 * Math.PI); 35 | controller.setContinuous(true); 36 | 37 | addRequirements(drivetrain); 38 | } 39 | 40 | public AlignRobotToShootCommand(DrivetrainSubsystem drivetrain, VisionSubsystem vision) { 41 | this(drivetrain, vision, () -> 0.0, () -> 0.0); 42 | } 43 | 44 | @Override 45 | public void initialize() { 46 | controller.reset(); 47 | } 48 | 49 | @Override 50 | public void execute() { 51 | if (targetSeen) { 52 | Rotation2d currentAngle = drivetrain.getPose().getRotation(); 53 | 54 | controller.setSetpoint(vision.getAngleToTarget()); 55 | 56 | double rotationalVelocity = controller.calculate(currentAngle.getRadians(), Robot.kDefaultPeriod); 57 | rotationalVelocity += Math.copySign(ROTATION_STATIC_CONSTANT / DrivetrainSubsystem.MAX_VOLTAGE 58 | * DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND, rotationalVelocity); 59 | 60 | drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds(xAxis.getAsDouble(), yAxis.getAsDouble(), 61 | -rotationalVelocity, currentAngle)); 62 | } else { 63 | targetSeen = vision.shooterHasTargets(); 64 | } 65 | } 66 | 67 | @Override 68 | public void end(boolean interrupted) { 69 | drivetrain.drive(new ChassisSpeeds(0.0, 0.0, 0.0)); 70 | } 71 | } -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/ZeroHoodCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | import org.frcteam2910.c2022.subsystems.ShooterSubsystem; 6 | 7 | public class ZeroHoodCommand extends CommandBase { 8 | private static final double ZERO_HOOD_VELOCITY_TIME = 0.25; // in sec 9 | private static final double HOOD_VOLTAGE = 1.5; 10 | private static final double HOOD_ALLOWABLE_ZERO_VELOCITY = Math.toRadians(0.1); 11 | 12 | private final ShooterSubsystem shooter; 13 | 14 | private final boolean forward; 15 | private final boolean flywheel; 16 | 17 | private double zeroHoodStartTime = Double.NaN; 18 | 19 | public ZeroHoodCommand(ShooterSubsystem shooter, boolean forward) { 20 | this(shooter, forward, false); 21 | } 22 | 23 | public ZeroHoodCommand(ShooterSubsystem shooter, boolean forward, boolean flywheel) { 24 | this.shooter = shooter; 25 | this.forward = forward; 26 | this.flywheel = flywheel; 27 | 28 | addRequirements(shooter); 29 | } 30 | 31 | @Override 32 | public void initialize() { 33 | shooter.setHoodZeroed(false); 34 | zeroHoodStartTime = Double.NaN; 35 | 36 | if (forward) { 37 | shooter.setHoodVoltage(HOOD_VOLTAGE); 38 | } else { 39 | shooter.setHoodVoltage(-HOOD_VOLTAGE); 40 | } 41 | 42 | if (flywheel) { 43 | shooter.setTargetFlywheelSpeed(ShooterSubsystem.FLYWHEEL_IDLE_SPEED); 44 | shooter.enableCurrentLimits(true); 45 | } 46 | } 47 | 48 | @Override 49 | public boolean isFinished() { 50 | if (Double.isFinite(zeroHoodStartTime)) { 51 | if (Math.abs(shooter.getHoodVelocity()) > HOOD_ALLOWABLE_ZERO_VELOCITY) { 52 | zeroHoodStartTime = Double.NaN; 53 | } else { 54 | return Timer.getFPGATimestamp() - zeroHoodStartTime >= ZERO_HOOD_VELOCITY_TIME; 55 | } 56 | } else { 57 | if (Math.abs(shooter.getHoodVelocity()) < HOOD_ALLOWABLE_ZERO_VELOCITY) { 58 | zeroHoodStartTime = Timer.getFPGATimestamp(); 59 | } 60 | } 61 | 62 | return false; 63 | } 64 | 65 | @Override 66 | public void end(boolean interrupted) { 67 | shooter.setHoodVoltage(0.0); 68 | if (!interrupted) { 69 | shooter.setHoodZeroed(true); 70 | if (forward) { 71 | shooter.setHoodMotorSensorPosition(ShooterSubsystem.HOOD_MAX_ANGLE); 72 | } else { 73 | shooter.setHoodMotorSensorPosition(ShooterSubsystem.HOOD_MIN_ANGLE - Math.toRadians(0.5)); 74 | } 75 | } 76 | shooter.enableCurrentLimits(false); 77 | } 78 | } -------------------------------------------------------------------------------- /gradlew.bat: -------------------------------------------------------------------------------- 1 | @rem 2 | @rem Copyright 2015 the original author or authors. 3 | @rem 4 | @rem Licensed under the Apache License, Version 2.0 (the "License"); 5 | @rem you may not use this file except in compliance with the License. 6 | @rem You may obtain a copy of the License at 7 | @rem 8 | @rem https://www.apache.org/licenses/LICENSE-2.0 9 | @rem 10 | @rem Unless required by applicable law or agreed to in writing, software 11 | @rem distributed under the License is distributed on an "AS IS" BASIS, 12 | @rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | @rem See the License for the specific language governing permissions and 14 | @rem limitations under the License. 15 | @rem 16 | 17 | @if "%DEBUG%" == "" @echo off 18 | @rem ########################################################################## 19 | @rem 20 | @rem Gradle startup script for Windows 21 | @rem 22 | @rem ########################################################################## 23 | 24 | @rem Set local scope for the variables with windows NT shell 25 | if "%OS%"=="Windows_NT" setlocal 26 | 27 | set DIRNAME=%~dp0 28 | if "%DIRNAME%" == "" set DIRNAME=. 29 | set APP_BASE_NAME=%~n0 30 | set APP_HOME=%DIRNAME% 31 | 32 | @rem Resolve any "." and ".." in APP_HOME to make it shorter. 33 | for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi 34 | 35 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 36 | set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" 37 | 38 | @rem Find java.exe 39 | if defined JAVA_HOME goto findJavaFromJavaHome 40 | 41 | set JAVA_EXE=java.exe 42 | %JAVA_EXE% -version >NUL 2>&1 43 | if "%ERRORLEVEL%" == "0" goto execute 44 | 45 | echo. 46 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 47 | echo. 48 | echo Please set the JAVA_HOME variable in your environment to match the 49 | echo location of your Java installation. 50 | 51 | goto fail 52 | 53 | :findJavaFromJavaHome 54 | set JAVA_HOME=%JAVA_HOME:"=% 55 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 56 | 57 | if exist "%JAVA_EXE%" goto execute 58 | 59 | echo. 60 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 61 | echo. 62 | echo Please set the JAVA_HOME variable in your environment to match the 63 | echo location of your Java installation. 64 | 65 | goto fail 66 | 67 | :execute 68 | @rem Setup the command line 69 | 70 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 71 | 72 | 73 | @rem Execute Gradle 74 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* 75 | 76 | :end 77 | @rem End local scope for the variables with windows NT shell 78 | if "%ERRORLEVEL%"=="0" goto mainEnd 79 | 80 | :fail 81 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 82 | rem the _cmd.exe /c_ return code! 83 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 84 | exit /b 1 85 | 86 | :mainEnd 87 | if "%OS%"=="Windows_NT" endlocal 88 | 89 | :omega 90 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/Constants.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022; 2 | 3 | import edu.wpi.first.math.util.Units; 4 | 5 | public class Constants { 6 | public static final int CONTROLLER_PORT = 0; 7 | 8 | public static final int INTAKE_LEFT_MOTOR_PORT = 12; 9 | public static final int INTAKE_RIGHT_MOTOR_PORT = 13; 10 | public static final int INTAKE_SOLENOID_PORT = 0; 11 | public static final int CLIMBER_FIRST_LEFT_MOTOR_PORT = 16; 12 | public static final int CLIMBER_FIRST_RIGHT_MOTOR_PORT = 11; 13 | public static final int CLIMBER_SECOND_LEFT_MOTOR_PORT = 17; 14 | public static final int CLIMBER_SECOND_RIGHT_MOTOR_PORT = 18; 15 | public static final int FEEDER_MOTOR_PORT = 10; 16 | 17 | public static final int FEEDER_SENSOR_FULL_PORT = 1; 18 | public static final int FEEDER_SENSOR_ENTRY_PORT = 0; 19 | 20 | public static final double DRIVETRAIN_TRACKWIDTH_METERS = Units.inchesToMeters(22); 21 | public static final double DRIVETRAIN_WHEELBASE_METERS = Units.inchesToMeters(22); 22 | public static final int DRIVETRAIN_PIGEON_ID = 1; 23 | 24 | public static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 7; 25 | public static final int FRONT_LEFT_MODULE_STEER_MOTOR = 8; 26 | public static final int FRONT_LEFT_MODULE_STEER_ENCODER = 4; 27 | public static final double FRONT_LEFT_MODULE_STEER_OFFSET = -Math.toRadians(90.87 + 180.0); 28 | 29 | public static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 3; 30 | public static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 4; 31 | public static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 3; 32 | public static final double FRONT_RIGHT_MODULE_STEER_OFFSET = -Math.toRadians(35.59 + 180.0); 33 | 34 | public static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 5; 35 | public static final int BACK_LEFT_MODULE_STEER_MOTOR = 6; 36 | public static final int BACK_LEFT_MODULE_STEER_ENCODER = 2; 37 | public static final double BACK_LEFT_MODULE_STEER_OFFSET = -Math.toRadians(110.21 + 180.0); 38 | 39 | public static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 1; 40 | public static final int BACK_RIGHT_MODULE_STEER_MOTOR = 2; 41 | public static final int BACK_RIGHT_MODULE_STEER_ENCODER = 1; 42 | public static final double BACK_RIGHT_MODULE_STEER_OFFSET = -Math.toRadians(78.13 + 180.0); 43 | 44 | public static final int HOOD_MOTOR_PORT = 14; 45 | public static final int FLYWHEEL_PRIMARY_MOTOR_PORT = 15; 46 | public static final int FLYWHEEL_SECONDARY_MOTOR_PORT = 9; 47 | 48 | public static final double HOOD_MANUAL_ADJUST_INTERVAL = Math.toRadians(0.5); 49 | public static final double FLYWHEEL_MANUAL_ADJUST_INTERVAL = Units.rotationsPerMinuteToRadiansPerSecond(25.0); 50 | 51 | public static final double HOOD_MOTOR_TO_HOOD_GEAR_RATIO = 1; 52 | public static final double HOOD_SHOOTING_ALLOWABLE_ERROR = Math.toRadians(0.5); 53 | public static final double HOOD_CLIMBING_ALLOWABLE_ERROR = Math.toRadians(1.0); 54 | 55 | public static final String SHOOTER_OFFSET_ENTRY_NAME = "Shooting Offset"; 56 | public static final String HOOD_OFFSET_ENTRY_NAME = "Hood Offset"; 57 | public static final String DRIVER_READOUT_TAB_NAME = "Driver Readout"; 58 | } 59 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/util/SysIdLogger.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.util; 2 | 3 | import java.util.ArrayList; 4 | import java.util.List; 5 | 6 | import edu.wpi.first.wpilibj.Timer; 7 | import edu.wpi.first.wpilibj.livewindow.LiveWindow; 8 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 9 | 10 | public abstract class SysIdLogger { 11 | protected static final int DATA_BUFFER_SIZE = 36000; 12 | 13 | private static final int THREAD_PRIORITY = 15; 14 | private static final int HAL_THREAD_PRIORITY = 40; 15 | 16 | protected double voltageCommand = 0.0; 17 | protected double motorVoltage = 0.0; 18 | protected double timestamp = 0.0; 19 | protected double startTime = 0.0; 20 | protected boolean rotate = false; 21 | protected String testType = ""; 22 | protected String mechanism = ""; 23 | protected final List data = new ArrayList<>(DATA_BUFFER_SIZE); 24 | 25 | public SysIdLogger() { 26 | System.out.println("Initializing logger"); 27 | LiveWindow.disableAllTelemetry(); 28 | SmartDashboard.putNumber("SysIdVoltageCommand", 0.0); 29 | SmartDashboard.putString("SysIdTestType", ""); 30 | SmartDashboard.putString("SysIdTest", ""); 31 | SmartDashboard.putBoolean("SysIdRotate", false); 32 | SmartDashboard.putBoolean("SysIdOverflow", false); 33 | SmartDashboard.putBoolean("SysIdWrongMech", false); 34 | } 35 | 36 | public void initLogging() { 37 | mechanism = SmartDashboard.getString("SysIdTest", ""); 38 | 39 | SmartDashboard.putBoolean("SysIdWrongMech", isWrongMechanism()); 40 | 41 | testType = SmartDashboard.getString("SysIdTestType", ""); 42 | rotate = SmartDashboard.getBoolean("SysIdRotate", false); 43 | voltageCommand = SmartDashboard.getNumber("SysIdVoltageCommand", 0.0); 44 | startTime = Timer.getFPGATimestamp(); 45 | data.clear(); 46 | } 47 | 48 | public void sendData() { 49 | System.out.printf("Collected: %d data points.%n", data.size()); 50 | 51 | SmartDashboard.putBoolean("SysIdOverflow", data.size() >= DATA_BUFFER_SIZE); 52 | 53 | StringBuilder sb = new StringBuilder(); 54 | for (int i = 0; i < data.size(); ++i) { 55 | sb.append(data.get(i)); 56 | if (i < data.size() - 1) { 57 | sb.append(','); 58 | } 59 | } 60 | 61 | SmartDashboard.putString("SysIdTelemetry", sb.toString()); 62 | 63 | reset(); 64 | } 65 | 66 | protected void updateData() { 67 | timestamp = Timer.getFPGATimestamp(); 68 | 69 | // Don't let robot move if it's characterizing the wrong mechanism 70 | if (!isWrongMechanism()) { 71 | if (testType.equals("Quasistatic")) { 72 | motorVoltage = voltageCommand * (timestamp - startTime); 73 | } else if (testType.equals("Dynamic")) { 74 | motorVoltage = voltageCommand; 75 | } else { 76 | motorVoltage = 0.0; 77 | } 78 | } else { 79 | motorVoltage = 0.0; 80 | } 81 | } 82 | 83 | protected void reset() { 84 | motorVoltage = 0.0; 85 | timestamp = 0.0; 86 | startTime = 0.0; 87 | data.clear(); 88 | } 89 | 90 | protected boolean isWrongMechanism() { 91 | return false; 92 | } 93 | } 94 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/TargetWithShooterCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.math.util.Units; 4 | import edu.wpi.first.wpilibj2.command.CommandBase; 5 | import org.frcteam2910.c2022.subsystems.ShooterSubsystem; 6 | import org.frcteam2910.c2022.subsystems.VisionSubsystem; 7 | import org.frcteam2910.common.math.Vector2; 8 | import org.frcteam2910.common.util.InterpolatingDouble; 9 | import org.frcteam2910.common.util.InterpolatingTreeMap; 10 | 11 | public class TargetWithShooterCommand extends CommandBase { 12 | private final ShooterSubsystem shooter; 13 | private final VisionSubsystem vision; 14 | 15 | private static final InterpolatingTreeMap SHOOTER_TUNING = new InterpolatingTreeMap<>(); 16 | 17 | static { 18 | // Old Limelight Tuning 19 | // SHOOTER_TUNING.put(new InterpolatingDouble(1.0), 20 | // new Vector2(Math.toRadians(6.0), 21 | // Units.rotationsPerMinuteToRadiansPerSecond(1725))); 22 | // SHOOTER_TUNING.put(new InterpolatingDouble(1.5), 23 | // new Vector2(Math.toRadians(8.5), 24 | // Units.rotationsPerMinuteToRadiansPerSecond(1825))); 25 | // SHOOTER_TUNING.put(new InterpolatingDouble(2.0), 26 | // new Vector2(Math.toRadians(11.0), 27 | // Units.rotationsPerMinuteToRadiansPerSecond(1900))); 28 | // SHOOTER_TUNING.put(new InterpolatingDouble(2.5), 29 | // new Vector2(Math.toRadians(12.5), 30 | // Units.rotationsPerMinuteToRadiansPerSecond(1950))); 31 | // SHOOTER_TUNING.put(new InterpolatingDouble(3.0), 32 | // new Vector2(Math.toRadians(14.5), 33 | // Units.rotationsPerMinuteToRadiansPerSecond(2025))); 34 | // SHOOTER_TUNING.put(new InterpolatingDouble(3.5), 35 | // new Vector2(Math.toRadians(16.5), 36 | // Units.rotationsPerMinuteToRadiansPerSecond(2100))); 37 | // SHOOTER_TUNING.put(new InterpolatingDouble(4.0), 38 | // new Vector2(Math.toRadians(19.0), 39 | // Units.rotationsPerMinuteToRadiansPerSecond(2200))); 40 | // SHOOTER_TUNING.put(new InterpolatingDouble(4.5), 41 | // new Vector2(Math.toRadians(22.0), 42 | // Units.rotationsPerMinuteToRadiansPerSecond(2250))); 43 | // SHOOTER_TUNING.put(new InterpolatingDouble(5.0), 44 | // new Vector2(Math.toRadians(24.0), 45 | // Units.rotationsPerMinuteToRadiansPerSecond(2325))); 46 | // SHOOTER_TUNING.put(new InterpolatingDouble(5.5), 47 | // new Vector2(Math.toRadians(25.5), 48 | // Units.rotationsPerMinuteToRadiansPerSecond(2425))); 49 | // SHOOTER_TUNING.put(new InterpolatingDouble(6.0), 50 | // new Vector2(Math.toRadians(27.5), 51 | // Units.rotationsPerMinuteToRadiansPerSecond(2575))); 52 | 53 | SHOOTER_TUNING.put(new InterpolatingDouble(1.0), 54 | new Vector2(Math.toRadians(7.0), Units.rotationsPerMinuteToRadiansPerSecond(1725))); 55 | SHOOTER_TUNING.put(new InterpolatingDouble(1.5), 56 | new Vector2(Math.toRadians(9.5), Units.rotationsPerMinuteToRadiansPerSecond(1725))); 57 | SHOOTER_TUNING.put(new InterpolatingDouble(2.0), 58 | new Vector2(Math.toRadians(11.5), Units.rotationsPerMinuteToRadiansPerSecond(1750))); 59 | SHOOTER_TUNING.put(new InterpolatingDouble(2.5), 60 | new Vector2(Math.toRadians(13.5), Units.rotationsPerMinuteToRadiansPerSecond(1825))); 61 | SHOOTER_TUNING.put(new InterpolatingDouble(3.0), 62 | new Vector2(Math.toRadians(16.0), Units.rotationsPerMinuteToRadiansPerSecond(1900))); 63 | SHOOTER_TUNING.put(new InterpolatingDouble(3.5), 64 | new Vector2(Math.toRadians(18.5), Units.rotationsPerMinuteToRadiansPerSecond(2000))); 65 | SHOOTER_TUNING.put(new InterpolatingDouble(4.0), 66 | new Vector2(Math.toRadians(21.5), Units.rotationsPerMinuteToRadiansPerSecond(2100))); 67 | SHOOTER_TUNING.put(new InterpolatingDouble(4.5), 68 | new Vector2(Math.toRadians(24.0), Units.rotationsPerMinuteToRadiansPerSecond(2175))); 69 | SHOOTER_TUNING.put(new InterpolatingDouble(5.0), 70 | new Vector2(Math.toRadians(25.5), Units.rotationsPerMinuteToRadiansPerSecond(2275))); 71 | SHOOTER_TUNING.put(new InterpolatingDouble(5.5), 72 | new Vector2(Math.toRadians(27.5), Units.rotationsPerMinuteToRadiansPerSecond(2400))); 73 | SHOOTER_TUNING.put(new InterpolatingDouble(6.0), 74 | new Vector2(Math.toRadians(30.0), Units.rotationsPerMinuteToRadiansPerSecond(2475))); 75 | 76 | } 77 | public TargetWithShooterCommand(ShooterSubsystem shooter, VisionSubsystem vision) { 78 | this.shooter = shooter; 79 | this.vision = vision; 80 | 81 | addRequirements(shooter); 82 | } 83 | 84 | @Override 85 | public void execute() { 86 | double distance = vision.getDistanceToTarget(); 87 | Vector2 angleAndSpeed = SHOOTER_TUNING.getInterpolated(new InterpolatingDouble(distance)); 88 | 89 | shooter.setTargetFlywheelSpeed(angleAndSpeed.y); 90 | shooter.setHoodTargetPosition(angleAndSpeed.x, true); 91 | } 92 | } 93 | -------------------------------------------------------------------------------- /gradlew: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | 3 | # 4 | # Copyright 2015 the original author or authors. 5 | # 6 | # Licensed under the Apache License, Version 2.0 (the "License"); 7 | # you may not use this file except in compliance with the License. 8 | # You may obtain a copy of the License at 9 | # 10 | # https://www.apache.org/licenses/LICENSE-2.0 11 | # 12 | # Unless required by applicable law or agreed to in writing, software 13 | # distributed under the License is distributed on an "AS IS" BASIS, 14 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | # See the License for the specific language governing permissions and 16 | # limitations under the License. 17 | # 18 | 19 | ############################################################################## 20 | ## 21 | ## Gradle start up script for UN*X 22 | ## 23 | ############################################################################## 24 | 25 | # Attempt to set APP_HOME 26 | # Resolve links: $0 may be a link 27 | PRG="$0" 28 | # Need this for relative symlinks. 29 | while [ -h "$PRG" ] ; do 30 | ls=`ls -ld "$PRG"` 31 | link=`expr "$ls" : '.*-> \(.*\)$'` 32 | if expr "$link" : '/.*' > /dev/null; then 33 | PRG="$link" 34 | else 35 | PRG=`dirname "$PRG"`"/$link" 36 | fi 37 | done 38 | SAVED="`pwd`" 39 | cd "`dirname \"$PRG\"`/" >/dev/null 40 | APP_HOME="`pwd -P`" 41 | cd "$SAVED" >/dev/null 42 | 43 | APP_NAME="Gradle" 44 | APP_BASE_NAME=`basename "$0"` 45 | 46 | # Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 47 | DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' 48 | 49 | # Use the maximum available, or set MAX_FD != -1 to use that value. 50 | MAX_FD="maximum" 51 | 52 | warn () { 53 | echo "$*" 54 | } 55 | 56 | die () { 57 | echo 58 | echo "$*" 59 | echo 60 | exit 1 61 | } 62 | 63 | # OS specific support (must be 'true' or 'false'). 64 | cygwin=false 65 | msys=false 66 | darwin=false 67 | nonstop=false 68 | case "`uname`" in 69 | CYGWIN* ) 70 | cygwin=true 71 | ;; 72 | Darwin* ) 73 | darwin=true 74 | ;; 75 | MSYS* | MINGW* ) 76 | msys=true 77 | ;; 78 | NONSTOP* ) 79 | nonstop=true 80 | ;; 81 | esac 82 | 83 | CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar 84 | 85 | 86 | # Determine the Java command to use to start the JVM. 87 | if [ -n "$JAVA_HOME" ] ; then 88 | if [ -x "$JAVA_HOME/jre/sh/java" ] ; then 89 | # IBM's JDK on AIX uses strange locations for the executables 90 | JAVACMD="$JAVA_HOME/jre/sh/java" 91 | else 92 | JAVACMD="$JAVA_HOME/bin/java" 93 | fi 94 | if [ ! -x "$JAVACMD" ] ; then 95 | die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME 96 | 97 | Please set the JAVA_HOME variable in your environment to match the 98 | location of your Java installation." 99 | fi 100 | else 101 | JAVACMD="java" 102 | which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 103 | 104 | Please set the JAVA_HOME variable in your environment to match the 105 | location of your Java installation." 106 | fi 107 | 108 | # Increase the maximum file descriptors if we can. 109 | if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then 110 | MAX_FD_LIMIT=`ulimit -H -n` 111 | if [ $? -eq 0 ] ; then 112 | if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then 113 | MAX_FD="$MAX_FD_LIMIT" 114 | fi 115 | ulimit -n $MAX_FD 116 | if [ $? -ne 0 ] ; then 117 | warn "Could not set maximum file descriptor limit: $MAX_FD" 118 | fi 119 | else 120 | warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" 121 | fi 122 | fi 123 | 124 | # For Darwin, add options to specify how the application appears in the dock 125 | if $darwin; then 126 | GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" 127 | fi 128 | 129 | # For Cygwin or MSYS, switch paths to Windows format before running java 130 | if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then 131 | APP_HOME=`cygpath --path --mixed "$APP_HOME"` 132 | CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` 133 | 134 | JAVACMD=`cygpath --unix "$JAVACMD"` 135 | 136 | # We build the pattern for arguments to be converted via cygpath 137 | ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` 138 | SEP="" 139 | for dir in $ROOTDIRSRAW ; do 140 | ROOTDIRS="$ROOTDIRS$SEP$dir" 141 | SEP="|" 142 | done 143 | OURCYGPATTERN="(^($ROOTDIRS))" 144 | # Add a user-defined pattern to the cygpath arguments 145 | if [ "$GRADLE_CYGPATTERN" != "" ] ; then 146 | OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" 147 | fi 148 | # Now convert the arguments - kludge to limit ourselves to /bin/sh 149 | i=0 150 | for arg in "$@" ; do 151 | CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` 152 | CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option 153 | 154 | if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition 155 | eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` 156 | else 157 | eval `echo args$i`="\"$arg\"" 158 | fi 159 | i=`expr $i + 1` 160 | done 161 | case $i in 162 | 0) set -- ;; 163 | 1) set -- "$args0" ;; 164 | 2) set -- "$args0" "$args1" ;; 165 | 3) set -- "$args0" "$args1" "$args2" ;; 166 | 4) set -- "$args0" "$args1" "$args2" "$args3" ;; 167 | 5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; 168 | 6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; 169 | 7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; 170 | 8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; 171 | 9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; 172 | esac 173 | fi 174 | 175 | # Escape application args 176 | save () { 177 | for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done 178 | echo " " 179 | } 180 | APP_ARGS=`save "$@"` 181 | 182 | # Collect all arguments for the java command, following the shell quoting and substitution rules 183 | eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" 184 | 185 | exec "$JAVACMD" "$@" 186 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/subsystems/VisionSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.subsystems; 2 | 3 | import edu.wpi.first.math.util.Units; 4 | import edu.wpi.first.networktables.NetworkTableInstance; 5 | import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; 6 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 7 | import edu.wpi.first.wpilibj2.command.Subsystem; 8 | import georegression.struct.point.Point2D_F64; 9 | import org.frcteam2910.common.math.MathUtils; 10 | import org.frcteam2910.common.util.MovingAverage; 11 | import org.photonvision.targeting.PhotonPipelineResult; 12 | import org.photonvision.targeting.TargetCorner; 13 | 14 | public class VisionSubsystem implements Subsystem { 15 | final double TARGET_HEIGHT_METERS = Units.inchesToMeters(104); 16 | 17 | private static final double TARGET_ALLOWABLE_ERROR = Math.toRadians(2.5); 18 | private static final double LIMELIGHT_FORWARD = Units.inchesToMeters(5.22); 19 | private static final double LIMELIGHT_MOUNTING_ANGLE = Math.toRadians(45.0); 20 | private static final double LIMELIGHT_HEIGHT = Units.inchesToMeters(17.5); 21 | 22 | private static final double CAMERA_HEIGHT_PIXELS = 720; 23 | private static final double CAMERA_WIDTH_PIXELS = 960; 24 | 25 | private static final double HORIZONTAL_FOV = Math.toRadians(59.6); 26 | private static final double VERTICAL_FOV = Math.toRadians(45.7); 27 | 28 | private final DrivetrainSubsystem drivetrain; 29 | 30 | // private final PhotonCamera shooterLimelight = new PhotonCamera("gloworm"); 31 | 32 | private boolean shooterHasTargets = false; 33 | private double distanceToTarget = Double.NaN; 34 | private double angleToTarget = Double.NaN; 35 | private MovingAverage averageDistance = new MovingAverage(10); 36 | private double theta; 37 | private double centerXPixels; 38 | private double centerYPixels; 39 | private double centerXAngle; 40 | private double centerYAngle; 41 | 42 | private PhotonPipelineResult result; 43 | 44 | public VisionSubsystem(DrivetrainSubsystem drivetrain) { 45 | this.drivetrain = drivetrain; 46 | 47 | ShuffleboardTab tab = Shuffleboard.getTab("Vision"); 48 | tab.addBoolean("shooter has targets", () -> shooterHasTargets).withPosition(0, 0).withSize(1, 1); 49 | tab.addNumber("distance to target", () -> distanceToTarget).withPosition(1, 0).withSize(1, 1); 50 | tab.addNumber("angle to target", () -> Units.radiansToDegrees(angleToTarget)).withPosition(2, 0).withSize(1, 1); 51 | tab.addNumber("avg distance to target", () -> averageDistance.get()).withPosition(3, 0).withSize(1, 1); 52 | tab.addNumber("theta", () -> Math.toDegrees(theta)); 53 | tab.addNumber("center X Pixels", () -> centerXPixels); 54 | tab.addNumber("center Y Pixels", () -> centerYPixels); 55 | tab.addNumber("center X Angle", () -> Math.toDegrees(centerXAngle)); 56 | tab.addNumber("center Y Angle", () -> Math.toDegrees(centerYAngle)); 57 | tab.addBoolean("Is Vision on Target", this::isOnTarget); 58 | } 59 | 60 | public double getDistanceToTarget() { 61 | return distanceToTarget; 62 | } 63 | 64 | public double getAngleToTarget() { 65 | return angleToTarget; 66 | } 67 | 68 | public boolean shooterHasTargets() { 69 | return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0) > 0.5; 70 | } 71 | 72 | public boolean isOnTarget() { 73 | shooterHasTargets = shooterHasTargets(); 74 | if (shooterHasTargets) { 75 | double delta = angleToTarget - drivetrain.getPose().getRotation().getRadians(); 76 | if (delta > Math.PI) { 77 | delta = 2.0 * Math.PI - delta; 78 | } 79 | 80 | return MathUtils.epsilonEquals(delta, 0, TARGET_ALLOWABLE_ERROR); 81 | } else { 82 | return false; 83 | } 84 | } 85 | 86 | @Override 87 | public void periodic() { 88 | boolean hasTarget = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0) > 0.5; 89 | // result = shooterLimelight.getLatestResult(); 90 | 91 | double pitch = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); 92 | double yaw = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); 93 | 94 | if (hasTarget) { 95 | theta = Math.toRadians(pitch) + LIMELIGHT_MOUNTING_ANGLE; 96 | distanceToTarget = (TARGET_HEIGHT_METERS - LIMELIGHT_HEIGHT) / Math.tan(theta); 97 | angleToTarget = drivetrain.getPose().getRotation().getRadians() + Math.toRadians(yaw); 98 | } 99 | 100 | // // Ellipse Fitting 101 | // result = shooterLimelight.getLatestResult(); 102 | // 103 | // if (result != null && result.hasTargets()) { 104 | // List targets = result.getTargets(); 105 | // List bottomCorners = new ArrayList<>(); 106 | // List topCorners = new ArrayList<>(); 107 | // for (int i = 0; i < targets.size(); i++) { 108 | // List corners = targets.get(i).getCorners(); 109 | // corners.sort((a, b) -> Double.compare(a.y, b.y)); 110 | // 111 | // bottomCorners.add(convertToPoint(corners.get(0))); 112 | // bottomCorners.add(convertToPoint(corners.get(1))); 113 | // 114 | // topCorners.add(convertToPoint(corners.get(2))); 115 | // topCorners.add(convertToPoint(corners.get(3))); 116 | // } 117 | // 118 | // FitEllipseAlgebraic_F64 topEllipse = new FitEllipseAlgebraic_F64(); 119 | // if (topEllipse.process(topCorners)) { 120 | // double a = topEllipse.getEllipse().A; 121 | // double c = topEllipse.getEllipse().C; 122 | // double d = topEllipse.getEllipse().D; 123 | // double e = topEllipse.getEllipse().E; 124 | // 125 | // centerXPixels = d / 2 * a; 126 | // centerYPixels = -e / 2 * c; 127 | // 128 | // centerXAngle = ((centerXPixels / CAMERA_WIDTH_PIXELS) - 0.5) * 129 | // HORIZONTAL_FOV; 130 | // centerYAngle = -((centerYPixels / CAMERA_HEIGHT_PIXELS) - 0.5) * 131 | // VERTICAL_FOV; 132 | // 133 | // theta = centerYAngle + LIMELIGHT_MOUNTING_ANGLE; 134 | // distanceToTarget = (TARGET_HEIGHT_METERS - LIMELIGHT_HEIGHT) / 135 | // Math.tan(theta); 136 | // averageDistance.add(distanceToTarget); 137 | // angleToTarget = drivetrain.getPose().getRotation().getRadians() + 138 | // centerXAngle; 139 | // } 140 | // } 141 | } 142 | 143 | private Point2D_F64 convertToPoint(TargetCorner corner) { 144 | return new Point2D_F64(corner.x, corner.y); 145 | } 146 | } -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/RobotContainer.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022; 2 | 3 | import edu.wpi.first.wpilibj.XboxController; 4 | import edu.wpi.first.wpilibj2.command.CommandScheduler; 5 | import edu.wpi.first.wpilibj2.command.ConditionalCommand; 6 | import edu.wpi.first.wpilibj2.command.WaitCommand; 7 | import edu.wpi.first.wpilibj2.command.button.Button; 8 | import org.frcteam2910.c2022.commands.*; 9 | import org.frcteam2910.c2022.subsystems.*; 10 | import org.frcteam2910.c2022.util.AutonomousChooser; 11 | import org.frcteam2910.c2022.util.AutonomousTrajectories; 12 | import org.frcteam2910.c2022.util.ClimbChooser; 13 | 14 | public class RobotContainer { 15 | private final Superstructure superstructure = new Superstructure(); 16 | 17 | private final ClimberSubsystem climber = new ClimberSubsystem(); 18 | private final ShooterSubsystem shooter = new ShooterSubsystem(); 19 | private final IntakeSubsystem intake = new IntakeSubsystem(); 20 | private final FeederSubsystem feeder = new FeederSubsystem(); 21 | private final DrivetrainSubsystem drivetrain = new DrivetrainSubsystem(); 22 | private final VisionSubsystem vision = new VisionSubsystem(drivetrain); 23 | 24 | private final AutonomousChooser autonomousChooser = new AutonomousChooser( 25 | new AutonomousTrajectories(DrivetrainSubsystem.TRAJECTORY_CONSTRAINTS)); 26 | private final ClimbChooser climbChooser = new ClimbChooser(); 27 | 28 | private final XboxController controller = new XboxController(Constants.CONTROLLER_PORT); 29 | 30 | public RobotContainer() { 31 | CommandScheduler.getInstance().registerSubsystem(climber); 32 | CommandScheduler.getInstance().registerSubsystem(shooter); 33 | CommandScheduler.getInstance().registerSubsystem(intake); 34 | CommandScheduler.getInstance().registerSubsystem(feeder); 35 | CommandScheduler.getInstance().registerSubsystem(drivetrain); 36 | CommandScheduler.getInstance().registerSubsystem(vision); 37 | 38 | shooter.setDefaultCommand(new DefaultShooterCommand(shooter)); 39 | intake.setDefaultCommand(new DefaultIntakeCommand(intake)); 40 | feeder.setDefaultCommand(new DefaultFeederCommand(feeder)); 41 | drivetrain.setDefaultCommand(new DefaultDriveCommand(drivetrain, this::getForwardInput, this::getStrafeInput, 42 | this::getRotationInput)); 43 | 44 | configureButtonBindings(); 45 | } 46 | 47 | public DrivetrainSubsystem getDrivetrain() { 48 | return drivetrain; 49 | } 50 | 51 | public ShooterSubsystem getShooter() { 52 | return shooter; 53 | } 54 | 55 | public IntakeSubsystem getIntake() { 56 | return intake; 57 | } 58 | 59 | public ClimberSubsystem getClimber() { 60 | return climber; 61 | } 62 | 63 | public FeederSubsystem getFeeder() { 64 | return feeder; 65 | } 66 | 67 | public VisionSubsystem getVision() { 68 | return vision; 69 | } 70 | 71 | public XboxController getController() { 72 | return controller; 73 | } 74 | 75 | public void configureButtonBindings() { 76 | new Button(controller::getRightBumper).whileHeld(new TargetWithShooterCommand(shooter, vision) 77 | .alongWith( 78 | new AlignRobotToShootCommand(drivetrain, vision, this::getForwardInput, this::getStrafeInput)) 79 | .alongWith(new WaitCommand(0.1).andThen(new ShootWhenReadyCommand(feeder, shooter, vision)))); 80 | new Button(controller::getLeftBumper).whileHeld(new SimpleIntakeCommand(intake, feeder, controller)); 81 | 82 | new Button(() -> controller.getRightTriggerAxis() > 0.5).whileHeld(new FenderShootCommand(feeder, shooter)); 83 | new Button(() -> controller.getLeftTriggerAxis() > 0.5).whenPressed(new ResetFeederCommand(feeder, intake)); 84 | 85 | new Button(controller::getYButton).whenPressed(new ZeroClimberCommand(climber)); 86 | new Button(controller::getXButton).whenPressed(new ZeroHoodCommand(shooter, true)); 87 | new Button(controller::getAButton).whileHeld(new ManualFeedToShooterCommand(feeder)); 88 | 89 | new Button(() -> controller.getPOV() == 0).whenPressed(new ConditionalCommand( 90 | new ClimberToPointCommand(climber, ClimberSubsystem.MID_RUNG_HEIGHT, false, true), 91 | new ClimberToPointCommand(climber, ClimberSubsystem.MAX_HEIGHT, false, true), () -> climber 92 | .getCurrentHeight() > (ClimberSubsystem.MAX_HEIGHT + ClimberSubsystem.MID_RUNG_HEIGHT) / 2.0)); 93 | new Button(() -> controller.getPOV() == 180) 94 | .whenPressed(new ClimberToPointCommand(climber, ClimberSubsystem.MIN_HEIGHT, false, true)); 95 | 96 | new Button(controller::getBackButton).whenPressed(drivetrain::zeroRotation); 97 | new Button(controller::getStartButton).whenPressed( 98 | new AutoClimbCommand(climber, shooter, () -> climbChooser.getClimbChooser().getSelected())); 99 | 100 | // // manual hood adjustment - 0: up, 180: down 101 | // new Button(() -> controller.getPOV() == 180.0).whenPressed(() -> 102 | // shooter.setHoodTargetPosition( 103 | // shooter.getHoodTargetPosition() - Constants.HOOD_MANUAL_ADJUST_INTERVAL) 104 | // ); 105 | // 106 | // new Button(() -> controller.getPOV() == 0.0).whenPressed(() -> 107 | // shooter.setHoodTargetPosition( 108 | // shooter.getHoodTargetPosition() + Constants.HOOD_MANUAL_ADJUST_INTERVAL) 109 | // ); 110 | // 111 | // //manual flywheel adjustment - 90: right, 270: left 112 | // new Button(() -> controller.getPOV() == 90.0).whenPressed(() -> 113 | // shooter.setTargetFlywheelSpeed( 114 | // shooter.getTargetFlywheelSpeed() + Constants.FLYWHEEL_MANUAL_ADJUST_INTERVAL) 115 | // ); 116 | // 117 | // new Button(() -> controller.getPOV() == 270.0).whenPressed(() -> 118 | // shooter.setTargetFlywheelSpeed( 119 | // shooter.getTargetFlywheelSpeed() - Constants.FLYWHEEL_MANUAL_ADJUST_INTERVAL) 120 | // ); 121 | } 122 | 123 | private static double deadband(double value, double tolerance) { 124 | if (Math.abs(value) < tolerance) 125 | return 0.0; 126 | 127 | return Math.copySign(value, (value - tolerance) / (1.0 - tolerance)); 128 | } 129 | 130 | private static double square(double value) { 131 | return Math.copySign(value * value, value); 132 | } 133 | 134 | private double getForwardInput() { 135 | return -square(deadband(controller.getLeftY(), 0.1)) * DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND; 136 | } 137 | 138 | private double getStrafeInput() { 139 | return -square(deadband(controller.getLeftX(), 0.1)) * DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND; 140 | } 141 | 142 | private double getRotationInput() { 143 | return -square(deadband(controller.getRightX(), 0.1)) 144 | * DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND; 145 | } 146 | 147 | public AutonomousChooser getAutonomousChooser() { 148 | return autonomousChooser; 149 | } 150 | 151 | public ClimbChooser getClimbChooser() { 152 | return climbChooser; 153 | } 154 | 155 | public Superstructure getSuperstructure() { 156 | return superstructure; 157 | } 158 | } 159 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/commands/AutoClimbCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.*; 4 | import org.frcteam2910.c2022.subsystems.ClimberSubsystem; 5 | import org.frcteam2910.c2022.subsystems.ShooterSubsystem; 6 | import org.frcteam2910.c2022.util.ClimbChooser; 7 | 8 | public class AutoClimbCommand extends SequentialCommandGroup { 9 | private final ClimberSubsystem climber; 10 | private final ShooterSubsystem shooter; 11 | 12 | private final ClimbTypeSupplier climbTypeSupplier; 13 | 14 | private boolean hasAddedCommands; 15 | 16 | @FunctionalInterface 17 | public interface ClimbTypeSupplier { 18 | ClimbChooser.ClimbType getClimbType(); 19 | } 20 | 21 | public AutoClimbCommand(ClimberSubsystem climber, ShooterSubsystem shooter, ClimbTypeSupplier climbTypeSupplier) { 22 | this.climber = climber; 23 | this.shooter = shooter; 24 | this.climbTypeSupplier = climbTypeSupplier; 25 | } 26 | 27 | @Override 28 | public void initialize() { 29 | if (!hasAddedCommands) { 30 | hasAddedCommands = true; 31 | ClimbChooser.ClimbType climbType = climbTypeSupplier.getClimbType(); 32 | // Climber is currently hooked on the mid rung, but the robot is on the ground 33 | addCommands(new InstantCommand(shooter::disableFlywheel)); 34 | 35 | // Prepare to transfer mid rung to hood 36 | addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.HOOD_TRANSFER_HEIGHT, true, false) 37 | .alongWith(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_PREPARE_TRANSFER_ANGLE))); 38 | 39 | // Transfer mid rung to hood 40 | addCommands(transferToHood(climber, shooter)); 41 | if (climbType != ClimbChooser.ClimbType.MID) { 42 | // Move from mid rung to high rung 43 | if (climbType == ClimbChooser.ClimbType.HIGH_PARTWAY) { 44 | addCommands(traverseToNextRung(climber, shooter, false, true)); 45 | addCommands(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_MIN_ANGLE)); 46 | } else { 47 | addCommands(traverseToNextRung(climber, shooter)); 48 | } 49 | if (climbType != ClimbChooser.ClimbType.HIGH_PARTWAY) { 50 | // Transfer high rung to hood 51 | addCommands(transferToHood(climber, shooter)); 52 | if (climbType != ClimbChooser.ClimbType.HIGH_HOOK) { 53 | // Move from high rung to traverse rung 54 | if (climbType == ClimbChooser.ClimbType.TRAVERSAL_PARTWAY) { 55 | addCommands(traverseToNextRung(climber, shooter, true, true)); 56 | } else { 57 | addCommands(traverseToNextRung(climber, shooter, true, false)); 58 | } 59 | if (climbType != ClimbChooser.ClimbType.TRAVERSAL_PARTWAY) { 60 | addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.HOOD_PASSAGE_HEIGHT, false, 61 | false)); 62 | // Transfer traverse rung to hood 63 | addCommands(transferToHood(climber, shooter)); 64 | } 65 | } 66 | } 67 | } 68 | addCommands(new InstantCommand(() -> climber.setTargetVoltage(0.0))); 69 | addCommands(new WaitCommand(5).perpetually()); 70 | super.initialize(); 71 | } 72 | } 73 | 74 | private static Command transferToHood(ClimberSubsystem climber, ShooterSubsystem shooter) { 75 | SequentialCommandGroup group = new SequentialCommandGroup(); 76 | 77 | // Move the climber to the transfer height 78 | group.addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.HOOD_TRANSFER_HEIGHT)); 79 | // Grab the rung with the hood 80 | group.addCommands(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_TRANSFER_ANGLE)); 81 | // Release the rung with the climber so the hood can move freely 82 | group.addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.HOOD_PASSAGE_HEIGHT)); 83 | 84 | return group; 85 | } 86 | 87 | private static Command traverseToNextRung(ClimberSubsystem climber, ShooterSubsystem shooter) { 88 | return traverseToNextRung(climber, shooter, false, false); 89 | } 90 | 91 | private static Command traverseToNextRung(ClimberSubsystem climber, ShooterSubsystem shooter, boolean traversal, 92 | boolean partway) { 93 | SequentialCommandGroup group = new SequentialCommandGroup(); 94 | 95 | if (!traversal) { 96 | group.addCommands( 97 | new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_TRAVERSE_EXTEND_ANGLE_HIGH, true, true) 98 | .alongWith(new ClimberToPointCommand(climber, ClimberSubsystem.TRAVERSE_EXTEND_HEIGHT))); 99 | } else { 100 | // Angle the robot to extend the climber 101 | group.addCommands(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_TRAVERSE_EXTEND_ANGLE_TRAVERSE, 102 | false, true)); 103 | // Extend the climber slightly past the next rung 104 | group.addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.TRAVERSE_EXTEND_HEIGHT)); 105 | } 106 | // Angle the robot so the climber hooks will grab the next rung 107 | group.addCommands(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_TRAVERSE_RETRACT_ANGLE, false, true)); 108 | 109 | if (traversal) { 110 | if (partway) { 111 | group.addCommands( 112 | new ClimberToPointCommand(climber, ClimberSubsystem.TRAVERSE_RUNG_PARTWAY_HEIGHT, false, false) 113 | .alongWith(new SetHoodAngleCommand(shooter, 114 | ShooterSubsystem.HOOD_PREPARE_TRANSFER_ANGLE, true, true))); 115 | group.addCommands(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_MIN_ANGLE)); 116 | } else { 117 | group.addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.HOOD_PASSAGE_HEIGHT, false, false) 118 | .alongWith(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_PREPARE_TRANSFER_ANGLE, true, 119 | true))); 120 | } 121 | } else { 122 | // Retract the climber, and move the hood to the transfer position after the 123 | // climber grabs onto the next rung 124 | if (!partway) { 125 | group.addCommands(new ClimberToPointCommand(climber, ClimberSubsystem.HOOD_PASSAGE_HEIGHT, false, false) 126 | .alongWith(new SetHoodAngleCommand(shooter, ShooterSubsystem.HOOD_PREPARE_TRANSFER_ANGLE, true, 127 | true))); 128 | } else { 129 | group.addCommands( 130 | new ClimberToPointCommand(climber, ClimberSubsystem.TRAVERSE_RUNG_PARTWAY_HEIGHT, false, false) 131 | .alongWith(new SetHoodAngleCommand(shooter, 132 | ShooterSubsystem.HOOD_PREPARE_TRANSFER_ANGLE, true, true))); 133 | } 134 | } 135 | 136 | return group; 137 | } 138 | } 139 | -------------------------------------------------------------------------------- /vendordeps/Phoenix-frc2022-latest.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "Phoenix.json", 3 | "name": "CTRE-Phoenix", 4 | "version": "5.21.1", 5 | "frcYear": 2022, 6 | "uuid": "ab676553-b602-441f-a38d-f1296eff6537", 7 | "mavenUrls": [ 8 | "https://maven.ctr-electronics.com/release/" 9 | ], 10 | "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix-frc2022-latest.json", 11 | "javaDependencies": [ 12 | { 13 | "groupId": "com.ctre.phoenix", 14 | "artifactId": "api-java", 15 | "version": "5.21.1" 16 | }, 17 | { 18 | "groupId": "com.ctre.phoenix", 19 | "artifactId": "wpiapi-java", 20 | "version": "5.21.1" 21 | } 22 | ], 23 | "jniDependencies": [ 24 | { 25 | "groupId": "com.ctre.phoenix", 26 | "artifactId": "cci", 27 | "version": "5.21.1", 28 | "isJar": false, 29 | "skipInvalidPlatforms": true, 30 | "validPlatforms": [ 31 | "linuxathena" 32 | ] 33 | }, 34 | { 35 | "groupId": "com.ctre.phoenix.sim", 36 | "artifactId": "cci-sim", 37 | "version": "5.21.1", 38 | "isJar": false, 39 | "skipInvalidPlatforms": true, 40 | "validPlatforms": [ 41 | "windowsx86-64", 42 | "linuxx86-64", 43 | "osxx86-64" 44 | ] 45 | }, 46 | { 47 | "groupId": "com.ctre.phoenix.sim", 48 | "artifactId": "simTalonSRX", 49 | "version": "5.21.1", 50 | "isJar": false, 51 | "skipInvalidPlatforms": true, 52 | "validPlatforms": [ 53 | "windowsx86-64", 54 | "linuxx86-64", 55 | "osxx86-64" 56 | ] 57 | }, 58 | { 59 | "groupId": "com.ctre.phoenix.sim", 60 | "artifactId": "simTalonFX", 61 | "version": "5.21.1", 62 | "isJar": false, 63 | "skipInvalidPlatforms": true, 64 | "validPlatforms": [ 65 | "windowsx86-64", 66 | "linuxx86-64", 67 | "osxx86-64" 68 | ] 69 | }, 70 | { 71 | "groupId": "com.ctre.phoenix.sim", 72 | "artifactId": "simVictorSPX", 73 | "version": "5.21.1", 74 | "isJar": false, 75 | "skipInvalidPlatforms": true, 76 | "validPlatforms": [ 77 | "windowsx86-64", 78 | "linuxx86-64", 79 | "osxx86-64" 80 | ] 81 | }, 82 | { 83 | "groupId": "com.ctre.phoenix.sim", 84 | "artifactId": "simPigeonIMU", 85 | "version": "5.21.1", 86 | "isJar": false, 87 | "skipInvalidPlatforms": true, 88 | "validPlatforms": [ 89 | "windowsx86-64", 90 | "linuxx86-64", 91 | "osxx86-64" 92 | ] 93 | }, 94 | { 95 | "groupId": "com.ctre.phoenix.sim", 96 | "artifactId": "simCANCoder", 97 | "version": "5.21.1", 98 | "isJar": false, 99 | "skipInvalidPlatforms": true, 100 | "validPlatforms": [ 101 | "windowsx86-64", 102 | "linuxx86-64", 103 | "osxx86-64" 104 | ] 105 | } 106 | ], 107 | "cppDependencies": [ 108 | { 109 | "groupId": "com.ctre.phoenix", 110 | "artifactId": "wpiapi-cpp", 111 | "version": "5.21.1", 112 | "libName": "CTRE_Phoenix_WPI", 113 | "headerClassifier": "headers", 114 | "sharedLibrary": true, 115 | "skipInvalidPlatforms": true, 116 | "binaryPlatforms": [ 117 | "linuxathena" 118 | ] 119 | }, 120 | { 121 | "groupId": "com.ctre.phoenix", 122 | "artifactId": "api-cpp", 123 | "version": "5.21.1", 124 | "libName": "CTRE_Phoenix", 125 | "headerClassifier": "headers", 126 | "sharedLibrary": true, 127 | "skipInvalidPlatforms": true, 128 | "binaryPlatforms": [ 129 | "linuxathena" 130 | ] 131 | }, 132 | { 133 | "groupId": "com.ctre.phoenix", 134 | "artifactId": "cci", 135 | "version": "5.21.1", 136 | "libName": "CTRE_PhoenixCCI", 137 | "headerClassifier": "headers", 138 | "sharedLibrary": true, 139 | "skipInvalidPlatforms": true, 140 | "binaryPlatforms": [ 141 | "linuxathena" 142 | ] 143 | }, 144 | { 145 | "groupId": "com.ctre.phoenix.sim", 146 | "artifactId": "wpiapi-cpp-sim", 147 | "version": "5.21.1", 148 | "libName": "CTRE_Phoenix_WPISim", 149 | "headerClassifier": "headers", 150 | "sharedLibrary": true, 151 | "skipInvalidPlatforms": true, 152 | "binaryPlatforms": [ 153 | "windowsx86-64", 154 | "linuxx86-64", 155 | "osxx86-64" 156 | ] 157 | }, 158 | { 159 | "groupId": "com.ctre.phoenix.sim", 160 | "artifactId": "api-cpp-sim", 161 | "version": "5.21.1", 162 | "libName": "CTRE_PhoenixSim", 163 | "headerClassifier": "headers", 164 | "sharedLibrary": true, 165 | "skipInvalidPlatforms": true, 166 | "binaryPlatforms": [ 167 | "windowsx86-64", 168 | "linuxx86-64", 169 | "osxx86-64" 170 | ] 171 | }, 172 | { 173 | "groupId": "com.ctre.phoenix.sim", 174 | "artifactId": "cci-sim", 175 | "version": "5.21.1", 176 | "libName": "CTRE_PhoenixCCISim", 177 | "headerClassifier": "headers", 178 | "sharedLibrary": true, 179 | "skipInvalidPlatforms": true, 180 | "binaryPlatforms": [ 181 | "windowsx86-64", 182 | "linuxx86-64", 183 | "osxx86-64" 184 | ] 185 | }, 186 | { 187 | "groupId": "com.ctre.phoenix.sim", 188 | "artifactId": "simTalonSRX", 189 | "version": "5.21.1", 190 | "libName": "CTRE_SimTalonSRX", 191 | "headerClassifier": "headers", 192 | "sharedLibrary": true, 193 | "skipInvalidPlatforms": true, 194 | "binaryPlatforms": [ 195 | "windowsx86-64", 196 | "linuxx86-64", 197 | "osxx86-64" 198 | ] 199 | }, 200 | { 201 | "groupId": "com.ctre.phoenix.sim", 202 | "artifactId": "simTalonFX", 203 | "version": "5.21.1", 204 | "libName": "CTRE_SimTalonFX", 205 | "headerClassifier": "headers", 206 | "sharedLibrary": true, 207 | "skipInvalidPlatforms": true, 208 | "binaryPlatforms": [ 209 | "windowsx86-64", 210 | "linuxx86-64", 211 | "osxx86-64" 212 | ] 213 | }, 214 | { 215 | "groupId": "com.ctre.phoenix.sim", 216 | "artifactId": "simVictorSPX", 217 | "version": "5.21.1", 218 | "libName": "CTRE_SimVictorSPX", 219 | "headerClassifier": "headers", 220 | "sharedLibrary": true, 221 | "skipInvalidPlatforms": true, 222 | "binaryPlatforms": [ 223 | "windowsx86-64", 224 | "linuxx86-64", 225 | "osxx86-64" 226 | ] 227 | }, 228 | { 229 | "groupId": "com.ctre.phoenix.sim", 230 | "artifactId": "simPigeonIMU", 231 | "version": "5.21.1", 232 | "libName": "CTRE_SimPigeonIMU", 233 | "headerClassifier": "headers", 234 | "sharedLibrary": true, 235 | "skipInvalidPlatforms": true, 236 | "binaryPlatforms": [ 237 | "windowsx86-64", 238 | "linuxx86-64", 239 | "osxx86-64" 240 | ] 241 | }, 242 | { 243 | "groupId": "com.ctre.phoenix.sim", 244 | "artifactId": "simCANCoder", 245 | "version": "5.21.1", 246 | "libName": "CTRE_SimCANCoder", 247 | "headerClassifier": "headers", 248 | "sharedLibrary": true, 249 | "skipInvalidPlatforms": true, 250 | "binaryPlatforms": [ 251 | "windowsx86-64", 252 | "linuxx86-64", 253 | "osxx86-64" 254 | ] 255 | } 256 | ] 257 | } -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/subsystems/DrivetrainSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.subsystems; 2 | 3 | import java.util.Map; 4 | 5 | import com.ctre.phoenix.sensors.Pigeon2; 6 | import com.swervedrivespecialties.swervelib.Mk4ModuleConfiguration; 7 | import com.swervedrivespecialties.swervelib.Mk4iSwerveModuleHelper; 8 | import com.swervedrivespecialties.swervelib.SdsModuleConfigurations; 9 | import com.swervedrivespecialties.swervelib.SwerveModule; 10 | import edu.wpi.first.math.VecBuilder; 11 | import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; 12 | import edu.wpi.first.math.geometry.Pose2d; 13 | import edu.wpi.first.math.geometry.Rotation2d; 14 | import edu.wpi.first.math.geometry.Translation2d; 15 | import edu.wpi.first.math.kinematics.ChassisSpeeds; 16 | import edu.wpi.first.math.kinematics.SwerveDriveKinematics; 17 | import edu.wpi.first.math.kinematics.SwerveModuleState; 18 | import edu.wpi.first.math.util.Units; 19 | import edu.wpi.first.networktables.NetworkTableEntry; 20 | import edu.wpi.first.wpilibj.Timer; 21 | import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; 22 | import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; 23 | import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; 24 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 25 | import edu.wpi.first.wpilibj2.command.SubsystemBase; 26 | import org.frcteam2910.c2022.Robot; 27 | import org.frcteam2910.c2022.util.Utilities; 28 | import org.frcteam2910.common.control.*; 29 | import org.frcteam2910.common.math.Vector2; 30 | import org.frcteam2910.common.util.DrivetrainFeedforwardConstants; 31 | import org.frcteam2910.common.util.HolonomicDriveSignal; 32 | import org.frcteam2910.common.util.HolonomicFeedforward; 33 | 34 | import static org.frcteam2910.c2022.Constants.*; 35 | 36 | public class DrivetrainSubsystem extends SubsystemBase { 37 | public static final double MAX_VOLTAGE = 12.0; 38 | public static final double MAX_VELOCITY_METERS_PER_SECOND = 6380.0 / 60.0 39 | * SdsModuleConfigurations.MK4_L3.getDriveReduction() * SdsModuleConfigurations.MK4_L3.getWheelDiameter() 40 | * Math.PI; 41 | public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND = MAX_VELOCITY_METERS_PER_SECOND 42 | / Math.hypot(DRIVETRAIN_TRACKWIDTH_METERS / 2.0, DRIVETRAIN_WHEELBASE_METERS / 2.0); 43 | 44 | public static final DrivetrainFeedforwardConstants FEEDFORWARD_CONSTANTS = new DrivetrainFeedforwardConstants(0.891, 45 | 0.15, 0.13592); 46 | 47 | public static final double DRIVETRAIN_CURRENT_LIMIT = 50.0; 48 | 49 | public static final TrajectoryConstraint[] TRAJECTORY_CONSTRAINTS = { 50 | new FeedforwardConstraint(4.0, FEEDFORWARD_CONSTANTS.getVelocityConstant(), 51 | FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), 52 | new MaxAccelerationConstraint(5.0), new CentripetalAccelerationConstraint(5.0)}; 53 | 54 | private final HolonomicMotionProfiledTrajectoryFollower follower = new HolonomicMotionProfiledTrajectoryFollower( 55 | new PidConstants(5.0, 0.0, 0.0), new PidConstants(5.0, 0.0, 0.0), 56 | new HolonomicFeedforward(FEEDFORWARD_CONSTANTS)); 57 | 58 | private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics( 59 | // Front left 60 | new Translation2d(DRIVETRAIN_TRACKWIDTH_METERS / 2.0, DRIVETRAIN_WHEELBASE_METERS / 2.0), 61 | // Front right 62 | new Translation2d(DRIVETRAIN_TRACKWIDTH_METERS / 2.0, -DRIVETRAIN_WHEELBASE_METERS / 2.0), 63 | // Back left 64 | new Translation2d(-DRIVETRAIN_TRACKWIDTH_METERS / 2.0, DRIVETRAIN_WHEELBASE_METERS / 2.0), 65 | // Back right 66 | new Translation2d(-DRIVETRAIN_TRACKWIDTH_METERS / 2.0, -DRIVETRAIN_WHEELBASE_METERS / 2.0)); 67 | 68 | private final SwerveDrivePoseEstimator estimator; 69 | 70 | private final Pigeon2 pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); 71 | 72 | private final SwerveModule frontLeftModule; 73 | private final SwerveModule frontRightModule; 74 | private final SwerveModule backLeftModule; 75 | private final SwerveModule backRightModule; 76 | 77 | private ChassisSpeeds currentVelocity = new ChassisSpeeds(); 78 | private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); 79 | 80 | private final NetworkTableEntry motorOutputPercentageLimiterEntry; 81 | 82 | private double motorOutputLimiter; 83 | 84 | public DrivetrainSubsystem() { 85 | Mk4ModuleConfiguration mk4ModuleConfiguration = new Mk4ModuleConfiguration(); 86 | mk4ModuleConfiguration.setDriveCurrentLimit(DRIVETRAIN_CURRENT_LIMIT); 87 | ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain"); 88 | frontLeftModule = Mk4iSwerveModuleHelper.createFalcon500( 89 | tab.getLayout("Front Left Module", BuiltInLayouts.kList).withSize(2, 4).withPosition(0, 0), 90 | mk4ModuleConfiguration, Mk4iSwerveModuleHelper.GearRatio.L3, FRONT_LEFT_MODULE_DRIVE_MOTOR, 91 | FRONT_LEFT_MODULE_STEER_MOTOR, FRONT_LEFT_MODULE_STEER_ENCODER, FRONT_LEFT_MODULE_STEER_OFFSET); 92 | frontRightModule = Mk4iSwerveModuleHelper.createFalcon500( 93 | tab.getLayout("Front Right Module", BuiltInLayouts.kList).withSize(2, 4).withPosition(2, 0), 94 | mk4ModuleConfiguration, Mk4iSwerveModuleHelper.GearRatio.L3, FRONT_RIGHT_MODULE_DRIVE_MOTOR, 95 | FRONT_RIGHT_MODULE_STEER_MOTOR, FRONT_RIGHT_MODULE_STEER_ENCODER, FRONT_RIGHT_MODULE_STEER_OFFSET); 96 | backLeftModule = Mk4iSwerveModuleHelper.createFalcon500( 97 | tab.getLayout("Back Left Module", BuiltInLayouts.kList).withSize(2, 4).withPosition(4, 0), 98 | mk4ModuleConfiguration, Mk4iSwerveModuleHelper.GearRatio.L3, BACK_LEFT_MODULE_DRIVE_MOTOR, 99 | BACK_LEFT_MODULE_STEER_MOTOR, BACK_LEFT_MODULE_STEER_ENCODER, BACK_LEFT_MODULE_STEER_OFFSET); 100 | backRightModule = Mk4iSwerveModuleHelper.createFalcon500( 101 | tab.getLayout("Back Right Module", BuiltInLayouts.kList).withSize(2, 4).withPosition(6, 0), 102 | mk4ModuleConfiguration, Mk4iSwerveModuleHelper.GearRatio.L3, BACK_RIGHT_MODULE_DRIVE_MOTOR, 103 | BACK_RIGHT_MODULE_STEER_MOTOR, BACK_RIGHT_MODULE_STEER_ENCODER, BACK_RIGHT_MODULE_STEER_OFFSET); 104 | estimator = new SwerveDrivePoseEstimator(getGyroscopeRotation(), new Pose2d(), kinematics, 105 | VecBuilder.fill(0.02, 0.02, 0.01), // estimator values (x, y, rotation) std-devs 106 | VecBuilder.fill(0.01), // Gyroscope rotation std-dev 107 | VecBuilder.fill(0.1, 0.1, 0.01)); // Vision (x, y, rotation) std-devs 108 | motorOutputPercentageLimiterEntry = tab.add("Motor Percentage", 100.0).withWidget(BuiltInWidgets.kNumberSlider) 109 | .withProperties(Map.of("min", 0.0, "max", 100.0, "Block increment", 10.0)).withPosition(0, 3) 110 | .getEntry(); 111 | 112 | tab.addNumber("Odometry X", () -> Units.metersToFeet(getPose().getX())); 113 | tab.addNumber("Odometry Y", () -> Units.metersToFeet(getPose().getY())); 114 | tab.addNumber("Odometry Angle", () -> getPose().getRotation().getDegrees()); 115 | tab.addNumber("Velocity X", () -> Units.metersToFeet(getCurrentVelocity().vxMetersPerSecond)); 116 | tab.addNumber("Trajectory Position X", () -> { 117 | var lastState = follower.getLastState(); 118 | if (lastState == null) 119 | return 0; 120 | 121 | return Units.metersToFeet(lastState.getPathState().getPosition().x); 122 | }); 123 | tab.addNumber("Trajectory Velocity X", () -> { 124 | var lastState = follower.getLastState(); 125 | if (lastState == null) 126 | return 0; 127 | 128 | return Units.metersToFeet(lastState.getVelocity()); 129 | }); 130 | tab.addNumber("Gyroscope Angle", () -> getGyroscopeRotation().getDegrees()); 131 | pigeon.configFactoryDefault(); 132 | } 133 | 134 | private Rotation2d getGyroscopeRotation() { 135 | return Rotation2d.fromDegrees(pigeon.getYaw()); 136 | } 137 | 138 | /** 139 | * Resets the rotation of the drivetrain to zero. 140 | */ 141 | public void zeroRotation() { 142 | estimator.resetPosition(new Pose2d(getPose().getX(), getPose().getY(), new Rotation2d()), 143 | getGyroscopeRotation()); 144 | } 145 | 146 | /** 147 | * Returns the position of the robot 148 | */ 149 | public Pose2d getPose() { 150 | return estimator.getEstimatedPosition(); 151 | } 152 | 153 | public double getMotorOutputLimiter() { 154 | return motorOutputLimiter; 155 | } 156 | 157 | public HolonomicMotionProfiledTrajectoryFollower getFollower() { 158 | return follower; 159 | } 160 | 161 | public ChassisSpeeds getCurrentVelocity() { 162 | return currentVelocity; 163 | } 164 | 165 | /** 166 | * Sets the position of the robot to the position passed in with the current 167 | * gyroscope rotation. 168 | */ 169 | public void setPose(Pose2d pose) { 170 | estimator.resetPosition(pose, getGyroscopeRotation()); 171 | } 172 | 173 | /** 174 | * Sets the desired chassis speed of the drivetrain. 175 | */ 176 | public void drive(ChassisSpeeds chassisSpeeds) { 177 | this.chassisSpeeds = chassisSpeeds; 178 | } 179 | 180 | public void periodic() { 181 | SwerveModuleState currentFrontLeftModuleState = new SwerveModuleState(frontLeftModule.getDriveVelocity(), 182 | new Rotation2d(frontLeftModule.getSteerAngle())); 183 | SwerveModuleState currentFrontRightModuleState = new SwerveModuleState(frontRightModule.getDriveVelocity(), 184 | new Rotation2d(frontRightModule.getSteerAngle())); 185 | SwerveModuleState currentBackLeftModuleState = new SwerveModuleState(backLeftModule.getDriveVelocity(), 186 | new Rotation2d(backLeftModule.getSteerAngle())); 187 | SwerveModuleState currentBackRightModuleState = new SwerveModuleState(backRightModule.getDriveVelocity(), 188 | new Rotation2d(backRightModule.getSteerAngle())); 189 | 190 | currentVelocity = kinematics.toChassisSpeeds(currentFrontLeftModuleState, currentFrontRightModuleState, 191 | currentBackLeftModuleState, currentBackRightModuleState); 192 | 193 | estimator.update(getGyroscopeRotation(), currentFrontLeftModuleState, currentFrontRightModuleState, 194 | currentBackLeftModuleState, currentBackRightModuleState); 195 | 196 | var driveSignalOpt = follower.update(Utilities.poseToRigidTransform(getPose()), 197 | new Vector2(currentVelocity.vxMetersPerSecond, currentVelocity.vyMetersPerSecond), 198 | currentVelocity.omegaRadiansPerSecond, Timer.getFPGATimestamp(), Robot.kDefaultPeriod); 199 | 200 | if (driveSignalOpt.isPresent()) { 201 | HolonomicDriveSignal driveSignal = driveSignalOpt.get(); 202 | if (driveSignalOpt.get().isFieldOriented()) { 203 | chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(driveSignal.getTranslation().x, 204 | driveSignal.getTranslation().y, driveSignal.getRotation(), getPose().getRotation()); 205 | } else { 206 | chassisSpeeds = new ChassisSpeeds(driveSignal.getTranslation().x, driveSignal.getTranslation().y, 207 | driveSignal.getRotation()); 208 | } 209 | } 210 | 211 | motorOutputLimiter = motorOutputPercentageLimiterEntry.getDouble(0.0) / 100; 212 | 213 | SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds); 214 | SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_VELOCITY_METERS_PER_SECOND); 215 | frontLeftModule.set(states[0].speedMetersPerSecond / MAX_VELOCITY_METERS_PER_SECOND * MAX_VOLTAGE, 216 | states[0].angle.getRadians()); 217 | frontRightModule.set(states[1].speedMetersPerSecond / MAX_VELOCITY_METERS_PER_SECOND * MAX_VOLTAGE, 218 | states[1].angle.getRadians()); 219 | backLeftModule.set(states[2].speedMetersPerSecond / MAX_VELOCITY_METERS_PER_SECOND * MAX_VOLTAGE, 220 | states[2].angle.getRadians()); 221 | backRightModule.set(states[3].speedMetersPerSecond / MAX_VELOCITY_METERS_PER_SECOND * MAX_VOLTAGE, 222 | states[3].angle.getRadians()); 223 | } 224 | } -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/subsystems/ClimberSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.subsystems; 2 | 3 | import com.ctre.phoenix.motorcontrol.NeutralMode; 4 | import com.ctre.phoenix.motorcontrol.StatusFrame; 5 | import com.ctre.phoenix.motorcontrol.TalonFXControlMode; 6 | import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; 7 | import com.ctre.phoenix.motorcontrol.can.TalonFX; 8 | import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; 9 | import edu.wpi.first.math.numbers.N1; 10 | import edu.wpi.first.math.numbers.N2; 11 | import edu.wpi.first.math.system.LinearSystem; 12 | import edu.wpi.first.math.system.plant.DCMotor; 13 | import edu.wpi.first.math.system.plant.LinearSystemId; 14 | import edu.wpi.first.math.util.Units; 15 | import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; 16 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 17 | import edu.wpi.first.wpilibj.simulation.ElevatorSim; 18 | import edu.wpi.first.wpilibj2.command.Subsystem; 19 | import org.frcteam2910.c2022.Constants; 20 | import org.frcteam2910.c2022.Robot; 21 | import org.frcteam2910.common.math.MathUtils; 22 | import org.frcteam2910.common.motion.MotionProfile; 23 | 24 | public class ClimberSubsystem implements Subsystem { 25 | public static final double HEIGHT_CHANGE = 6.75; 26 | public static final double MAX_HEIGHT = Units.inchesToMeters(32.75); 27 | public static final double MID_RUNG_HEIGHT = Units.inchesToMeters(30.0); 28 | public static final double TRAVERSE_EXTEND_HEIGHT = Units.inchesToMeters(28.5); 29 | public static final double TRAVERSE_RUNG_PARTWAY_HEIGHT = Units.inchesToMeters(20.0); 30 | public static final double HOOD_PASSAGE_HEIGHT = Units.inchesToMeters(3.5); 31 | public static final double HOOD_TRANSFER_HEIGHT = Units.inchesToMeters(-0.5); 32 | public static final double MIN_HEIGHT = Units.feetToMeters(0.0); 33 | 34 | private static final DCMotor MOTOR = DCMotor.getFalcon500(2); 35 | private static final double REDUCTION = (16.0 / 36.0) * (28.0 / 36.0) * (18.0 / 48.0); 36 | private static final double MASS = Units.lbsToKilograms(60.0); 37 | private static final double RADIUS = Units.inchesToMeters(0.90); 38 | 39 | private static final double VELOCITY_CONSTANT = 1.0 / (RADIUS * REDUCTION * MOTOR.KvRadPerSecPerVolt); 40 | private static final double ACCELERATION_CONSTANT = (MOTOR.rOhms * RADIUS * MASS * REDUCTION) / (MOTOR.KtNMPerAmp); 41 | 42 | private static final double ALLOWABLE_POSITION_ERROR = Units.inchesToMeters(0.5); 43 | 44 | private static final double SENSOR_POSITION_COEFFICIENT = REDUCTION * RADIUS * 2 * Math.PI / 2048.0; 45 | private static final double SENSOR_VELOCITY_COEFFICIENT = SENSOR_POSITION_COEFFICIENT * 10.0; 46 | 47 | private static final MotionProfile.Constraints EVEN_FASTER_MOTION_CONSTRAINTS = new MotionProfile.Constraints( 48 | Units.feetToMeters(7.0), Units.feetToMeters(30.0)); 49 | private static final MotionProfile.Constraints FAST_MOTION_CONSTRAINTS = new MotionProfile.Constraints( 50 | Units.feetToMeters(6.0), Units.feetToMeters(12.0)); 51 | private static final MotionProfile.Constraints SLOW_MOTION_CONSTRAINTS = new MotionProfile.Constraints( 52 | Units.feetToMeters(3.0), Units.feetToMeters(6.0)); 53 | 54 | private final LinearSystem plant = LinearSystemId.identifyPositionSystem(VELOCITY_CONSTANT, 55 | ACCELERATION_CONSTANT); 56 | private final ElevatorSim simulation = new ElevatorSim(plant, MOTOR, 1.0 / REDUCTION, RADIUS, 0.0, 57 | MAX_HEIGHT * 1.1); 58 | private final TalonFX firstLeftMotor = new TalonFX(Constants.CLIMBER_FIRST_LEFT_MOTOR_PORT, "CANivore"); 59 | private final TalonFX firstRightMotor = new TalonFX(Constants.CLIMBER_FIRST_RIGHT_MOTOR_PORT, "CANivore"); 60 | private final TalonFX secondLeftMotor = new TalonFX(Constants.CLIMBER_SECOND_LEFT_MOTOR_PORT, "CANivore"); 61 | private final TalonFX secondRightMotor = new TalonFX(Constants.CLIMBER_SECOND_RIGHT_MOTOR_PORT, "CANivore"); 62 | 63 | private Mode mode = Mode.VOLTAGE; 64 | private boolean zeroed = false; 65 | private double targetHeight = 0.0; 66 | private double targetVoltage = 0.0; 67 | 68 | public ClimberSubsystem() { 69 | ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Climber"); 70 | shuffleboardTab.addNumber("Target Height", () -> Units.metersToInches(getTargetHeight())); 71 | shuffleboardTab.addNumber("Height", () -> Units.metersToInches(getCurrentHeight())); 72 | shuffleboardTab.addNumber("Velocity", () -> Units.metersToInches(getCurrentVelocity())); 73 | shuffleboardTab.addNumber("Voltage", () -> targetVoltage); 74 | shuffleboardTab.addString("Mode", () -> mode.name()); 75 | 76 | TalonFXConfiguration configuration = new TalonFXConfiguration(); 77 | configuration.motionCruiseVelocity = FAST_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT; 78 | configuration.motionAcceleration = FAST_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT; 79 | configuration.slot0.kP = 0.25; 80 | configuration.slot0.kI = 0.0; 81 | configuration.slot0.kD = 0.0; 82 | configuration.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice(); 83 | configuration.voltageCompSaturation = 12.0; 84 | 85 | firstLeftMotor.configAllSettings(configuration); 86 | firstRightMotor.configAllSettings(configuration); 87 | secondLeftMotor.configAllSettings(configuration); 88 | secondRightMotor.configAllSettings(configuration); 89 | 90 | firstLeftMotor.setNeutralMode(NeutralMode.Brake); 91 | firstRightMotor.setNeutralMode(NeutralMode.Brake); 92 | secondLeftMotor.setNeutralMode(NeutralMode.Brake); 93 | secondRightMotor.setNeutralMode(NeutralMode.Brake); 94 | 95 | firstRightMotor.setStatusFramePeriod(StatusFrame.Status_1_General, 255); 96 | firstRightMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 255); 97 | secondRightMotor.setStatusFramePeriod(StatusFrame.Status_1_General, 255); 98 | secondRightMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 255); 99 | firstLeftMotor.setStatusFramePeriod(StatusFrame.Status_1_General, 255); 100 | firstLeftMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 20); 101 | secondLeftMotor.setStatusFramePeriod(StatusFrame.Status_1_General, 255); 102 | secondLeftMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 255); 103 | 104 | firstLeftMotor.enableVoltageCompensation(true); 105 | firstRightMotor.enableVoltageCompensation(true); 106 | secondLeftMotor.enableVoltageCompensation(true); 107 | secondRightMotor.enableVoltageCompensation(true); 108 | 109 | firstRightMotor.setInverted(true); 110 | secondRightMotor.setInverted(true); 111 | } 112 | 113 | public double getTargetHeight() { 114 | if (mode == Mode.POSITION) { 115 | return targetHeight; 116 | } 117 | return 0.0; 118 | } 119 | 120 | public void setTargetHeight(double targetHeight) { 121 | this.targetHeight = MathUtils.clamp(targetHeight, Units.inchesToMeters(-10.0), MAX_HEIGHT); 122 | mode = Mode.POSITION; 123 | } 124 | 125 | public boolean isAtTargetPosition() { 126 | return Math.abs(getCurrentHeight() - getTargetHeight()) < ALLOWABLE_POSITION_ERROR; 127 | } 128 | 129 | public void setTargetVoltage(double targetVoltage) { 130 | this.targetVoltage = targetVoltage; 131 | mode = Mode.VOLTAGE; 132 | } 133 | 134 | public void setZeroed(boolean zeroed) { 135 | this.zeroed = zeroed; 136 | } 137 | 138 | public boolean isClimberZeroed() { 139 | return zeroed; 140 | } 141 | 142 | public double getCurrentHeight() { 143 | if (Robot.isSimulation()) { 144 | return simulation.getPositionMeters(); 145 | } else { 146 | return firstLeftMotor.getSelectedSensorPosition() * SENSOR_POSITION_COEFFICIENT; 147 | } 148 | } 149 | 150 | public double getCurrentVelocity() { 151 | if (Robot.isSimulation()) { 152 | return simulation.getVelocityMetersPerSecond(); 153 | } else { 154 | return firstLeftMotor.getSelectedSensorVelocity() * SENSOR_VELOCITY_COEFFICIENT; 155 | } 156 | } 157 | 158 | public void setZeroPosition() { 159 | if (Robot.isReal()) { 160 | firstLeftMotor.setSelectedSensorPosition(Units.inchesToMeters(-0.1875) / SENSOR_POSITION_COEFFICIENT); 161 | firstRightMotor.setSelectedSensorPosition(Units.inchesToMeters(-0.1875) / SENSOR_POSITION_COEFFICIENT); 162 | secondLeftMotor.setSelectedSensorPosition(Units.inchesToMeters(-0.1875) / SENSOR_POSITION_COEFFICIENT); 163 | secondRightMotor.setSelectedSensorPosition(Units.inchesToMeters(-0.1875) / SENSOR_POSITION_COEFFICIENT); 164 | } 165 | } 166 | 167 | public void setFastClimberConstraints(boolean fast) { 168 | if (fast) { 169 | firstLeftMotor 170 | .configMotionCruiseVelocity(FAST_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT); 171 | firstLeftMotor 172 | .configMotionAcceleration(FAST_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT); 173 | firstRightMotor 174 | .configMotionCruiseVelocity(FAST_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT); 175 | firstRightMotor 176 | .configMotionAcceleration(FAST_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT); 177 | secondLeftMotor 178 | .configMotionCruiseVelocity(FAST_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT); 179 | secondLeftMotor 180 | .configMotionAcceleration(FAST_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT); 181 | secondRightMotor 182 | .configMotionCruiseVelocity(FAST_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT); 183 | secondRightMotor 184 | .configMotionAcceleration(FAST_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT); 185 | } else { 186 | firstLeftMotor 187 | .configMotionCruiseVelocity(SLOW_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT); 188 | firstLeftMotor 189 | .configMotionAcceleration(SLOW_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT); 190 | firstRightMotor 191 | .configMotionCruiseVelocity(SLOW_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT); 192 | firstRightMotor 193 | .configMotionAcceleration(SLOW_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT); 194 | secondLeftMotor 195 | .configMotionCruiseVelocity(SLOW_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT); 196 | secondLeftMotor 197 | .configMotionAcceleration(SLOW_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT); 198 | secondRightMotor 199 | .configMotionCruiseVelocity(SLOW_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT); 200 | secondRightMotor 201 | .configMotionAcceleration(SLOW_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT); 202 | } 203 | } 204 | 205 | public void setEvenFasterMotionConstraints() { 206 | firstLeftMotor 207 | .configMotionCruiseVelocity(EVEN_FASTER_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT); 208 | firstLeftMotor 209 | .configMotionAcceleration(EVEN_FASTER_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT); 210 | firstRightMotor 211 | .configMotionCruiseVelocity(EVEN_FASTER_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT); 212 | firstRightMotor 213 | .configMotionAcceleration(EVEN_FASTER_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT); 214 | secondLeftMotor 215 | .configMotionCruiseVelocity(EVEN_FASTER_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT); 216 | secondLeftMotor 217 | .configMotionAcceleration(EVEN_FASTER_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT); 218 | secondRightMotor 219 | .configMotionCruiseVelocity(EVEN_FASTER_MOTION_CONSTRAINTS.maxVelocity / SENSOR_VELOCITY_COEFFICIENT); 220 | secondRightMotor 221 | .configMotionAcceleration(EVEN_FASTER_MOTION_CONSTRAINTS.maxAcceleration / SENSOR_VELOCITY_COEFFICIENT); 222 | } 223 | 224 | @Override 225 | public void simulationPeriodic() { 226 | simulation.setInputVoltage(targetVoltage); 227 | simulation.update(0.020); 228 | } 229 | 230 | @Override 231 | public void periodic() { 232 | switch (mode) { 233 | case POSITION : 234 | if (isClimberZeroed()) { 235 | firstLeftMotor.set(TalonFXControlMode.MotionMagic, targetHeight / SENSOR_POSITION_COEFFICIENT); 236 | firstRightMotor.set(TalonFXControlMode.MotionMagic, targetHeight / SENSOR_POSITION_COEFFICIENT); 237 | secondLeftMotor.set(TalonFXControlMode.MotionMagic, targetHeight / SENSOR_POSITION_COEFFICIENT); 238 | secondRightMotor.set(TalonFXControlMode.MotionMagic, targetHeight / SENSOR_POSITION_COEFFICIENT); 239 | } 240 | break; 241 | case VOLTAGE : 242 | firstLeftMotor.set(TalonFXControlMode.PercentOutput, targetVoltage / 12.0); 243 | firstRightMotor.set(TalonFXControlMode.PercentOutput, targetVoltage / 12.0); 244 | secondLeftMotor.set(TalonFXControlMode.PercentOutput, targetVoltage / 12.0); 245 | secondRightMotor.set(TalonFXControlMode.PercentOutput, targetVoltage / 12.0); 246 | break; 247 | } 248 | } 249 | 250 | private enum Mode { 251 | POSITION, VOLTAGE 252 | } 253 | } 254 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/util/AutonomousTrajectories.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.util; 2 | 3 | import edu.wpi.first.math.util.Units; 4 | import org.frcteam2910.common.control.SimplePathBuilder; 5 | import org.frcteam2910.common.control.Trajectory; 6 | import org.frcteam2910.common.control.TrajectoryConstraint; 7 | import org.frcteam2910.common.math.Rotation2; 8 | import org.frcteam2910.common.math.Vector2; 9 | 10 | public class AutonomousTrajectories { 11 | 12 | private static final double SAMPLE_DISTANCE = Units.inchesToMeters(0.1); 13 | 14 | private final Trajectory testAutoPartOne; 15 | private final Trajectory fenderRedPartOne; 16 | private final Trajectory fenderBluePartOne; 17 | private final Trajectory twoBallGreenPartOne; 18 | private final Trajectory twoBallPurplePartOne; 19 | private final Trajectory twoBallDefensivePartOne; 20 | private final Trajectory twoBallDefensivePartTwo; 21 | private final Trajectory threeBallOrangePartOne; 22 | private final Trajectory threeBallTwoPreloadOrangePartOne; 23 | private final Trajectory threeBallTwoPreloadOrangePartTwo; 24 | private final Trajectory threeBallOrangePartTwo; 25 | private final Trajectory threeBallOrangePartThree; 26 | private final Trajectory fiveBallTwoPreloadOrangePartOne; 27 | private final Trajectory fiveBallTwoPreloadOrangePartTwo; 28 | private final Trajectory fiveBallTwoPreloadOrangePartThree; 29 | private final Trajectory fiveBallOrangePartOne; 30 | private final Trajectory fiveBallOrangePartTwo; 31 | private final Trajectory fiveBallOrangePartThree; 32 | private final Trajectory fiveBallDefensivePartThree; 33 | private final Trajectory sixBallOrangePartOne; 34 | private final Trajectory sixBallOrangePartTwo; 35 | private final Trajectory sixBallOrangePartThree; 36 | private final Trajectory sixBallOrangePartFour; 37 | private final Trajectory sixBallOrangePartFive; 38 | private final Trajectory sixBallOrangePartSix; 39 | 40 | public AutonomousTrajectories(TrajectoryConstraint[] trajectoryConstraints) { 41 | testAutoPartOne = new Trajectory( 42 | new SimplePathBuilder(new Vector2(0.0, 0.0), Rotation2.fromDegrees(0.0)) 43 | .lineTo(new Vector2(Units.feetToMeters(30.0), 0.0)).build(), 44 | trajectoryConstraints, SAMPLE_DISTANCE); 45 | 46 | fenderRedPartOne = new Trajectory( 47 | new SimplePathBuilder(new Vector2(-1.119, 0.618), Rotation2.fromDegrees(-21.0)) 48 | .arcTo(new Vector2(-1.598, 1.096), new Vector2(-0.820, 1.396), Rotation2.fromDegrees(-21.0)) 49 | .lineTo(new Vector2(-2.434, 3.265), Rotation2.fromDegrees(24.0)).build(), 50 | trajectoryConstraints, SAMPLE_DISTANCE); 51 | 52 | fenderBluePartOne = new Trajectory( 53 | new SimplePathBuilder(new Vector2(-0.290, -1.245), Rotation2.fromDegrees(69.0)) 54 | .arcTo(new Vector2(-0.246, -1.926), new Vector2(0.475, -1.538), Rotation2.fromDegrees(69.0)) 55 | .lineTo(new Vector2(0.474, -3.265), Rotation2.fromDegrees(24.0)).build(), 56 | trajectoryConstraints, SAMPLE_DISTANCE); 57 | 58 | twoBallGreenPartOne = new Trajectory( 59 | new SimplePathBuilder(new Vector2(-2.386, 0.669), Rotation2.fromDegrees(1.5)) 60 | .arcTo(new Vector2(-4.356, 1.897), new Vector2(-2.327, 2.956), Rotation2.fromDegrees(-32.13)) 61 | .arcTo(new Vector2(-3.815, 2.396), new Vector2(-4.018, 2.073)) 62 | .lineTo(new Vector2(-3.294, 2.068)).build(), 63 | trajectoryConstraints, SAMPLE_DISTANCE); 64 | 65 | twoBallPurplePartOne = new Trajectory( 66 | new SimplePathBuilder(new Vector2(-1.765, -1.581), Rotation2.fromDegrees(46.5)) 67 | .arcTo(new Vector2(-3.477, -3.268), new Vector2(-3.472, -1.561), Rotation2.fromDegrees(34.88)) 68 | .arcTo(new Vector2(-3.693, -2.575), new Vector2(-3.475, -2.887)) 69 | .lineTo(new Vector2(-3.188, -2.222)).build(), 70 | trajectoryConstraints, SAMPLE_DISTANCE); 71 | 72 | twoBallDefensivePartOne = new Trajectory( 73 | new SimplePathBuilder(new Vector2(-3.294, 2.068), Rotation2.fromDegrees(-32.13)) 74 | .lineTo(new Vector2(-3.225, 2.141), Rotation2.fromDegrees(46.5)) 75 | .lineTo(new Vector2(-2.247, 3.171)).build(), 76 | trajectoryConstraints, SAMPLE_DISTANCE); 77 | 78 | twoBallDefensivePartTwo = new Trajectory( 79 | new SimplePathBuilder(new Vector2(-2.247, 3.171), Rotation2.fromDegrees(46.5)) 80 | .lineTo(new Vector2(-1.5, 0.4), Rotation2.fromDegrees(-21.0)).build(), 81 | trajectoryConstraints, SAMPLE_DISTANCE); 82 | 83 | threeBallOrangePartOne = new Trajectory( 84 | new SimplePathBuilder(new Vector2(-0.669, -2.386), Rotation2.fromDegrees(-88.5)) 85 | .lineTo(new Vector2(-0.66, -3.417), Rotation2.fromDegrees(-90.0)).build(), 86 | trajectoryConstraints, SAMPLE_DISTANCE); 87 | 88 | threeBallTwoPreloadOrangePartOne = new Trajectory( 89 | new SimplePathBuilder(new Vector2(-0.669, -2.386), Rotation2.fromDegrees(91.5)) 90 | .lineTo(new Vector2(-0.669, -2.396), Rotation2.fromDegrees(-90.0)) 91 | .lineTo(new Vector2(-0.66, -3.417)).build(), 92 | trajectoryConstraints, SAMPLE_DISTANCE); 93 | 94 | threeBallTwoPreloadOrangePartTwo = new Trajectory( 95 | new SimplePathBuilder(new Vector2(-0.66, -3.417), Rotation2.fromDegrees(-90.0)) 96 | .lineTo(new Vector2(-3.518, -3.268), Rotation2.fromDegrees(34.88)) 97 | .arcTo(new Vector2(-3.778, -2.358), new Vector2(-3.492, -2.768)) 98 | .lineTo(new Vector2(-3.34, -2.053)).build(), 99 | trajectoryConstraints, SAMPLE_DISTANCE); 100 | 101 | threeBallOrangePartTwo = new Trajectory( 102 | new SimplePathBuilder(new Vector2(-0.66, -3.417), Rotation2.fromDegrees(-90.0)) 103 | .lineTo(new Vector2(-3.200, -3.284), Rotation2.fromDegrees(34.88)) 104 | .arcTo(new Vector2(-3.950, -2.600), new Vector2(-3.150, -2.485)).build(), 105 | trajectoryConstraints, SAMPLE_DISTANCE); 106 | 107 | threeBallOrangePartThree = new Trajectory( 108 | new SimplePathBuilder(new Vector2(-3.950, -2.600), Rotation2.fromDegrees(34.88)) 109 | .lineTo(new Vector2(-3.540, -2.314)).build(), 110 | trajectoryConstraints, SAMPLE_DISTANCE); 111 | 112 | fiveBallTwoPreloadOrangePartOne = new Trajectory( 113 | new SimplePathBuilder(new Vector2(-3.34, -2.053), Rotation2.fromDegrees(34.88)) 114 | .lineTo(new Vector2(-5.34, -2.293), Rotation2.fromDegrees(-131.25)) 115 | .lineTo(new Vector2(-7.153, -2.511)).build(), 116 | trajectoryConstraints, SAMPLE_DISTANCE); 117 | 118 | fiveBallTwoPreloadOrangePartTwo = new Trajectory( 119 | new SimplePathBuilder(new Vector2(-7.153, -2.511), Rotation2.fromDegrees(-131.25)) 120 | .lineTo(new Vector2(-6.792, -2.166)).build(), 121 | trajectoryConstraints, SAMPLE_DISTANCE); 122 | 123 | fiveBallTwoPreloadOrangePartThree = new Trajectory( 124 | new SimplePathBuilder(new Vector2(-6.792, -2.166), Rotation2.fromDegrees(-131.25)) 125 | .lineTo(new Vector2(-2.5, -2.5), Rotation2.fromDegrees(45.0)).build(), 126 | trajectoryConstraints, SAMPLE_DISTANCE); 127 | 128 | fiveBallOrangePartOne = new Trajectory( 129 | new SimplePathBuilder(new Vector2(-3.540, -2.314), Rotation2.fromDegrees(34.88)) 130 | .lineTo(new Vector2(-5.537, -2.481), Rotation2.fromDegrees(-131.25)) 131 | .lineTo(new Vector2(-6.853, -2.511)).build(), 132 | trajectoryConstraints, SAMPLE_DISTANCE); 133 | 134 | fiveBallOrangePartTwo = new Trajectory( 135 | new SimplePathBuilder(new Vector2(-6.853, -2.511), Rotation2.fromDegrees(-131.25)) 136 | .lineTo(new Vector2(-6.669, -2.336)).build(), 137 | trajectoryConstraints, SAMPLE_DISTANCE); 138 | 139 | fiveBallOrangePartThree = new Trajectory( 140 | new SimplePathBuilder(new Vector2(-6.669, -2.336), Rotation2.fromDegrees(-131.25)) 141 | .lineTo(new Vector2(-2.500, -2.500), Rotation2.fromDegrees(45.0)).build(), 142 | trajectoryConstraints, SAMPLE_DISTANCE); 143 | 144 | fiveBallDefensivePartThree = new Trajectory( 145 | new SimplePathBuilder(new Vector2(-6.669, -2.336), Rotation2.fromDegrees(-131.25)) 146 | .lineTo(new Vector2(-4.127, -0.851), Rotation2.fromDegrees(16.4)).build(), 147 | trajectoryConstraints, SAMPLE_DISTANCE); 148 | 149 | sixBallOrangePartOne = new Trajectory( 150 | new SimplePathBuilder(new Vector2(-0.669, -2.386), Rotation2.fromDegrees(-88.5)) 151 | .lineTo(new Vector2(-0.66, -3.417), Rotation2.fromDegrees(-90.0)).build(), 152 | trajectoryConstraints, SAMPLE_DISTANCE); 153 | 154 | sixBallOrangePartTwo = new Trajectory( 155 | new SimplePathBuilder(new Vector2(-0.66, -3.417), Rotation2.fromDegrees(-90.0)) 156 | .lineTo(new Vector2(-3.2, -3.284)) 157 | .arcTo(new Vector2(-3.95, -2.6), new Vector2(-3.158, -2.485), Rotation2.fromDegrees(34.88)) 158 | .build(), 159 | trajectoryConstraints, SAMPLE_DISTANCE); 160 | 161 | sixBallOrangePartThree = new Trajectory( 162 | new SimplePathBuilder(new Vector2(-3.95, -2.6), Rotation2.fromDegrees(34.88)) 163 | .lineTo(new Vector2(-3.54, -2.314)).build(), 164 | trajectoryConstraints, SAMPLE_DISTANCE); 165 | 166 | sixBallOrangePartFour = new Trajectory( 167 | new SimplePathBuilder(new Vector2(-3.54, -2.314), Rotation2.fromDegrees(34.88)) 168 | .lineTo(new Vector2(-5.537, -2.481), Rotation2.fromDegrees(-131.25)) 169 | .lineTo(new Vector2(-6.853, -2.511)).build(), 170 | trajectoryConstraints, SAMPLE_DISTANCE); 171 | 172 | sixBallOrangePartFive = new Trajectory( 173 | new SimplePathBuilder(new Vector2(-6.853, -2.511), Rotation2.fromDegrees(-131.25)) 174 | .lineTo(new Vector2(-6.963, 0.638)) 175 | .arcTo(new Vector2(-3.593, 2.667), new Vector2(-4.960, 1.165), Rotation2.fromDegrees(-32.13)) 176 | .build(), 177 | trajectoryConstraints, SAMPLE_DISTANCE); 178 | 179 | sixBallOrangePartSix = new Trajectory( 180 | new SimplePathBuilder(new Vector2(-3.593, 2.667), Rotation2.fromDegrees(-32.13)) 181 | .lineTo(new Vector2(-2.893, 2.667)).build(), 182 | trajectoryConstraints, SAMPLE_DISTANCE); 183 | } 184 | 185 | public Trajectory getTestAutoPartOne() { 186 | return testAutoPartOne; 187 | } 188 | 189 | public Trajectory getFenderRedPartOne() { 190 | return fenderRedPartOne; 191 | } 192 | 193 | public Trajectory getFenderBluePartOne() { 194 | return fenderBluePartOne; 195 | } 196 | 197 | public Trajectory getTwoBallGreenPartOne() { 198 | return twoBallGreenPartOne; 199 | } 200 | 201 | public Trajectory getTwoBallPurplePartOne() { 202 | return twoBallPurplePartOne; 203 | } 204 | 205 | public Trajectory getTwoBallDefensivePartOne() { 206 | return twoBallDefensivePartOne; 207 | } 208 | 209 | public Trajectory getTwoBallDefensivePartTwo() { 210 | return twoBallDefensivePartTwo; 211 | } 212 | 213 | public Trajectory getThreeBallOrangePartOne() { 214 | return threeBallOrangePartOne; 215 | } 216 | 217 | public Trajectory getThreeBallTwoPreloadOrangePartOne() { 218 | return threeBallTwoPreloadOrangePartOne; 219 | } 220 | 221 | public Trajectory getThreeBallTwoPreloadOrangePartTwo() { 222 | return threeBallTwoPreloadOrangePartTwo; 223 | } 224 | 225 | public Trajectory getThreeBallOrangePartTwo() { 226 | return threeBallOrangePartTwo; 227 | } 228 | 229 | public Trajectory getThreeBallOrangePartThree() { 230 | return threeBallOrangePartThree; 231 | } 232 | 233 | public Trajectory getFiveBallTwoPreloadOrangePartOne() { 234 | return fiveBallTwoPreloadOrangePartOne; 235 | } 236 | 237 | public Trajectory getFiveBallTwoPreloadOrangePartTwo() { 238 | return fiveBallTwoPreloadOrangePartTwo; 239 | } 240 | 241 | public Trajectory getFiveBallTwoPreloadOrangePartThree() { 242 | return fiveBallTwoPreloadOrangePartThree; 243 | } 244 | 245 | public Trajectory getFiveBallOrangePartOne() { 246 | return fiveBallOrangePartOne; 247 | } 248 | 249 | public Trajectory getFiveBallOrangePartTwo() { 250 | return fiveBallOrangePartTwo; 251 | } 252 | 253 | public Trajectory getFiveBallOrangePartThree() { 254 | return fiveBallOrangePartThree; 255 | } 256 | 257 | public Trajectory getFiveBallDefensivePartThree() { 258 | return fiveBallDefensivePartThree; 259 | } 260 | 261 | public Trajectory getSixBallOrangePartOne() { 262 | return sixBallOrangePartOne; 263 | } 264 | 265 | public Trajectory getSixBallOrangePartTwo() { 266 | return sixBallOrangePartTwo; 267 | } 268 | 269 | public Trajectory getSixBallOrangePartThree() { 270 | return sixBallOrangePartThree; 271 | } 272 | 273 | public Trajectory getSixBallOrangePartFour() { 274 | return sixBallOrangePartFour; 275 | } 276 | 277 | public Trajectory getSixBallOrangePartFive() { 278 | return sixBallOrangePartFive; 279 | } 280 | 281 | public Trajectory getSixBallOrangePartSix() { 282 | return sixBallOrangePartSix; 283 | } 284 | } 285 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/subsystems/ShooterSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.subsystems; 2 | 3 | import java.util.Map; 4 | 5 | import com.ctre.phoenix.motorcontrol.can.TalonFX; 6 | import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; 7 | import com.ctre.phoenix.motorcontrol.*; 8 | import edu.wpi.first.math.numbers.N1; 9 | import edu.wpi.first.math.numbers.N2; 10 | import edu.wpi.first.math.system.LinearSystem; 11 | import edu.wpi.first.math.system.plant.DCMotor; 12 | import edu.wpi.first.math.system.plant.LinearSystemId; 13 | import edu.wpi.first.math.util.Units; 14 | import edu.wpi.first.networktables.NetworkTableEntry; 15 | import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; 16 | import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; 17 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 18 | import edu.wpi.first.wpilibj.simulation.FlywheelSim; 19 | import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; 20 | import edu.wpi.first.wpilibj2.command.Subsystem; 21 | import org.frcteam2910.c2022.Constants; 22 | import org.frcteam2910.c2022.Robot; 23 | import org.frcteam2910.common.math.MathUtils; 24 | import org.frcteam2910.common.motion.MotionProfile; 25 | 26 | public class ShooterSubsystem implements Subsystem { 27 | public static final double HOOD_MAX_ANGLE = Math.toRadians(85.2); 28 | public static final double HOOD_PREPARE_TRANSFER_ANGLE = Math.toRadians(84.2); 29 | public static final double HOOD_TRANSFER_ANGLE = Math.toRadians(76.4); 30 | public static final double HOOD_TRAVERSE_RETRACT_ANGLE = Math.toRadians(45.0); 31 | public static final double HOOD_TRAVERSE_EXTEND_ANGLE_HIGH = Math.toRadians(20.0); 32 | public static final double HOOD_TRAVERSE_EXTEND_ANGLE_TRAVERSE = Math.toRadians(25.0); 33 | public static final double HOOD_MIN_ANGLE = Math.toRadians(0.0); 34 | 35 | private static final double HOOD_MOMENT_OF_INERTIA = Units.lbsToKilograms(Units.inchesToMeters(450)); 36 | private static final double HOOD_GEAR_REDUCTION = (14.0 / 54.0) * (18.0 / 38.0) * (38.0 / 18.0) * (10.0 / 220.0); 37 | private static final double FLYWHEEL_GEAR_REDUCTION = 36.0 / 53.0; 38 | private static final double FLYWHEEL_ALLOWABLE_ERROR = Units.rotationsPerMinuteToRadiansPerSecond(75); 39 | 40 | private static final DCMotor HOOD_MOTOR = DCMotor.getFalcon500(1); 41 | private static final double HOOD_VELOCITY_CONSTANT = 5.5657; 42 | private static final double HOOD_ACCELERATION_CONSTANT = 0.098378; 43 | private static final double HOOD_SENSOR_POSITION_COEFFICIENT = (HOOD_GEAR_REDUCTION / 2048.0) * 2 * Math.PI; 44 | private static final double HOOD_SENSOR_VELOCITY_COEFFICIENT = HOOD_SENSOR_POSITION_COEFFICIENT * 10.0; 45 | 46 | private static final double FLYWHEEL_VELOCITY_CONSTANT = 0.028; 47 | private static final double FLYWHEEL_ACCELERATION_CONSTANT = 0.0030108; 48 | private static final double FLYWHEEL_SENSOR_POSITION_COEFFICIENT = (FLYWHEEL_GEAR_REDUCTION / 2048.0) * 2 * Math.PI; 49 | private static final double FLYWHEEL_SENSOR_VELOCITY_COEFFICIENT = FLYWHEEL_SENSOR_POSITION_COEFFICIENT * 10.0; 50 | public static final double FLYWHEEL_IDLE_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(2000); 51 | 52 | private static final MotionProfile.Constraints FAST_MOTION_CONSTRAINTS = new MotionProfile.Constraints( 53 | Math.toRadians(450.0), Math.toRadians(2000.0)); 54 | private static final MotionProfile.Constraints SLOW_MOTION_CONSTRAINTS = new MotionProfile.Constraints( 55 | Math.toRadians(450.0), Math.toRadians(500.0)); 56 | 57 | private final TalonFX hoodAngleMotor = new TalonFX(Constants.HOOD_MOTOR_PORT); 58 | private final LinearSystem hoodPlant = LinearSystemId 59 | .createSingleJointedArmSystem(DCMotor.getFalcon500(2), HOOD_MOMENT_OF_INERTIA, HOOD_GEAR_REDUCTION); 60 | private final SingleJointedArmSim hoodSim = new SingleJointedArmSim(hoodPlant, HOOD_MOTOR, 61 | 1.0 / HOOD_GEAR_REDUCTION, 0.0, 0.0, Math.PI, 0.0, false); 62 | 63 | private final FlywheelSim flywheel = new FlywheelSim(DCMotor.getFalcon500(2), 1.0, Units.inchesToMeters(6)); 64 | private final TalonFX flywheelPrimaryMotor = new TalonFX(Constants.FLYWHEEL_PRIMARY_MOTOR_PORT); 65 | private final TalonFX flywheelSecondaryMotor = new TalonFX(Constants.FLYWHEEL_SECONDARY_MOTOR_PORT); 66 | 67 | private double flywheelVoltage; 68 | private double hoodVoltage; 69 | private double hoodTargetAngle = Math.toRadians(5.0); 70 | private boolean isHoodZeroed = false; 71 | private boolean flywheelDisabled = false; 72 | private double targetFlywheelSpeed; 73 | private double shootingOffset = 0.0; 74 | private double angleOffset = 0.0; 75 | private final NetworkTableEntry shooterRPMOffsetEntry; 76 | private final NetworkTableEntry hoodAngleOffsetEntry; 77 | 78 | public ShooterSubsystem() { 79 | ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Shooter"); 80 | shuffleboardTab.addNumber("Flywheel Speed", 81 | () -> Units.radiansPerSecondToRotationsPerMinute(getFlywheelVelocity())); 82 | shuffleboardTab.addNumber("Target Flywheel Speed", 83 | () -> Units.radiansPerSecondToRotationsPerMinute(getTargetFlywheelSpeed())); 84 | shuffleboardTab.addNumber("Hood Target Angle", () -> Math.toDegrees(getHoodTargetPosition())); 85 | shuffleboardTab.addNumber("Hood Angle", () -> Math.toDegrees(getHoodAngle())); 86 | shuffleboardTab.addNumber("Hood Velocity", () -> Math.toDegrees(getHoodVelocity())); 87 | shuffleboardTab.addNumber("Hood Voltage", () -> hoodVoltage); 88 | shuffleboardTab.addBoolean("Is Hood at Angle", this::isHoodAtTargetAngle); 89 | shuffleboardTab.addBoolean("Is Flywheel at Speed", this::isFlywheelAtTargetSpeed); 90 | shooterRPMOffsetEntry = Shuffleboard.getTab(Constants.DRIVER_READOUT_TAB_NAME) 91 | .add(Constants.SHOOTER_OFFSET_ENTRY_NAME, 0.0).withWidget(BuiltInWidgets.kNumberSlider) 92 | .withProperties(Map.of("min", -250.0, "max", 250.0, "Block increment", 25.0)).withPosition(2, 1) 93 | .getEntry(); 94 | hoodAngleOffsetEntry = Shuffleboard.getTab(Constants.DRIVER_READOUT_TAB_NAME) 95 | .add(Constants.HOOD_OFFSET_ENTRY_NAME, 0.0).withWidget(BuiltInWidgets.kNumberSlider) 96 | .withProperties(Map.of("min", -10.0, "max", 10.0, "Block increment", 0.5)).withPosition(2, 2) 97 | .getEntry(); 98 | 99 | TalonFXConfiguration flywheelConfiguration = new TalonFXConfiguration(); 100 | flywheelConfiguration.supplyCurrLimit.currentLimit = 20.0; 101 | flywheelConfiguration.supplyCurrLimit.enable = false; 102 | flywheelConfiguration.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.IntegratedSensor 103 | .toFeedbackDevice(); 104 | flywheelConfiguration.slot0.kP = 0.25; 105 | flywheelConfiguration.slot0.kI = 0.0; 106 | flywheelConfiguration.slot0.kD = 0.0; 107 | 108 | flywheelPrimaryMotor.configAllSettings(flywheelConfiguration); 109 | flywheelSecondaryMotor.configAllSettings(flywheelConfiguration); 110 | 111 | flywheelPrimaryMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); 112 | flywheelPrimaryMotor.setStatusFramePeriod(StatusFrame.Status_1_General, 255); 113 | flywheelPrimaryMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 20); 114 | flywheelSecondaryMotor.setStatusFramePeriod(StatusFrame.Status_1_General, 255); 115 | flywheelSecondaryMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 255); 116 | 117 | TalonFXConfiguration hoodConfiguration = new TalonFXConfiguration(); 118 | hoodConfiguration.motionCruiseVelocity = FAST_MOTION_CONSTRAINTS.maxVelocity / HOOD_SENSOR_VELOCITY_COEFFICIENT; 119 | hoodConfiguration.motionAcceleration = FAST_MOTION_CONSTRAINTS.maxAcceleration 120 | / HOOD_SENSOR_VELOCITY_COEFFICIENT; 121 | hoodConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.IntegratedSensor; 122 | hoodConfiguration.slot0.kP = 0.5; 123 | hoodConfiguration.slot0.kI = 0.0; 124 | hoodConfiguration.slot0.kD = 0.0; 125 | hoodConfiguration.supplyCurrLimit.currentLimit = 50.0; 126 | hoodConfiguration.supplyCurrLimit.enable = true; 127 | 128 | hoodAngleMotor.configAllSettings(hoodConfiguration); 129 | 130 | hoodAngleMotor.setStatusFramePeriod(StatusFrame.Status_1_General, 255); 131 | hoodAngleMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 20); 132 | hoodAngleMotor.setNeutralMode(NeutralMode.Brake); 133 | 134 | flywheelPrimaryMotor.configVoltageCompSaturation(12.0); 135 | flywheelSecondaryMotor.configVoltageCompSaturation(12.0); 136 | flywheelPrimaryMotor.enableVoltageCompensation(true); 137 | flywheelSecondaryMotor.enableVoltageCompensation(true); 138 | 139 | flywheelPrimaryMotor.setNeutralMode(NeutralMode.Coast); 140 | flywheelSecondaryMotor.setNeutralMode(NeutralMode.Coast); 141 | flywheelPrimaryMotor.setInverted(true); 142 | flywheelSecondaryMotor.setInverted(true); 143 | } 144 | 145 | public double getHoodAngle() { 146 | if (Robot.isSimulation()) { 147 | return hoodSim.getAngleRads(); 148 | } else { 149 | return hoodAngleMotor.getSelectedSensorPosition() * HOOD_SENSOR_POSITION_COEFFICIENT; 150 | } 151 | } 152 | 153 | public void setFlywheelVoltage(double flywheelVoltage) { 154 | this.flywheelVoltage = flywheelVoltage; 155 | } 156 | 157 | public void setHoodVoltage(double hoodVoltage) { 158 | this.hoodVoltage = hoodVoltage; 159 | } 160 | 161 | public double getHoodVelocity() { 162 | if (Robot.isSimulation()) { 163 | return hoodSim.getVelocityRadPerSec(); 164 | } else { 165 | return hoodAngleMotor.getSelectedSensorVelocity() * HOOD_SENSOR_VELOCITY_COEFFICIENT; 166 | } 167 | } 168 | 169 | public void setHoodZeroed(boolean zeroed) { 170 | this.isHoodZeroed = zeroed; 171 | } 172 | 173 | public boolean isHoodAtTargetAngle() { 174 | return isHoodAtTargetAngle(false); 175 | } 176 | 177 | public boolean isHoodAtTargetAngle(boolean climbing) { 178 | if (Robot.isSimulation()) { 179 | return Math.abs(getHoodTargetPosition() - hoodSim.getAngleRads()) < Constants.HOOD_SHOOTING_ALLOWABLE_ERROR; 180 | } else if (climbing) { 181 | return Math.abs(getHoodTargetPosition() - getHoodAngle()) < Constants.HOOD_CLIMBING_ALLOWABLE_ERROR; 182 | } else { 183 | return Math.abs(getHoodTargetPosition() - getHoodAngle()) < Constants.HOOD_SHOOTING_ALLOWABLE_ERROR; 184 | } 185 | } 186 | 187 | public void setTargetFlywheelSpeed(double targetFlywheelSpeed) { 188 | this.targetFlywheelSpeed = targetFlywheelSpeed + Units.rotationsPerMinuteToRadiansPerSecond(shootingOffset); 189 | } 190 | 191 | public double getTargetFlywheelSpeed() { 192 | return targetFlywheelSpeed; 193 | } 194 | 195 | public double getFlywheelVelocity() { 196 | if (Robot.isSimulation()) { 197 | return flywheel.getAngularVelocityRadPerSec(); 198 | } else { 199 | return flywheelPrimaryMotor.getSelectedSensorVelocity() * FLYWHEEL_SENSOR_VELOCITY_COEFFICIENT; 200 | } 201 | } 202 | 203 | public boolean isFlywheelAtTargetSpeed() { 204 | return Math.abs(getFlywheelVelocity() - targetFlywheelSpeed) < FLYWHEEL_ALLOWABLE_ERROR; 205 | } 206 | 207 | public void setHoodTargetPosition(double position) { 208 | setHoodTargetPosition(position, false); 209 | } 210 | 211 | public void setHoodTargetPosition(double position, boolean offset) { 212 | if (!offset) { 213 | hoodTargetAngle = MathUtils.clamp(position, HOOD_MIN_ANGLE, HOOD_MAX_ANGLE); 214 | } else { 215 | hoodTargetAngle = MathUtils.clamp(position + Units.degreesToRadians(angleOffset), HOOD_MIN_ANGLE, 216 | HOOD_MAX_ANGLE); 217 | } 218 | } 219 | 220 | public double getHoodTargetPosition() { 221 | return hoodTargetAngle; 222 | } 223 | 224 | public boolean isHoodZeroed() { 225 | return isHoodZeroed; 226 | } 227 | 228 | public void setHoodMotorSensorPosition(double position) { 229 | hoodAngleMotor.setSelectedSensorPosition(position / HOOD_SENSOR_POSITION_COEFFICIENT); 230 | } 231 | 232 | public void simulationPeriodic() { 233 | flywheel.setInputVoltage(flywheelVoltage); 234 | flywheel.update(0.02); 235 | 236 | hoodSim.setInputVoltage(hoodVoltage); 237 | hoodSim.update(0.02); 238 | } 239 | 240 | public void enableCurrentLimits(boolean enabled) { 241 | SupplyCurrentLimitConfiguration configuration = new SupplyCurrentLimitConfiguration(); 242 | configuration.currentLimit = 20.0; 243 | configuration.enable = enabled; 244 | flywheelPrimaryMotor.configSupplyCurrentLimit(configuration); 245 | flywheelSecondaryMotor.configSupplyCurrentLimit(configuration); 246 | } 247 | 248 | public void setHoodBrakeMode(boolean brake) { 249 | if (brake) { 250 | hoodAngleMotor.setNeutralMode(NeutralMode.Brake); 251 | } else { 252 | hoodAngleMotor.setNeutralMode(NeutralMode.Coast); 253 | } 254 | } 255 | 256 | public void setFastHoodConfig(boolean fast) { 257 | if (fast) { 258 | hoodAngleMotor 259 | .configMotionCruiseVelocity(FAST_MOTION_CONSTRAINTS.maxVelocity / HOOD_SENSOR_VELOCITY_COEFFICIENT); 260 | hoodAngleMotor.configMotionAcceleration( 261 | FAST_MOTION_CONSTRAINTS.maxAcceleration / HOOD_SENSOR_VELOCITY_COEFFICIENT); 262 | } else { 263 | hoodAngleMotor 264 | .configMotionCruiseVelocity(SLOW_MOTION_CONSTRAINTS.maxVelocity / HOOD_SENSOR_VELOCITY_COEFFICIENT); 265 | hoodAngleMotor.configMotionAcceleration( 266 | SLOW_MOTION_CONSTRAINTS.maxAcceleration / HOOD_SENSOR_VELOCITY_COEFFICIENT); 267 | } 268 | } 269 | 270 | public void disableFlywheel() { 271 | flywheelDisabled = true; 272 | } 273 | 274 | @Override 275 | public void periodic() { 276 | if (isHoodZeroed) { 277 | hoodAngleMotor.set(TalonFXControlMode.MotionMagic, hoodTargetAngle / HOOD_SENSOR_POSITION_COEFFICIENT); 278 | } else { 279 | hoodAngleMotor.set(TalonFXControlMode.PercentOutput, hoodVoltage / 12); 280 | } 281 | 282 | if (flywheelDisabled) { 283 | flywheelPrimaryMotor.set(TalonFXControlMode.PercentOutput, 0.0); 284 | flywheelSecondaryMotor.set(TalonFXControlMode.PercentOutput, 0.0); 285 | } else { 286 | double feedForward = FLYWHEEL_VELOCITY_CONSTANT * targetFlywheelSpeed / 12.0; 287 | flywheelPrimaryMotor.set(TalonFXControlMode.Velocity, 288 | targetFlywheelSpeed / FLYWHEEL_SENSOR_VELOCITY_COEFFICIENT, DemandType.ArbitraryFeedForward, 289 | feedForward); 290 | flywheelSecondaryMotor.set(TalonFXControlMode.Velocity, 291 | targetFlywheelSpeed / FLYWHEEL_SENSOR_VELOCITY_COEFFICIENT, DemandType.ArbitraryFeedForward, 292 | feedForward); 293 | } 294 | 295 | shootingOffset = shooterRPMOffsetEntry.getDouble(0.0); 296 | angleOffset = hoodAngleOffsetEntry.getDouble(0.0); 297 | } 298 | } -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2022/util/AutonomousChooser.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2022.util; 2 | 3 | import java.util.function.BooleanSupplier; 4 | 5 | import edu.wpi.first.math.geometry.Pose2d; 6 | import edu.wpi.first.math.geometry.Rotation2d; 7 | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; 8 | import edu.wpi.first.wpilibj2.command.*; 9 | import org.frcteam2910.c2022.RobotContainer; 10 | import org.frcteam2910.c2022.commands.*; 11 | import org.frcteam2910.common.control.Path; 12 | import org.frcteam2910.common.control.Trajectory; 13 | 14 | public class AutonomousChooser { 15 | private final AutonomousTrajectories trajectories; 16 | 17 | private final SendableChooser autonomousModeChooser = new SendableChooser<>(); 18 | 19 | public AutonomousChooser(AutonomousTrajectories trajectories) { 20 | this.trajectories = trajectories; 21 | 22 | autonomousModeChooser.setDefaultOption("Five Ball (Orange)", AutonomousMode.FIVE_BALL_ORANGE); 23 | autonomousModeChooser.addOption("Test Auto", AutonomousMode.TEST_AUTO); 24 | autonomousModeChooser.addOption("Fender (Red)", AutonomousMode.FENDER_RED); 25 | autonomousModeChooser.addOption("Fender (Blue)", AutonomousMode.FENDER_BLUE); 26 | autonomousModeChooser.addOption("Two Ball (Green)", AutonomousMode.TWO_BALL_GREEN); 27 | autonomousModeChooser.addOption("Two Ball (Purple)", AutonomousMode.TWO_BALL_PURPLE); 28 | autonomousModeChooser.addOption("Three Ball (Orange)", AutonomousMode.THREE_BALL_ORANGE); 29 | autonomousModeChooser.addOption("Five Ball (Orange)", AutonomousMode.FIVE_BALL_ORANGE); 30 | autonomousModeChooser.addOption("Five Ball Defensive (Orange)", AutonomousMode.FIVE_BALL_DEFENSIVE); 31 | autonomousModeChooser.addOption("Six Ball (Orange)", AutonomousMode.SIX_BALL_ORANGE); 32 | autonomousModeChooser.addOption("Two Ball Defensive (Green)", AutonomousMode.TWO_BALL_DEFENSIVE); 33 | autonomousModeChooser.addOption("Fender Double Preload (Red)", AutonomousMode.FENDER_DOUBLE_PRELOAD_RED); 34 | autonomousModeChooser.addOption("Two Ball Double Preload (Green)", 35 | AutonomousMode.TWO_BALL_DOUBLE_PRELOAD_GREEN); 36 | autonomousModeChooser.addOption("Three Ball Double Preload (Orange)", 37 | AutonomousMode.THREE_BALL_DOUBLE_PRELOAD_ORANGE); 38 | autonomousModeChooser.addOption("Five Ball Double Preload (Orange)", 39 | AutonomousMode.FIVE_BALL_DOUBLE_PRELOAD_ORANGE); 40 | } 41 | 42 | public SendableChooser getModeChooser() { 43 | return autonomousModeChooser; 44 | } 45 | 46 | public Command getTestAuto(RobotContainer container) { 47 | SequentialCommandGroup command = new SequentialCommandGroup(); 48 | 49 | resetRobotPose(command, container, trajectories.getTestAutoPartOne()); 50 | 51 | command.addCommands(follow(container, trajectories.getTestAutoPartOne())); 52 | 53 | return command; 54 | } 55 | 56 | public Command getFenderBlueAuto(RobotContainer container) { 57 | SequentialCommandGroup command = new SequentialCommandGroup(); 58 | 59 | resetRobotPose(command, container, trajectories.getFenderBluePartOne()); 60 | command.addCommands(new ZeroHoodCommand(container.getShooter(), true)); 61 | 62 | command.addCommands(new FenderShootCommand(container.getFeeder(), container.getShooter()).withTimeout(1.5)); 63 | command.addCommands(follow(container, trajectories.getFenderBluePartOne())); 64 | 65 | return command; 66 | } 67 | 68 | public Command getFenderRedAuto(RobotContainer container) { 69 | SequentialCommandGroup command = new SequentialCommandGroup(); 70 | 71 | resetRobotPose(command, container, trajectories.getFenderRedPartOne()); 72 | 73 | command.addCommands(new ZeroHoodCommand(container.getShooter(), true)); 74 | 75 | command.addCommands(new FenderShootCommand(container.getFeeder(), container.getShooter()).withTimeout(2.5)); 76 | command.addCommands(follow(container, trajectories.getFenderRedPartOne())); 77 | 78 | return command; 79 | } 80 | 81 | public Command get2BallPurpleAuto(RobotContainer container) { 82 | SequentialCommandGroup command = new SequentialCommandGroup(); 83 | 84 | resetRobotPose(command, container, trajectories.getTwoBallPurplePartOne()); 85 | 86 | command.addCommands(followAndIntake(container, trajectories.getTwoBallPurplePartOne()) 87 | .alongWith(new ZeroHoodCommand(container.getShooter(), true))); 88 | shootAtTarget(command, container, 1.5); 89 | 90 | return command; 91 | } 92 | 93 | public Command get2BallGreenAuto(RobotContainer container) { 94 | SequentialCommandGroup command = new SequentialCommandGroup(); 95 | 96 | resetRobotPose(command, container, trajectories.getTwoBallGreenPartOne()); 97 | 98 | command.addCommands(followAndIntake(container, trajectories.getTwoBallGreenPartOne()) 99 | .alongWith(new ZeroHoodCommand(container.getShooter(), true))); 100 | shootAtTarget(command, container, 2.0); 101 | 102 | return command; 103 | } 104 | 105 | public Command get3BallOrangeAuto(RobotContainer container) { 106 | SequentialCommandGroup command = new SequentialCommandGroup(); 107 | 108 | resetRobotPose(command, container, trajectories.getThreeBallOrangePartOne()); 109 | 110 | // Grab the second ball and move to the shooting position 111 | // Zero the hood along the way 112 | command.addCommands(followAndIntake(container, trajectories.getThreeBallOrangePartOne()) 113 | .andThen(follow(container, trajectories.getThreeBallOrangePartTwo())) 114 | .alongWith(new ZeroHoodCommand(container.getShooter(), true, true))); 115 | 116 | // Shoot the 1st and 2nd balls 117 | shootAtTarget(command, container, 1.5); 118 | 119 | // Grab the 3rd ball 120 | command.addCommands(followAndIntake(container, trajectories.getThreeBallOrangePartThree())); 121 | 122 | // Shoot the 3rd ball 123 | shootAtTarget(command, container, 1.5); 124 | 125 | return command; 126 | } 127 | 128 | public Command get5BallOrangeAuto(RobotContainer container) { 129 | SequentialCommandGroup command = new SequentialCommandGroup(); 130 | 131 | // First run the three ball 132 | command.addCommands(get3BallOrangeAuto(container)); 133 | 134 | // Grab the 4th ball and wait for the 5th 135 | command.addCommands(followAndIntake(container, trajectories.getFiveBallOrangePartOne(), 136 | () -> container.getFeeder().isFull(), 0.0)); 137 | command.addCommands(followAndIntake(container, trajectories.getFiveBallOrangePartTwo(), 138 | () -> container.getFeeder().isFull(), 2.0)); 139 | 140 | // Go to shooting location 141 | command.addCommands(follow(container, trajectories.getFiveBallOrangePartThree())); 142 | 143 | // Shoot the 4th and 5th balls 144 | shootAtTarget(command, container, 1.5); 145 | 146 | return command; 147 | } 148 | 149 | public Command get6BallOrangeAuto(RobotContainer container) { 150 | SequentialCommandGroup command = new SequentialCommandGroup(); 151 | 152 | resetRobotPose(command, container, trajectories.getSixBallOrangePartOne()); 153 | 154 | // Grab the second ball and move to the shooting position 155 | // Zero the hood along the way 156 | command.addCommands(followAndIntake(container, trajectories.getSixBallOrangePartOne()) 157 | .andThen(follow(container, trajectories.getSixBallOrangePartTwo())) 158 | .alongWith(new ZeroHoodCommand(container.getShooter(), true, true))); 159 | 160 | // Shoot the 1st and 2nd balls 161 | shootAtTarget(command, container, 1.5); 162 | 163 | // Grab the 3rd ball 164 | command.addCommands(followAndIntake(container, trajectories.getSixBallOrangePartThree())); 165 | 166 | // Shoot the 3rd ball 167 | shootAtTarget(command, container, 1.0); 168 | 169 | // Grab the 4th ball and wait for the 5th 170 | command.addCommands(followAndIntake(container, trajectories.getSixBallOrangePartFour(), 171 | () -> container.getFeeder().isFull(), 1.0)); 172 | 173 | // Retract Intake 174 | command.addCommands(new InstantCommand(() -> container.getIntake().setExtended(false))); 175 | 176 | // Pass through the hangar to the 6th 177 | command.addCommands(follow(container, trajectories.getSixBallOrangePartFive())); 178 | command.addCommands(new InstantCommand(() -> container.getIntake().setExtended(true))); 179 | 180 | // Shoot 4th and 5th 181 | shootAtTarget(command, container, 1.5); 182 | 183 | // Intake 6th 184 | command.addCommands(followAndIntake(container, trajectories.getSixBallOrangePartSix())); 185 | 186 | // Shoot 6th 187 | shootAtTarget(command, container, 1.5); 188 | 189 | return command; 190 | } 191 | 192 | public Command get2BallDefensiveAuto(RobotContainer container) { 193 | SequentialCommandGroup command = new SequentialCommandGroup(); 194 | 195 | // Run 2 ball white 196 | command.addCommands(get2BallGreenAuto(container)); 197 | 198 | // Pick up both opponent ball 199 | command.addCommands(followAndIntake(container, trajectories.getTwoBallDefensivePartOne())); 200 | 201 | // Move up to hub 202 | command.addCommands(new InstantCommand(() -> container.getIntake().setExtended(false))); 203 | command.addCommands(follow(container, trajectories.getTwoBallDefensivePartTwo())); 204 | 205 | // Reverse feed ball 206 | command.addCommands(new ResetFeederCommand(container.getFeeder(), container.getIntake())); 207 | 208 | return command; 209 | } 210 | 211 | public Command getFenderTwoPreloadRedAuto(RobotContainer container) { 212 | SequentialCommandGroup command = new SequentialCommandGroup(); 213 | 214 | resetRobotPose(command, container, trajectories.getFenderRedPartOne()); 215 | 216 | command.addCommands( 217 | new SimpleIntakeCommand(container.getIntake(), container.getFeeder(), container.getController()) 218 | .withTimeout(1.0).alongWith(new ZeroHoodCommand(container.getShooter(), true, true))); 219 | 220 | command.addCommands(new FenderShootCommand(container.getFeeder(), container.getShooter()).withTimeout(2.5)); 221 | command.addCommands(follow(container, trajectories.getFenderRedPartOne())); 222 | 223 | return command; 224 | } 225 | 226 | public Command get2BallTwoPreloadGreenAuto(RobotContainer container) { 227 | SequentialCommandGroup command = new SequentialCommandGroup(); 228 | 229 | resetRobotPose(command, container, trajectories.getTwoBallGreenPartOne()); 230 | 231 | // Grab the 2nd preloaded ball 232 | // Zero the hood along the way 233 | command.addCommands( 234 | new SimpleIntakeCommand(container.getIntake(), container.getFeeder(), container.getController()) 235 | .withTimeout(1.0).alongWith(new ZeroHoodCommand(container.getShooter(), true, true))); 236 | 237 | // Shoot preloaded balls 238 | shootAtTarget(command, container, 1.5); 239 | 240 | command.addCommands(followAndIntake(container, trajectories.getTwoBallGreenPartOne())); 241 | shootAtTarget(command, container, 2.0); 242 | 243 | return command; 244 | } 245 | 246 | public Command get3BallTwoPreloadOrangeAuto(RobotContainer container) { 247 | SequentialCommandGroup command = new SequentialCommandGroup(); 248 | 249 | resetRobotPose(command, container, trajectories.getThreeBallTwoPreloadOrangePartOne()); 250 | 251 | // Grab the 2nd preloaded ball 252 | // Zero the hood along the way 253 | command.addCommands( 254 | new SimpleIntakeCommand(container.getIntake(), container.getFeeder(), container.getController()) 255 | .withTimeout(1.0).alongWith(new ZeroHoodCommand(container.getShooter(), true, true))); 256 | 257 | // Shoot preloaded balls 258 | shootAtTarget(command, container, 1.5); 259 | 260 | // Grab the 2nd ball 261 | command.addCommands(followAndIntake(container, trajectories.getThreeBallTwoPreloadOrangePartOne())); 262 | 263 | // Grab the 3rd ball 264 | command.addCommands(followAndIntake(container, trajectories.getThreeBallTwoPreloadOrangePartTwo())); 265 | 266 | // Shoot the 2nd and 3rd ball 267 | shootAtTarget(command, container, 1.5); 268 | 269 | return command; 270 | } 271 | 272 | public Command get5BallTwoPreloadOrangeAuto(RobotContainer container) { 273 | SequentialCommandGroup command = new SequentialCommandGroup(); 274 | 275 | // First run the three ball 276 | command.addCommands(get3BallTwoPreloadOrangeAuto(container)); 277 | 278 | // Grab the 4th ball and wait for the 5th 279 | command.addCommands(followAndIntake(container, trajectories.getFiveBallTwoPreloadOrangePartOne(), 280 | () -> container.getFeeder().isFull(), 0.0)); 281 | command.addCommands(followAndIntake(container, trajectories.getFiveBallTwoPreloadOrangePartTwo(), 282 | () -> container.getFeeder().isFull(), 2.0)); 283 | 284 | // Go to shooting location 285 | command.addCommands(follow(container, trajectories.getFiveBallTwoPreloadOrangePartThree())); 286 | 287 | // Shoot the 4th and 5th balls 288 | shootAtTarget(command, container, 1.5); 289 | 290 | return command; 291 | } 292 | 293 | public Command get5BallDefensiveAuto(RobotContainer container) { 294 | SequentialCommandGroup command = new SequentialCommandGroup(); 295 | 296 | // First run the three ball 297 | command.addCommands(get3BallOrangeAuto(container)); 298 | 299 | // Grab the 4th ball and wait for the 5th 300 | command.addCommands(followAndIntake(container, trajectories.getFiveBallOrangePartOne(), 301 | () -> container.getFeeder().isFull(), 1.0)); 302 | command.addCommands(followAndIntake(container, trajectories.getFiveBallOrangePartTwo(), 303 | () -> container.getFeeder().isFull(), 1.0)); 304 | 305 | // Retract Intake 306 | command.addCommands(new InstantCommand(() -> container.getIntake().setExtended(false))); 307 | 308 | // Go to shooting location 309 | command.addCommands(follow(container, trajectories.getFiveBallDefensivePartThree())); 310 | 311 | // Shoot the 4th and 5th balls 312 | shootAtTarget(command, container, 1.5); 313 | 314 | return command; 315 | } 316 | 317 | private void shootAtTarget(SequentialCommandGroup command, RobotContainer container, double timeToWait) { 318 | command.addCommands(new TargetWithShooterCommand(container.getShooter(), container.getVision()) 319 | .alongWith(new AlignRobotToShootCommand(container.getDrivetrain(), container.getVision())) 320 | .alongWith(new WaitCommand(0.1).andThen(new ShootWhenReadyCommand(container.getFeeder(), 321 | container.getShooter(), container.getVision()))) 322 | .withTimeout(timeToWait)); 323 | } 324 | 325 | private Command follow(RobotContainer container, Trajectory trajectory) { 326 | return new FollowTrajectoryCommand(container.getDrivetrain(), trajectory); 327 | } 328 | 329 | private Command followAndIntake(RobotContainer container, Trajectory trajectory) { 330 | return followAndIntake(container, trajectory, () -> true, 0.0); 331 | } 332 | 333 | private Command followAndIntake(RobotContainer container, Trajectory trajectory, BooleanSupplier condition, 334 | double conditionTimeout) { 335 | return new FollowTrajectoryCommand(container.getDrivetrain(), trajectory) 336 | .andThen(new WaitUntilCommand(condition).withTimeout(conditionTimeout)) 337 | .deadlineWith(new SimpleIntakeCommand(container.getIntake(), container.getFeeder(), 338 | container.getController()), new DefaultFeederCommand(container.getFeeder())); 339 | } 340 | 341 | public void resetRobotPose(SequentialCommandGroup command, RobotContainer container, Trajectory trajectory) { 342 | Path.State start = trajectory.getPath().calculate(0.0); 343 | command.addCommands(new InstantCommand(() -> container.getDrivetrain().setPose(new Pose2d(start.getPosition().x, 344 | start.getPosition().y, new Rotation2d(start.getRotation().toRadians()))))); 345 | } 346 | 347 | public Command getCommand(RobotContainer container) { 348 | switch (autonomousModeChooser.getSelected()) { 349 | case TEST_AUTO : 350 | return getTestAuto(container); 351 | case FENDER_RED : 352 | return getFenderRedAuto(container); 353 | case FENDER_BLUE : 354 | return getFenderBlueAuto(container); 355 | case TWO_BALL_PURPLE : 356 | return get2BallPurpleAuto(container); 357 | case TWO_BALL_GREEN : 358 | return get2BallGreenAuto(container); 359 | case THREE_BALL_ORANGE : 360 | return get3BallOrangeAuto(container); 361 | case FIVE_BALL_ORANGE : 362 | return get5BallOrangeAuto(container); 363 | case FIVE_BALL_DEFENSIVE : 364 | return get5BallDefensiveAuto(container); 365 | case SIX_BALL_ORANGE : 366 | return get6BallOrangeAuto(container); 367 | case TWO_BALL_DEFENSIVE : 368 | return get2BallDefensiveAuto(container); 369 | case FENDER_DOUBLE_PRELOAD_RED : 370 | return getFenderTwoPreloadRedAuto(container); 371 | case TWO_BALL_DOUBLE_PRELOAD_GREEN : 372 | return get2BallTwoPreloadGreenAuto(container); 373 | case THREE_BALL_DOUBLE_PRELOAD_ORANGE : 374 | return get3BallTwoPreloadOrangeAuto(container); 375 | case FIVE_BALL_DOUBLE_PRELOAD_ORANGE : 376 | return get5BallTwoPreloadOrangeAuto(container); 377 | } 378 | return new InstantCommand(); 379 | } 380 | 381 | private enum AutonomousMode { 382 | TEST_AUTO, FENDER_RED, FENDER_BLUE, TWO_BALL_GREEN, TWO_BALL_PURPLE, THREE_BALL_ORANGE, FIVE_BALL_ORANGE, FIVE_BALL_DEFENSIVE, SIX_BALL_ORANGE, TWO_BALL_DEFENSIVE, FENDER_DOUBLE_PRELOAD_RED, TWO_BALL_DOUBLE_PRELOAD_GREEN, THREE_BALL_DOUBLE_PRELOAD_ORANGE, FIVE_BALL_DOUBLE_PRELOAD_ORANGE 383 | } 384 | } --------------------------------------------------------------------------------