├── .gitmodules ├── 2019 Auto Paths.SLDPRT ├── gradle └── wrapper │ ├── gradle-wrapper.jar │ └── gradle-wrapper.properties ├── settings.gradle ├── vision-coprocessor ├── api │ ├── src │ │ ├── main │ │ │ └── java │ │ │ │ └── org │ │ │ │ └── frcteam2910 │ │ │ │ └── c2019 │ │ │ │ └── vision │ │ │ │ └── api │ │ │ │ ├── Gamepiece.java │ │ │ │ ├── Ports.java │ │ │ │ ├── ConfigCoprocessorPacket.java │ │ │ │ ├── TrajectoryRequestPacket.java │ │ │ │ ├── VisionServer.java │ │ │ │ ├── PeriodicPacket.java │ │ │ │ └── VisionClient.java │ │ └── test │ │ │ └── java │ │ │ └── org │ │ │ └── frcteam2910 │ │ │ └── c2019 │ │ │ └── vision │ │ │ └── api │ │ │ └── VisionClientServerTest.java │ └── build.gradle ├── pivision.service ├── src │ ├── test │ │ └── java │ │ │ ├── Test.java │ │ │ └── VisionPipelineTest.java │ └── main │ │ └── java │ │ └── org │ │ └── frcteam2910 │ │ └── c2019 │ │ └── vision │ │ ├── VisionTargetingPipeline2019.java │ │ ├── Main.java │ │ ├── PeriodicHandler.java │ │ ├── ImagePoint.java │ │ ├── RobotStateEstimator.java │ │ ├── ImagePoint2019.java │ │ ├── VisionCalculationLoop.java │ │ ├── drivers │ │ └── Limelight.java │ │ ├── ControlHandler.java │ │ └── VisionTargetingPipeline.java └── build.gradle ├── src └── main │ └── java │ └── org │ └── frcteam2910 │ └── c2019 │ ├── Main.java │ ├── commands │ ├── AbortClimbCommand.java │ ├── ExtendKickstandCommand.java │ ├── RetractKickstandCommand.java │ ├── GrabHatchPanelCommand.java │ ├── ReleaseHatchPanelCommand.java │ ├── ExtendHatchPlacerCommand.java │ ├── RetractHatchPlacerCommand.java │ ├── GrabHatchFromFloorPart1Command.java │ ├── SetClimberExtendedCommand.java │ ├── SetArmAngleCommand.java │ ├── SetHatchFloorGathererAngleCommand.java │ ├── SetBottomCargoRollerSpeedCommand.java │ ├── WaitForFrontWheelsToExceedCurrentCommand.java │ ├── GrabHatchFromFloorPart2Command.java │ ├── ClimbCommand.java │ ├── DriveCommand.java │ ├── SetRobotPitchCommand.java │ ├── GrabOnHasHatchPanelCommand.java │ ├── RumbleWhileHasHatchPanelCommand.java │ ├── HolonomicDriveCommand.java │ ├── SetHatchFloorGathererIntakeSpeedCommand.java │ ├── WaitForFrontWheelsToDriveDistanceCommand.java │ ├── OverhangCommand.java │ ├── FollowTrajectoryCommand.java │ ├── VisionTargetCommand.java │ ├── CorrectPitchCommand.java │ ├── DoTheThingCommand.java │ ├── CargoGrabberDefaultCommand.java │ └── ImprovedVisionPlaceCommand.java │ ├── subsystems │ ├── VisionSubsystem.java │ ├── ClimberSubsystem.java │ ├── Superstructure.java │ ├── HatchPlacerSubsystem.java │ ├── CargoGrabberSubsystem.java │ ├── CargoArmSubsystem.java │ ├── DrivetrainSubsystem.java │ └── HatchFloorGathererSubsystem.java │ ├── RobotMap.java │ ├── Robot.java │ ├── drivers │ └── Mk2SwerveModule.java │ ├── OI.java │ └── autonomous │ └── AutonomousSelector.java ├── .gitignore ├── .github ├── pull_request_template.md ├── ISSUE_TEMPLATE │ ├── bug_report.md │ └── feature.md └── CODEOWNERS ├── azure-pipelines.yml ├── vendordeps └── REVRobotics.json ├── gradlew.bat └── gradlew /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "common"] 2 | path = common 3 | url = https://github.com/FRCTeam2910/Common-Public.git 4 | -------------------------------------------------------------------------------- /2019 Auto Paths.SLDPRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FRCTeam2910/2019CompetitionRobot-Public/HEAD/2019 Auto Paths.SLDPRT -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FRCTeam2910/2019CompetitionRobot-Public/HEAD/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /settings.gradle: -------------------------------------------------------------------------------- 1 | rootProject.name = '2019CompetitionRobot' 2 | 3 | include 'common' 4 | include 'common:robot' 5 | include 'vision-coprocessor' 6 | include 'vision-coprocessor:api' 7 | -------------------------------------------------------------------------------- /vision-coprocessor/api/src/main/java/org/frcteam2910/c2019/vision/api/Gamepiece.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision.api; 2 | 3 | public enum Gamepiece { 4 | CARGO, 5 | HATCH_PANEL 6 | } 7 | -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=permwrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-5.1-all.zip 4 | zipStoreBase=GRADLE_USER_HOME 5 | zipStorePath=permwrapper/dists 6 | -------------------------------------------------------------------------------- /vision-coprocessor/api/src/main/java/org/frcteam2910/c2019/vision/api/Ports.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision.api; 2 | 3 | public class Ports { 4 | public static final int PERIODIC_PORT = 5803; 5 | public static final int CONTROL_PORT = 5800; 6 | } 7 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/Main.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019; 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 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Intellij files 2 | .idea/ 3 | out/ 4 | *.iml 5 | 6 | # Gradle files 7 | .gradle/ 8 | build/ 9 | 10 | 11 | # Test outputs 12 | *.csv 13 | 14 | # Eclipse files 15 | .classpath 16 | .project 17 | .settings/ 18 | bin/ 19 | 20 | # VS Code files 21 | .vscode/ 22 | 23 | # MacOS system files 24 | .DS_Store 25 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/AbortClimbCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.CommandGroup; 4 | 5 | public class AbortClimbCommand extends CommandGroup { 6 | public AbortClimbCommand() { 7 | this.addParallel(new SetClimberExtendedCommand(false)); 8 | this.addParallel(new SetBottomCargoRollerSpeedCommand(0.3)); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/ExtendKickstandCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.InstantCommand; 4 | import org.frcteam2910.c2019.subsystems.ClimberSubsystem; 5 | 6 | public class ExtendKickstandCommand extends InstantCommand { 7 | public ExtendKickstandCommand() { 8 | super(() -> ClimberSubsystem.getInstance().extendKickstand()); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/RetractKickstandCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.InstantCommand; 4 | import org.frcteam2910.c2019.subsystems.ClimberSubsystem; 5 | 6 | public class RetractKickstandCommand extends InstantCommand { 7 | public RetractKickstandCommand() { 8 | super(() -> ClimberSubsystem.getInstance().retractKickstand()); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /vision-coprocessor/api/build.gradle: -------------------------------------------------------------------------------- 1 | plugins { 2 | id 'java' 3 | } 4 | 5 | sourceCompatibility = JavaVersion.VERSION_11 6 | targetCompatibility = JavaVersion.VERSION_11 7 | 8 | repositories { 9 | mavenCentral() 10 | } 11 | 12 | dependencies { 13 | testCompile group: 'junit', name: 'junit', version: '4.12' 14 | testCompile group: 'org.hamcrest', name: 'hamcrest-library', version: '1.3' 15 | 16 | compile project(':common') 17 | } -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/GrabHatchPanelCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.InstantCommand; 4 | import org.frcteam2910.c2019.subsystems.HatchPlacerSubsystem; 5 | 6 | public class GrabHatchPanelCommand extends InstantCommand { 7 | public GrabHatchPanelCommand() { 8 | super(() -> HatchPlacerSubsystem.getInstance().grab()); 9 | 10 | this.setRunWhenDisabled(true); 11 | } 12 | } 13 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/ReleaseHatchPanelCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.InstantCommand; 4 | import org.frcteam2910.c2019.subsystems.HatchPlacerSubsystem; 5 | 6 | public class ReleaseHatchPanelCommand extends InstantCommand { 7 | public ReleaseHatchPanelCommand() { 8 | super(() -> HatchPlacerSubsystem.getInstance().release()); 9 | 10 | this.setRunWhenDisabled(true); 11 | } 12 | } 13 | -------------------------------------------------------------------------------- /.github/pull_request_template.md: -------------------------------------------------------------------------------- 1 | 9 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/ExtendHatchPlacerCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.InstantCommand; 4 | import org.frcteam2910.c2019.subsystems.HatchPlacerSubsystem; 5 | 6 | public class ExtendHatchPlacerCommand extends InstantCommand { 7 | public ExtendHatchPlacerCommand() { 8 | this.setRunWhenDisabled(true); 9 | } 10 | 11 | @Override 12 | protected void initialize() { 13 | HatchPlacerSubsystem.getInstance().extend(); 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/RetractHatchPlacerCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.InstantCommand; 4 | import org.frcteam2910.c2019.subsystems.HatchPlacerSubsystem; 5 | 6 | public class RetractHatchPlacerCommand extends InstantCommand { 7 | public RetractHatchPlacerCommand() { 8 | this.setRunWhenDisabled(true); 9 | } 10 | 11 | @Override 12 | protected void initialize() { 13 | HatchPlacerSubsystem.getInstance().retract(); 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/GrabHatchFromFloorPart1Command.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.CommandGroup; 4 | 5 | public class GrabHatchFromFloorPart1Command extends CommandGroup { 6 | public GrabHatchFromFloorPart1Command() { 7 | addSequential(new ReleaseHatchPanelCommand()); 8 | addSequential(new RetractHatchPlacerCommand()); 9 | addSequential(new SetHatchFloorGathererAngleCommand(0.0)); 10 | addSequential(new SetHatchFloorGathererIntakeSpeedCommand(0.3)); 11 | } 12 | } 13 | -------------------------------------------------------------------------------- /vision-coprocessor/pivision.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=PiVision service. Used for calculating the distance away from a target detected by the Limelight. 3 | After=network.target 4 | StartLimitIntervalSec=0 5 | 6 | [Service] 7 | Type=simple 8 | # Restart only if the service ends in a non-zero status code 9 | # Can be changed to "always" to always run the service 10 | Restart=on-failure 11 | # Restart 1 second after service dies 12 | RestartSec=1 13 | User=pi 14 | WorkingDirectory=/home/pi 15 | ExecStart=/usr/bin/java -jar /home/pi/vision-coprocessor.jar 16 | 17 | [Install] 18 | WantedBy=multi-user.target -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug Report 3 | about: Create a bug report for the robot 4 | title: '' 5 | labels: Bug 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Additional context** 27 | Add any other context about the problem here. 28 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/SetClimberExtendedCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.InstantCommand; 4 | import org.frcteam2910.c2019.subsystems.ClimberSubsystem; 5 | 6 | public class SetClimberExtendedCommand extends InstantCommand { 7 | private final boolean extended; 8 | 9 | public SetClimberExtendedCommand(boolean extended) { 10 | this.extended = extended; 11 | 12 | requires(ClimberSubsystem.getInstance()); 13 | } 14 | 15 | 16 | @Override 17 | protected void initialize() { 18 | ClimberSubsystem.getInstance().setClimberExtended(extended); 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /vision-coprocessor/api/src/main/java/org/frcteam2910/c2019/vision/api/ConfigCoprocessorPacket.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision.api; 2 | 3 | import java.io.Serializable; 4 | 5 | public class ConfigCoprocessorPacket implements Serializable { 6 | private final Gamepiece gamepiece; 7 | private final boolean driverMode; 8 | 9 | public ConfigCoprocessorPacket(Gamepiece gamepiece, boolean driverMode) { 10 | this.gamepiece = gamepiece; 11 | this.driverMode = driverMode; 12 | } 13 | 14 | public Gamepiece getGamepiece() { 15 | return gamepiece; 16 | } 17 | 18 | public boolean isDriverMode() { 19 | return driverMode; 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /vision-coprocessor/src/test/java/Test.java: -------------------------------------------------------------------------------- 1 | import edu.wpi.first.wpiutil.RuntimeLoader; 2 | import org.opencv.core.*; 3 | import java.io.IOException; 4 | 5 | public class Test { 6 | public static void main(String[] args) { 7 | try { 8 | RuntimeLoader loader = new RuntimeLoader<>(Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class); 9 | loader.loadLibraryHashed(); 10 | } catch (IOException ex) { 11 | ex.printStackTrace(); 12 | System.exit(1); 13 | } 14 | 15 | Point point1 = new Point(1, 2); 16 | Point point2 = new Point(3, 4); 17 | System.out.println(point1.dot(point2)); 18 | } 19 | } -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/SetArmAngleCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.Command; 4 | import org.frcteam2910.c2019.subsystems.CargoArmSubsystem; 5 | 6 | public class SetArmAngleCommand extends Command { 7 | private double angle; 8 | 9 | public SetArmAngleCommand(double angle) { 10 | this.angle = angle; 11 | requires(CargoArmSubsystem.getInstance()); 12 | 13 | this.setRunWhenDisabled(true); 14 | } 15 | 16 | @Override 17 | protected void initialize() { 18 | CargoArmSubsystem.getInstance().setTargetAngle(angle); 19 | } 20 | 21 | @Override 22 | protected boolean isFinished() { 23 | return CargoArmSubsystem.getInstance().isWithinTargetAngleRange(angle); 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /vision-coprocessor/api/src/main/java/org/frcteam2910/c2019/vision/api/TrajectoryRequestPacket.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision.api; 2 | 3 | import java.io.Serializable; 4 | 5 | public class TrajectoryRequestPacket implements Serializable { 6 | private static final long serialVersionUID = 6087006825146565015L; 7 | 8 | private final double timestamp; 9 | private final Gamepiece gamepiece; 10 | 11 | public TrajectoryRequestPacket(double timestamp, Gamepiece gamepiece) { 12 | this.timestamp = timestamp; 13 | this.gamepiece = gamepiece; 14 | } 15 | 16 | public double getTimestamp() { 17 | return timestamp; 18 | } 19 | 20 | public Gamepiece getGamepiece() { 21 | return gamepiece; 22 | } 23 | } 24 | -------------------------------------------------------------------------------- /azure-pipelines.yml: -------------------------------------------------------------------------------- 1 | # Gradle 2 | # Build your Java project and run tests with Gradle using a Gradle wrapper script. 3 | # Add steps that analyze code, save build artifacts, deploy, and more: 4 | # https://docs.microsoft.com/azure/devops/pipelines/languages/java 5 | 6 | trigger: 7 | - master 8 | 9 | pool: 10 | vmImage: 'Ubuntu-16.04' 11 | 12 | steps: 13 | - task: Gradle@2 14 | inputs: 15 | workingDirectory: '' 16 | gradleWrapperFile: 'gradlew' 17 | gradleOptions: '-Xmx3072m' 18 | javaHomeOption: 'JDKVersion' 19 | jdkVersionOption: '1.11' 20 | jdkArchitectureOption: 'x64' 21 | publishJUnitResults: false 22 | testResultsFiles: '**/TEST-*.xml' 23 | 24 | # We're also using --no-daemon in order to speed up build times. 25 | tasks: 'build --no-daemon' -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature 3 | about: Template for Features to be implemented 4 | title: '' 5 | labels: Feature 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the feature in question** 11 | Use this space to describe the feature. 12 | 13 | **Does this feature have a deadline? If so, when?** 14 | YYYY-MM-DD if relevant, otherwise give general time. If no deadline, leave this space blank. 15 | 16 | **Does this feature rely on any others to be implemented first? If so, which?** 17 | 19 | 20 | 21 | **Additional context** 22 | Add any other context or screenshots about the feature request here. 23 | -------------------------------------------------------------------------------- /vision-coprocessor/api/src/main/java/org/frcteam2910/c2019/vision/api/VisionServer.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision.api; 2 | 3 | import java.io.IOException; 4 | import java.net.ServerSocket; 5 | 6 | public class VisionServer implements AutoCloseable { 7 | private ServerSocket socket; 8 | 9 | public VisionServer(int port) throws IOException { 10 | this.socket = new ServerSocket(port); 11 | } 12 | 13 | public VisionClient accept() throws IOException { 14 | return new VisionClient<>(socket.accept()); 15 | } 16 | 17 | @Override 18 | public void close() throws IOException { 19 | socket.close(); 20 | } 21 | 22 | public ServerSocket getSocket() { 23 | return socket; 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/SetHatchFloorGathererAngleCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.Command; 4 | import org.frcteam2910.c2019.subsystems.HatchFloorGathererSubsystem; 5 | 6 | public class SetHatchFloorGathererAngleCommand extends Command { 7 | private double angle; 8 | 9 | public SetHatchFloorGathererAngleCommand(double angle) { 10 | this.angle = angle; 11 | 12 | requires(HatchFloorGathererSubsystem.getInstance()); 13 | } 14 | 15 | @Override 16 | protected void initialize() { 17 | HatchFloorGathererSubsystem.getInstance().setTargetAngle(angle); 18 | } 19 | 20 | @Override 21 | protected boolean isFinished() { 22 | return HatchFloorGathererSubsystem.getInstance().isAtTargetAngle(); 23 | } 24 | } 25 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/SetBottomCargoRollerSpeedCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.Command; 4 | import org.frcteam2910.c2019.subsystems.CargoGrabberSubsystem; 5 | 6 | public class SetBottomCargoRollerSpeedCommand extends Command { 7 | private final double speed; 8 | 9 | public SetBottomCargoRollerSpeedCommand(double speed) { 10 | this.speed = speed; 11 | 12 | requires(CargoGrabberSubsystem.getInstance()); 13 | } 14 | 15 | @Override 16 | protected void initialize() { 17 | CargoGrabberSubsystem.getInstance().setBottomIntakeSpeed(speed); 18 | } 19 | 20 | @Override 21 | protected void end() { 22 | CargoGrabberSubsystem.getInstance().setBottomIntakeSpeed(0.0); 23 | } 24 | 25 | @Override 26 | protected boolean isFinished() { 27 | return false; 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/WaitForFrontWheelsToExceedCurrentCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.Command; 4 | import org.frcteam2910.c2019.subsystems.DrivetrainSubsystem; 5 | import org.frcteam2910.common.drivers.SwerveModule; 6 | 7 | public class WaitForFrontWheelsToExceedCurrentCommand extends Command { 8 | private final double current; 9 | 10 | public WaitForFrontWheelsToExceedCurrentCommand(double current) { 11 | this.current = current; 12 | } 13 | 14 | @Override 15 | protected boolean isFinished() { 16 | for (SwerveModule module : DrivetrainSubsystem.getInstance().getSwerveModules()) { 17 | if (module.getModulePosition().y > 0.0) { 18 | // Is front module 19 | if (!(module.getDriveCurrent() > current)) { 20 | // Current does not exceed threshold 21 | return false; 22 | } 23 | } 24 | } 25 | 26 | return true; 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/GrabHatchFromFloorPart2Command.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.CommandGroup; 4 | import edu.wpi.first.wpilibj.command.WaitCommand; 5 | import org.frcteam2910.c2019.subsystems.HatchFloorGathererSubsystem; 6 | 7 | public class GrabHatchFromFloorPart2Command extends CommandGroup { 8 | public GrabHatchFromFloorPart2Command() { 9 | addParallel(new SetHatchFloorGathererIntakeSpeedCommand(0.3, false), 0.5); 10 | addSequential(new SetHatchFloorGathererAngleCommand(Math.toRadians(90.0))); 11 | addSequential(new ExtendHatchPlacerCommand()); 12 | addSequential(new GrabHatchPanelCommand()); 13 | addSequential(new WaitCommand(0.25)); 14 | addParallel(new SetHatchFloorGathererIntakeSpeedCommand(-0.3, false), 0.5); 15 | addSequential(new SetHatchFloorGathererAngleCommand(Math.toRadians(45.0))); 16 | addSequential(new SetHatchFloorGathererAngleCommand(HatchFloorGathererSubsystem.getInstance().getMaxAngle())); 17 | addSequential(new RetractHatchPlacerCommand()); 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/ClimbCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.CommandGroup; 4 | import edu.wpi.first.wpilibj.command.ConditionalCommand; 5 | import edu.wpi.first.wpilibj.command.WaitCommand; 6 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 7 | import org.frcteam2910.c2019.subsystems.CargoArmSubsystem; 8 | import org.frcteam2910.common.math.Vector2; 9 | 10 | public class ClimbCommand extends CommandGroup { 11 | private static final double DRIVE_SPEED = 0.25; 12 | private static final double INTAKE_SPEED = 0.75; 13 | private static final double TARGET_PITCH = Math.toRadians(-5.0); 14 | 15 | public ClimbCommand() { 16 | addParallel(new SetClimberExtendedCommand(true)); 17 | addParallel(new DriveCommand(new Vector2(DRIVE_SPEED, 0.0), 0.0, false)); 18 | addParallel(new SetBottomCargoRollerSpeedCommand(INTAKE_SPEED)); 19 | addParallel(new ExtendKickstandCommand()); 20 | addParallel(new CorrectPitchCommand(TARGET_PITCH, true)); 21 | // addSequential(new SetArmAngleCommand(CargoArmSubsystem.CARGO_SHIP_SCORE_ANGLE)); 22 | } 23 | } 24 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/DriveCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.Command; 4 | import org.frcteam2910.c2019.subsystems.DrivetrainSubsystem; 5 | import org.frcteam2910.common.math.Vector2; 6 | 7 | public class DriveCommand extends Command { 8 | private Vector2 translation; 9 | private double rotation; 10 | private boolean fieldOriented; 11 | 12 | public DriveCommand(Vector2 translation, double rotation, boolean fieldOriented) { 13 | this.translation = translation; 14 | this.rotation = rotation; 15 | this.fieldOriented = fieldOriented; 16 | 17 | requires(DrivetrainSubsystem.getInstance()); 18 | 19 | this.setRunWhenDisabled(true); 20 | } 21 | 22 | @Override 23 | protected void initialize() { 24 | DrivetrainSubsystem.getInstance().holonomicDrive(translation, rotation, fieldOriented); 25 | } 26 | 27 | @Override 28 | protected void end() { 29 | DrivetrainSubsystem.getInstance().holonomicDrive(Vector2.ZERO, 0.0); 30 | } 31 | 32 | @Override 33 | protected boolean isFinished() { 34 | return false; 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /vision-coprocessor/src/main/java/org/frcteam2910/c2019/vision/VisionTargetingPipeline2019.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision; 2 | 3 | import org.frcteam2910.c2019.vision.drivers.Limelight; 4 | 5 | public class VisionTargetingPipeline2019 extends VisionTargetingPipeline { 6 | private static final double[][] OBJET_POINTS = { 7 | {5.9363, 2.9128, 0.0}, 8 | {4.0, 2.4120, 0.0}, 9 | {-4.0, 2.4120, 0.0}, 10 | {-5.9363, 2.9128, 0.0}, 11 | {-7.3134, -2.4120, 0.0}, 12 | {-5.3771, -2.9128, 0.0}, 13 | {5.3771, -2.9128, 0.0}, 14 | {7.3134, -2.4120, 0.0} 15 | }; 16 | 17 | private static final double[][] CAMERA_MATRIX = { 18 | {773.86723397, 0.0, 431.29426601}, 19 | {0.0, 777.62582296, 306.80364947}, 20 | {0.0, 0.0, 1.0} 21 | }; 22 | 23 | private static final double[] DISTORTION_COEFFICIENTS = { 24 | 3.15600348e-01, -1.17776987e+00, -9.30063427e-03, 1.46275541e-03, 1.61055001e+00 25 | }; 26 | 27 | 28 | public VisionTargetingPipeline2019(Limelight limelight) { 29 | super(limelight, OBJET_POINTS, DISTORTION_COEFFICIENTS, CAMERA_MATRIX); 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /vision-coprocessor/api/src/main/java/org/frcteam2910/c2019/vision/api/PeriodicPacket.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision.api; 2 | 3 | import org.frcteam2910.common.math.RigidTransform2; 4 | import org.frcteam2910.common.math.Vector2; 5 | 6 | import java.io.Serializable; 7 | 8 | public class PeriodicPacket implements Serializable { 9 | private static final long serialVersionUID = 5090957247874698962L; 10 | 11 | private final double timestamp; 12 | private final RigidTransform2 pose; 13 | private final Vector2 velocity; 14 | private final double rotationalVelocity; 15 | 16 | public PeriodicPacket(double timestamp, RigidTransform2 pose, Vector2 velocity, double rotationalVelocity) { 17 | this.timestamp = timestamp; 18 | this.pose = pose; 19 | this.velocity = velocity; 20 | this.rotationalVelocity = rotationalVelocity; 21 | } 22 | 23 | public double getTimestamp() { 24 | return timestamp; 25 | } 26 | 27 | public RigidTransform2 getPose() { 28 | return pose; 29 | } 30 | 31 | public Vector2 getVelocity() { 32 | return velocity; 33 | } 34 | 35 | public double getRotationalVelocity() { 36 | return rotationalVelocity; 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/SetRobotPitchCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.Command; 4 | import org.frcteam2910.c2019.subsystems.ClimberSubsystem; 5 | import org.frcteam2910.c2019.subsystems.Superstructure; 6 | import org.frcteam2910.common.robot.drivers.NavX; 7 | 8 | public class SetRobotPitchCommand extends Command { 9 | private final double targetPitch; 10 | 11 | public SetRobotPitchCommand(double targetPitch) { 12 | this.targetPitch = targetPitch; 13 | 14 | requires(ClimberSubsystem.getInstance()); 15 | } 16 | 17 | @Override 18 | protected void execute() { 19 | double currentPitch = Superstructure.getInstance().getGyroscope().getAxis(NavX.Axis.ROLL); // Roll is pitch 20 | if (currentPitch > targetPitch) { 21 | ClimberSubsystem.getInstance().setClimberExtended(false); 22 | } else { 23 | ClimberSubsystem.getInstance().setClimberExtended(true); 24 | } 25 | } 26 | 27 | @Override 28 | protected void end() { 29 | ClimberSubsystem.getInstance().setClimberExtended(false); 30 | } 31 | 32 | @Override 33 | protected boolean isFinished() { 34 | return false; 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /vision-coprocessor/src/main/java/org/frcteam2910/c2019/vision/Main.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision; 2 | 3 | import edu.wpi.first.networktables.NetworkTableInstance; 4 | import edu.wpi.first.wpiutil.RuntimeLoader; 5 | import org.opencv.core.Core; 6 | 7 | import java.io.IOException; 8 | 9 | public class Main { 10 | public static void main(String[] args) { 11 | try { 12 | RuntimeLoader loader = new RuntimeLoader<>(Core.NATIVE_LIBRARY_NAME, 13 | RuntimeLoader.getDefaultExtractionRoot(), Core.class); 14 | loader.loadLibraryHashed(); 15 | } catch (IOException e) { 16 | e.printStackTrace(); 17 | } 18 | 19 | // Initialize network tables 20 | NetworkTableInstance.getDefault().startClientTeam(2910); 21 | 22 | new VisionCalculationLoop().run(); 23 | 24 | // try { 25 | // Handle periodic packets in a separate thread 26 | // Thread periodicThread = new Thread(new PeriodicHandler()); 27 | // periodicThread.start(); 28 | 29 | // Handle control packets in the main thread 30 | // new ControlHandler().run(); 31 | // } catch (IOException e) { 32 | // e.printStackTrace(); 33 | // } 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/GrabOnHasHatchPanelCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import edu.wpi.first.wpilibj.command.Command; 5 | import org.frcteam2910.c2019.subsystems.HatchPlacerSubsystem; 6 | 7 | public class GrabOnHasHatchPanelCommand extends Command { 8 | private static final double SIGNAL_LENGTH = 0.1; 9 | 10 | private double lastSignalTime = Double.NaN; 11 | 12 | @Override 13 | protected void initialize() { 14 | lastSignalTime = Double.NaN; 15 | } 16 | 17 | @Override 18 | protected void end() { 19 | HatchPlacerSubsystem.getInstance().grab(); 20 | } 21 | 22 | @Override 23 | protected boolean isFinished() { 24 | boolean hasValidSignal = HatchPlacerSubsystem.getInstance().getLeftLimitSwitch() || HatchPlacerSubsystem.getInstance().getRightLimitSwitch(); 25 | 26 | if (!Double.isNaN(lastSignalTime)) { 27 | if (lastSignalTime + SIGNAL_LENGTH < Timer.getFPGATimestamp()) { 28 | return true; 29 | } else if (!hasValidSignal){ 30 | lastSignalTime = Double.NaN; 31 | } 32 | } else if (hasValidSignal){ 33 | lastSignalTime = Timer.getFPGATimestamp(); 34 | } 35 | 36 | return false; 37 | } 38 | 39 | @Override 40 | protected void interrupted() { 41 | 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/RumbleWhileHasHatchPanelCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.GenericHID; 4 | import edu.wpi.first.wpilibj.command.Command; 5 | import org.frcteam2910.c2019.Robot; 6 | import org.frcteam2910.c2019.subsystems.HatchPlacerSubsystem; 7 | 8 | public class RumbleWhileHasHatchPanelCommand extends Command { 9 | 10 | @Override 11 | protected void execute() { 12 | if(HatchPlacerSubsystem.getInstance().getLeftLimitSwitch() && HatchPlacerSubsystem.getInstance().getRightLimitSwitch()) { 13 | Robot.getOi().primaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kRightRumble, 1.0); 14 | Robot.getOi().secondaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kRightRumble, 1.0); 15 | } else { 16 | Robot.getOi().primaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kRightRumble, 0.0); 17 | Robot.getOi().secondaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kRightRumble, 0.0); 18 | } 19 | } 20 | 21 | @Override 22 | protected void end() { 23 | Robot.getOi().primaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kRightRumble, 0.0); 24 | Robot.getOi().secondaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kRightRumble, 0.0); 25 | } 26 | 27 | @Override 28 | protected boolean isFinished() { 29 | return false; 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /vendordeps/REVRobotics.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "REVRobotics.json", 3 | "name": "REVRobotics", 4 | "version": "1.1.9", 5 | "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", 6 | "mavenUrls": [ 7 | "http://www.revrobotics.com/content/sw/max/sdk/maven/" 8 | ], 9 | "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.revrobotics.frc", 13 | "artifactId": "SparkMax-java", 14 | "version": "1.1.9" 15 | } 16 | ], 17 | "jniDependencies": [ 18 | { 19 | "groupId": "com.revrobotics.frc", 20 | "artifactId": "SparkMax-driver", 21 | "version": "1.1.9", 22 | "skipInvalidPlatforms": true, 23 | "isJar": false, 24 | "validPlatforms": [ 25 | "linuxathena" 26 | ] 27 | } 28 | ], 29 | "cppDependencies": [ 30 | { 31 | "groupId": "com.revrobotics.frc", 32 | "artifactId": "SparkMax-cpp", 33 | "version": "1.1.9", 34 | "libName": "libSparkMax", 35 | "headerClassifier": "headers", 36 | "sharedLibrary": false, 37 | "skipInvalidPlatforms": true, 38 | "binaryPlatforms": [ 39 | "linuxathena" 40 | ] 41 | }, 42 | { 43 | "groupId": "com.revrobotics.frc", 44 | "artifactId": "SparkMax-driver", 45 | "version": "1.1.9", 46 | "libName": "libSparkMaxDriver", 47 | "headerClassifier": "headers", 48 | "sharedLibrary": false, 49 | "skipInvalidPlatforms": true, 50 | "binaryPlatforms": [ 51 | "linuxathena" 52 | ] 53 | } 54 | ] 55 | } 56 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/HolonomicDriveCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.Command; 4 | import org.frcteam2910.c2019.Robot; 5 | import org.frcteam2910.c2019.subsystems.DrivetrainSubsystem; 6 | import org.frcteam2910.common.math.Rotation2; 7 | import org.frcteam2910.common.math.Vector2; 8 | 9 | public class HolonomicDriveCommand extends Command { 10 | public HolonomicDriveCommand() { 11 | requires(DrivetrainSubsystem.getInstance()); 12 | } 13 | 14 | @Override 15 | protected void execute() { 16 | boolean ignoreScalars = Robot.getOi().primaryController.getLeftBumperButton().get(); 17 | 18 | double forward = Robot.getOi().primaryController.getLeftYAxis().get(true); 19 | double strafe = Robot.getOi().primaryController.getLeftXAxis().get(true); 20 | double rotation = Robot.getOi().primaryController.getRightXAxis().get(true, ignoreScalars); 21 | 22 | boolean robotOriented = Robot.getOi().primaryController.getXButton().get(); 23 | boolean reverseRobotOriented = Robot.getOi().primaryController.getYButton().get(); 24 | 25 | Vector2 translation = new Vector2(forward, strafe); 26 | 27 | if (reverseRobotOriented) { 28 | robotOriented = true; 29 | translation = translation.rotateBy(Rotation2.fromDegrees(180.0)); 30 | } 31 | 32 | DrivetrainSubsystem.getInstance().holonomicDrive(translation, rotation, !robotOriented); 33 | } 34 | 35 | @Override 36 | protected boolean isFinished() { 37 | return false; 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | ########## 2 | #Taken from github CODEOWNERS template 3 | #https://help.github.com/articles/about-code-owners/ 4 | ########## 5 | 6 | # Each line is a file pattern followed by one or more owners. 7 | 8 | # These owners will be the default owners for everything in 9 | # the repo. Unless a later match takes precedence, 10 | # @global-owner1 and @global-owner2 will be requested for 11 | # review when someone opens a pull request. 12 | #* @global-owner1 @global-owner2 13 | 14 | # leads/mentors as owners for all files so their approval will be required for any merge. 15 | * @frcteam2910/leads-mentors 16 | 17 | 18 | # Order is important; the last matching pattern takes the most 19 | # precedence. When someone opens a pull request that only 20 | # modifies JS files, only @js-owner and not the global 21 | # owner(s) will be requested for a review. 22 | #*.js @js-owner 23 | 24 | # You can also use email addresses if you prefer. They'll be 25 | # used to look up users just like we do for commit author 26 | # emails. 27 | #*.go docs@example.com 28 | 29 | # In this example, @doctocat owns any files in the build/logs 30 | # directory at the root of the repository and any of its 31 | # subdirectories. 32 | #/build/logs/ @doctocat 33 | 34 | # The `docs/*` pattern will match files like 35 | # `docs/getting-started.md` but not further nested files like 36 | # `docs/build-app/troubleshooting.md`. 37 | #docs/* docs@example.com 38 | 39 | # In this example, @octocat owns any file in an apps directory 40 | # anywhere in your repository. 41 | #apps/ @octocat 42 | 43 | # In this example, @doctocat owns any file in the `/docs` 44 | # directory in the root of your repository. 45 | #/docs/ @doctocat 46 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/SetHatchFloorGathererIntakeSpeedCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.GenericHID; 4 | import edu.wpi.first.wpilibj.command.Command; 5 | import org.frcteam2910.c2019.Robot; 6 | import org.frcteam2910.c2019.subsystems.HatchFloorGathererSubsystem; 7 | 8 | public class SetHatchFloorGathererIntakeSpeedCommand extends Command { 9 | private double speed; 10 | 11 | public SetHatchFloorGathererIntakeSpeedCommand(double speed) { 12 | this(speed, true); 13 | } 14 | 15 | public SetHatchFloorGathererIntakeSpeedCommand(double speed, boolean shouldRequire) { 16 | this.speed = speed; 17 | 18 | if (shouldRequire) { 19 | requires(HatchFloorGathererSubsystem.getInstance()); 20 | } 21 | } 22 | 23 | @Override 24 | protected void initialize() { 25 | HatchFloorGathererSubsystem.getInstance().setTargetIntakeSpeed(speed); 26 | } 27 | 28 | @Override 29 | protected void execute() { 30 | // If we have a hatch panel, vibrate the secondary controller 31 | if (HatchFloorGathererSubsystem.getInstance().hasHatchPanel()) { 32 | Robot.getOi().secondaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kRightRumble, 1.0); 33 | } 34 | } 35 | 36 | @Override 37 | protected void end() { 38 | Robot.getOi().secondaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kRightRumble, 0.0); 39 | 40 | HatchFloorGathererSubsystem.getInstance().setTargetIntakeSpeed(0.0); 41 | } 42 | 43 | @Override 44 | protected boolean isFinished() { 45 | return false; 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /vision-coprocessor/src/main/java/org/frcteam2910/c2019/vision/PeriodicHandler.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision; 2 | 3 | import org.frcteam2910.c2019.vision.api.PeriodicPacket; 4 | import org.frcteam2910.c2019.vision.api.Ports; 5 | import org.frcteam2910.c2019.vision.api.VisionClient; 6 | import org.frcteam2910.c2019.vision.api.VisionServer; 7 | import org.frcteam2910.common.Logger; 8 | 9 | import java.io.IOException; 10 | 11 | public class PeriodicHandler implements Runnable { 12 | private static final Logger LOGGER = new Logger(PeriodicHandler.class); 13 | 14 | private VisionServer server; 15 | 16 | public PeriodicHandler() throws IOException { 17 | this.server = new VisionServer<>(Ports.PERIODIC_PORT); 18 | } 19 | 20 | @Override 21 | public void run() { 22 | while (!Thread.interrupted() && server.getSocket().isBound()) { 23 | try (VisionClient connection = server.accept()) { 24 | LOGGER.info("Client connected from IP: %s", connection.getSocket().getInetAddress().getHostName()); 25 | while (!Thread.interrupted() && connection.getSocket().isConnected()) { 26 | PeriodicPacket packet = connection.receivePacket(); 27 | 28 | RobotStateEstimator.addState( 29 | packet.getTimestamp(), 30 | packet.getPose(), 31 | packet.getVelocity(), 32 | packet.getRotationalVelocity() 33 | ); 34 | } 35 | LOGGER.info("Connection to client (%s) was closed.", connection.getSocket().getInetAddress().getHostName()); 36 | } catch (IOException e) { 37 | LOGGER.error(e); 38 | } 39 | } 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/WaitForFrontWheelsToDriveDistanceCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.Command; 4 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 5 | import org.frcteam2910.c2019.subsystems.DrivetrainSubsystem; 6 | import org.frcteam2910.common.drivers.SwerveModule; 7 | 8 | public class WaitForFrontWheelsToDriveDistanceCommand extends Command { 9 | private final double distance; 10 | 11 | private double[] startingDistances; 12 | 13 | public WaitForFrontWheelsToDriveDistanceCommand(double distance) { 14 | this.distance = distance; 15 | } 16 | 17 | @Override 18 | protected void initialize() { 19 | startingDistances = new double[DrivetrainSubsystem.getInstance().getSwerveModules().length]; 20 | for (int i = 0; i < startingDistances.length; i++) { 21 | startingDistances[i] = DrivetrainSubsystem.getInstance().getSwerveModules()[i].getCurrentDistance(); 22 | } 23 | } 24 | 25 | @Override 26 | protected boolean isFinished() { 27 | double avgDistance = 0.0; 28 | int moduleCount = 0; 29 | for (int i = 0; i < startingDistances.length; i++) { 30 | SwerveModule module = DrivetrainSubsystem.getInstance().getSwerveModules()[i]; 31 | if (module.getModulePosition().y > 0.0) { 32 | double delta = Math.abs(module.getCurrentDistance() - startingDistances[i]); 33 | 34 | avgDistance += delta; 35 | moduleCount++; 36 | } 37 | } 38 | 39 | if (moduleCount == 0) { 40 | return false; 41 | } else { 42 | SmartDashboard.putNumber("Distance Driven", avgDistance / moduleCount); 43 | return avgDistance / moduleCount > distance; 44 | } 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/subsystems/VisionSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.subsystems; 2 | 3 | import edu.wpi.first.networktables.NetworkTableInstance; 4 | import org.frcteam2910.c2019.vision.api.Gamepiece; 5 | import org.frcteam2910.common.robot.drivers.Limelight; 6 | import org.frcteam2910.common.robot.subsystems.Subsystem; 7 | 8 | public class VisionSubsystem extends Subsystem { 9 | private static final String CARGO_LIMELIGHT_TABLE_NAME = "limelight-cargo"; 10 | private static final String HATCH_LIMELIGHT_TABLE_NAME = "limelight-hatch"; 11 | 12 | private static final VisionSubsystem instance = new VisionSubsystem(); 13 | 14 | private final Limelight cargoLimelight = new Limelight(NetworkTableInstance.getDefault().getTable(CARGO_LIMELIGHT_TABLE_NAME)); 15 | private final Limelight hatchLimelight = new Limelight(NetworkTableInstance.getDefault().getTable(HATCH_LIMELIGHT_TABLE_NAME)); 16 | 17 | public VisionSubsystem() { 18 | cargoLimelight.setCamMode(Limelight.CamMode.DRIVER); 19 | 20 | hatchLimelight.setCamMode(Limelight.CamMode.DRIVER); 21 | hatchLimelight.setPipeline(8); 22 | } 23 | 24 | public static VisionSubsystem getInstance() { 25 | return instance; 26 | } 27 | 28 | public Limelight getLimelight(Gamepiece gamepiece) { 29 | switch (gamepiece) { 30 | case CARGO: 31 | return cargoLimelight; 32 | case HATCH_PANEL: 33 | return hatchLimelight; 34 | } 35 | 36 | throw new IllegalArgumentException(String.format("Unknown gamepiece %s", gamepiece)); 37 | } 38 | 39 | @Override 40 | public void outputToSmartDashboard() { 41 | 42 | } 43 | 44 | @Override 45 | public void stop() { 46 | 47 | } 48 | 49 | @Override 50 | public void zeroSensors() { 51 | 52 | } 53 | 54 | @Override 55 | protected void initDefaultCommand() { 56 | 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /vision-coprocessor/api/src/main/java/org/frcteam2910/c2019/vision/api/VisionClient.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision.api; 2 | 3 | import org.frcteam2910.common.Logger; 4 | 5 | import java.io.IOException; 6 | import java.io.ObjectInputStream; 7 | import java.io.ObjectOutputStream; 8 | import java.net.InetAddress; 9 | import java.net.Socket; 10 | 11 | public class VisionClient implements AutoCloseable { 12 | private static final Logger LOGGER = new Logger(VisionClient.class); 13 | 14 | private Socket socket; 15 | 16 | VisionClient(Socket socket) { 17 | this.socket = socket; 18 | } 19 | 20 | public VisionClient(String address, int port) throws IOException { 21 | this(InetAddress.getByName(address), port); 22 | } 23 | 24 | public VisionClient(InetAddress address, int port) throws IOException { 25 | this.socket = new Socket(address, port); 26 | } 27 | 28 | public void sendPacket(SendPacketType packet) throws IOException { 29 | ObjectOutputStream out = new ObjectOutputStream(socket.getOutputStream()); 30 | 31 | out.writeObject(packet); 32 | } 33 | 34 | public ReceivePacketType receivePacket() throws IOException { 35 | ObjectInputStream in = new ObjectInputStream(socket.getInputStream()); 36 | 37 | // Block until we receive a valid packet or our connection to the server closes. 38 | while (!Thread.interrupted() && socket.isConnected()) { 39 | try { 40 | return (ReceivePacketType) in.readObject(); 41 | } catch (ClassNotFoundException e) { 42 | LOGGER.error(e); 43 | } 44 | } 45 | 46 | throw new IOException("Connection was closed."); 47 | } 48 | 49 | @Override 50 | public void close() throws IOException { 51 | socket.close(); 52 | } 53 | 54 | public Socket getSocket() { 55 | return socket; 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/RobotMap.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019; 2 | 3 | public class RobotMap { 4 | public static final int GRABBER_TOP_MOTOR = 5; 5 | public static final int GRABBER_BOTTOM_MOTOR = 2; 6 | 7 | public static final int CARGO_ARM_ENCODER_PORT = 4; 8 | 9 | public static final int HATCH_EXTENDER_SOLENOID_MODULE = 0; 10 | public static final int HATCH_EXTENDER_SOLENOID_CHANNEL = 2; 11 | public static final int HATCH_GRABBER_SOLENOID_MODULE = 0; 12 | public static final int HATCH_GRABBER_SOLENOID_CHANNEL = 1; 13 | public static final int HATCH_PLACER_SOLENOID_MODULE = 0; 14 | public static final int HATCH_PLACER_SOLENOID_CHANNEL = 4; 15 | 16 | public static final int HATCH_GRABBER_LIMIT_SWITCH_LEFT = 0; 17 | public static final int HATCH_GRABBER_LIMIT_SWITCH_RIGHT = 1; 18 | 19 | public static final int ARM_MOTOR_A = 9; 20 | public static final int ARM_MOTOR_B = 8; 21 | 22 | public static final int CLIMBER_SOLENOID_MODULE = 0; 23 | public static final int CLIMBER_SOLENOID_CHANNEL = 0; 24 | public static final int KICKSTAND_SOLENOID_MODULE = 0; 25 | public static final int KICKSTAND_SOLENOID_CHANNEL = 3; 26 | 27 | public static final int HATCH_FLOOR_GATHERER_ARM_MOTOR = 1; 28 | public static final int HATCH_FLOOR_GATHERER_FLOOR_MOTOR = 8; 29 | 30 | public static final int DRIVETRAIN_FRONT_LEFT_ANGLE_MOTOR = 4; 31 | public static final int DRIVETRAIN_FRONT_LEFT_ANGLE_ENCODER = 0; 32 | public static final int DRIVETRAIN_FRONT_LEFT_DRIVE_MOTOR = 10; 33 | 34 | public static final int DRIVETRAIN_FRONT_RIGHT_ANGLE_MOTOR = 7; 35 | public static final int DRIVETRAIN_FRONT_RIGHT_ANGLE_ENCODER = 3; 36 | public static final int DRIVETRAIN_FRONT_RIGHT_DRIVE_MOTOR = 6; 37 | 38 | public static final int DRIVETRAIN_BACK_LEFT_ANGLE_MOTOR = 5; 39 | public static final int DRIVETRAIN_BACK_LEFT_ANGLE_ENCODER = 1; 40 | public static final int DRIVETRAIN_BACK_LEFT_DRIVE_MOTOR = 9; 41 | 42 | public static final int DRIVETRAIN_BACK_RIGHT_ANGLE_MOTOR = 6; 43 | public static final int DRIVETRAIN_BACK_RIGHT_ANGLE_ENCODER = 2; 44 | public static final int DRIVETRAIN_BACK_RIGHT_DRIVE_MOTOR = 7; 45 | } 46 | -------------------------------------------------------------------------------- /vision-coprocessor/api/src/test/java/org/frcteam2910/c2019/vision/api/VisionClientServerTest.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision.api; 2 | 3 | import org.frcteam2910.common.math.MathUtils; 4 | import org.frcteam2910.common.math.RigidTransform2; 5 | import org.frcteam2910.common.math.Rotation2; 6 | import org.frcteam2910.common.math.Vector2; 7 | import org.junit.Test; 8 | 9 | import java.io.IOException; 10 | import java.net.InetAddress; 11 | 12 | import static org.junit.Assert.assertEquals; 13 | 14 | public class VisionClientServerTest { 15 | @Test 16 | public void sendReceiveTest() { 17 | PeriodicPacket expectedPacket = new PeriodicPacket( 18 | 0.0, 19 | new RigidTransform2(new Vector2(1.0, 0.0), Rotation2.ZERO), 20 | new Vector2(-5.0, 0.2), 21 | 0.2 22 | ); 23 | PeriodicPacket actualPacket; 24 | 25 | try (VisionServer server = new VisionServer<>(8888)) { 26 | Thread clientThread = new Thread(() -> { 27 | try (VisionClient client = new VisionClient<>(InetAddress.getLocalHost(), 8888)) { 28 | client.sendPacket(expectedPacket); 29 | } catch (IOException e) { 30 | e.printStackTrace(); 31 | } 32 | }); 33 | clientThread.start(); 34 | 35 | VisionClient connection = server.accept(); 36 | 37 | actualPacket = connection.receivePacket(); 38 | 39 | assertEquals("Timestamp does not match", expectedPacket.getTimestamp(), 40 | actualPacket.getTimestamp(), MathUtils.EPSILON); 41 | assertEquals("Pose does not match", expectedPacket.getPose(), actualPacket.getPose()); 42 | assertEquals("Velocity does not match", expectedPacket.getVelocity(), actualPacket.getVelocity()); 43 | assertEquals("Rotational velocity does not match", expectedPacket.getRotationalVelocity(), 44 | actualPacket.getRotationalVelocity(), MathUtils.EPSILON); 45 | 46 | clientThread.join(); 47 | } catch (IOException | InterruptedException e) { 48 | e.printStackTrace(); 49 | } 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/OverhangCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.CommandGroup; 4 | import edu.wpi.first.wpilibj.command.InstantCommand; 5 | import edu.wpi.first.wpilibj.command.WaitCommand; 6 | import org.frcteam2910.c2019.subsystems.CargoArmSubsystem; 7 | import org.frcteam2910.c2019.subsystems.DrivetrainSubsystem; 8 | import org.frcteam2910.common.math.MathUtils; 9 | import org.frcteam2910.common.math.Vector2; 10 | 11 | public class OverhangCommand extends CommandGroup { 12 | private static final double DRIVE_SPEED = 0.25; 13 | private static final double INTAKE_SPEED = 0.75; 14 | private static final double TARGET_PITCH = Math.toRadians(-5.0); 15 | 16 | public OverhangCommand(double armAngle, boolean moveWhileArmMoves) { 17 | addSequential(new GrabHatchPanelCommand()); 18 | addParallel(new SetClimberExtendedCommand(true)); 19 | addParallel(new DriveCommand(new Vector2(DRIVE_SPEED, 0.0), 0.0, false)); 20 | addParallel(new SetBottomCargoRollerSpeedCommand(INTAKE_SPEED)); 21 | addParallel(new ExtendKickstandCommand()); 22 | addParallel(new CorrectPitchCommand(TARGET_PITCH, true)); 23 | 24 | addSequential(new WaitCommand(0.5)); 25 | addSequential(new WaitForFrontWheelsToExceedCurrentCommand(10.0)); 26 | if (moveWhileArmMoves) { 27 | addSequential(new WaitForFrontWheelsToDriveDistanceCommand(10.0)); 28 | } else { 29 | addSequential(new WaitForFrontWheelsToDriveDistanceCommand(4.0)); 30 | } 31 | if (moveWhileArmMoves) { 32 | CommandGroup group = new CommandGroup(); 33 | group.addSequential(new WaitForFrontWheelsToDriveDistanceCommand(4.0)); 34 | group.addSequential(new SetClimberExtendedCommand(false)); 35 | 36 | addParallel(group); 37 | } else { 38 | addParallel(new DriveCommand(new Vector2(MathUtils.EPSILON, 0.0), 0.0, false)); 39 | } 40 | addSequential(new SetArmAngleCommand(armAngle)); 41 | // addSequential(new InstantCommand(() -> { 42 | // DrivetrainSubsystem.getInstance().getCurrentCommand().cancel(); 43 | // })); 44 | 45 | addParallel(new DriveCommand(new Vector2(DRIVE_SPEED, 0.0), 0.0, false)); 46 | addSequential(new WaitForFrontWheelsToDriveDistanceCommand(12.0), 5.0); 47 | addSequential(new DriveCommand(Vector2.ZERO, 0.0, false)); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/FollowTrajectoryCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.GenericHID; 4 | import edu.wpi.first.wpilibj.Timer; 5 | import edu.wpi.first.wpilibj.command.Command; 6 | import org.frcteam2910.c2019.Robot; 7 | import org.frcteam2910.c2019.subsystems.DrivetrainSubsystem; 8 | import org.frcteam2910.common.control.Trajectory; 9 | import org.frcteam2910.common.math.Vector2; 10 | 11 | import java.util.function.Supplier; 12 | 13 | public class FollowTrajectoryCommand extends Command { 14 | private final Supplier trajectorySupplier; 15 | 16 | private Trajectory trajectory; 17 | 18 | public FollowTrajectoryCommand(Trajectory trajectory) { 19 | this(() -> trajectory); 20 | } 21 | 22 | public FollowTrajectoryCommand(Supplier trajectorySupplier) { 23 | this.trajectorySupplier = trajectorySupplier; 24 | 25 | requires(DrivetrainSubsystem.getInstance()); 26 | this.setRunWhenDisabled(true); 27 | } 28 | 29 | @Override 30 | protected void initialize() { 31 | trajectory = trajectorySupplier.get(); 32 | DrivetrainSubsystem.getInstance().resetKinematics(Vector2.ZERO, Timer.getFPGATimestamp()); 33 | DrivetrainSubsystem.getInstance().getFollower().follow(trajectory); 34 | } 35 | 36 | @Override 37 | protected void end() { 38 | DrivetrainSubsystem.getInstance().setSnapRotation(trajectory.calculateSegment(trajectory.getDuration()).rotation.toRadians()); 39 | 40 | new Thread(() -> { 41 | Robot.getOi().primaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kLeftRumble, 1.0); 42 | Robot.getOi().primaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kRightRumble, 1.0); 43 | Timer.delay(0.5); 44 | Robot.getOi().primaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kLeftRumble, 0.0); 45 | Robot.getOi().primaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kRightRumble, 0.0); 46 | }).start(); 47 | } 48 | 49 | @Override 50 | protected void interrupted() { 51 | end(); 52 | DrivetrainSubsystem.getInstance().getFollower().cancel(); 53 | } 54 | 55 | @Override 56 | protected boolean isFinished() { 57 | // Only finish when the trajectory is completed 58 | return DrivetrainSubsystem.getInstance().getFollower().getCurrentTrajectory().isEmpty(); 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /vision-coprocessor/src/main/java/org/frcteam2910/c2019/vision/ImagePoint.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision; 2 | 3 | import org.opencv.core.Point; 4 | 5 | public class ImagePoint implements Comparable { 6 | private double angle; 7 | private Point point; 8 | 9 | /** 10 | * Default constructor 11 | */ 12 | public ImagePoint() { 13 | this(0.0, new Point(0.0, 0.0)); 14 | } 15 | 16 | /** 17 | * Main constructor 18 | * @param angle The angle between this point and it's right principal axis 19 | * @param point The point in pixel coordinates 20 | */ 21 | public ImagePoint(double angle, Point point) { 22 | this.angle = angle; 23 | this.point = point; 24 | } 25 | 26 | /** 27 | * Return the angle for this Image Point 28 | * @return The angle for this image point 29 | */ 30 | public double getAngle() { 31 | return angle; 32 | } 33 | 34 | /** 35 | * Return the point in pixel coordinates for this point 36 | * @return The point in pixel coordinates for this image point 37 | */ 38 | public Point getPoint() { 39 | return point; 40 | } 41 | 42 | /** 43 | * Set the angle for the Image Point. This is the angle between this point and it's right principal axis 44 | * @param angle 45 | */ 46 | public void setAngle(double angle) { 47 | this.angle = angle; 48 | } 49 | 50 | /** 51 | * Set the point for this Image Point. 52 | * @param point 53 | */ 54 | public void setPoint(Point point) { 55 | this.point = point; 56 | } 57 | 58 | /** 59 | * Returns a string representation of this point for debugging purposes 60 | * @return A string representation of this point 61 | */ 62 | public String toString() { 63 | return point.toString() + ", " + Math.toDegrees(angle); 64 | } 65 | 66 | /** 67 | * Compares against another point based on the angle. 68 | * @param other The other point to compare to 69 | * @return returns 1 if the other point is greater than this point, -1 if this point is less than the other point, and 0 if this point is equal to the other point 70 | */ 71 | @Override 72 | public int compareTo(ImagePoint other) { 73 | if (this.angle < other.getAngle()) { 74 | return -1; 75 | } 76 | if (this.angle == other.getAngle()) { 77 | return 0; 78 | } 79 | return 1; 80 | } 81 | } 82 | -------------------------------------------------------------------------------- /gradlew.bat: -------------------------------------------------------------------------------- 1 | @if "%DEBUG%" == "" @echo off 2 | @rem ########################################################################## 3 | @rem 4 | @rem Gradle startup script for Windows 5 | @rem 6 | @rem ########################################################################## 7 | 8 | @rem Set local scope for the variables with windows NT shell 9 | if "%OS%"=="Windows_NT" setlocal 10 | 11 | set DIRNAME=%~dp0 12 | if "%DIRNAME%" == "" set DIRNAME=. 13 | set APP_BASE_NAME=%~n0 14 | set APP_HOME=%DIRNAME% 15 | 16 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 17 | set DEFAULT_JVM_OPTS="-Xmx64m" 18 | 19 | @rem Find java.exe 20 | if defined JAVA_HOME goto findJavaFromJavaHome 21 | 22 | set JAVA_EXE=java.exe 23 | %JAVA_EXE% -version >NUL 2>&1 24 | if "%ERRORLEVEL%" == "0" goto init 25 | 26 | echo. 27 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 28 | echo. 29 | echo Please set the JAVA_HOME variable in your environment to match the 30 | echo location of your Java installation. 31 | 32 | goto fail 33 | 34 | :findJavaFromJavaHome 35 | set JAVA_HOME=%JAVA_HOME:"=% 36 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 37 | 38 | if exist "%JAVA_EXE%" goto init 39 | 40 | echo. 41 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 42 | echo. 43 | echo Please set the JAVA_HOME variable in your environment to match the 44 | echo location of your Java installation. 45 | 46 | goto fail 47 | 48 | :init 49 | @rem Get command-line arguments, handling Windows variants 50 | 51 | if not "%OS%" == "Windows_NT" goto win9xME_args 52 | 53 | :win9xME_args 54 | @rem Slurp the command line arguments. 55 | set CMD_LINE_ARGS= 56 | set _SKIP=2 57 | 58 | :win9xME_args_slurp 59 | if "x%~1" == "x" goto execute 60 | 61 | set CMD_LINE_ARGS=%* 62 | 63 | :execute 64 | @rem Setup the command line 65 | 66 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 67 | 68 | @rem Execute Gradle 69 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% 70 | 71 | :end 72 | @rem End local scope for the variables with windows NT shell 73 | if "%ERRORLEVEL%"=="0" goto mainEnd 74 | 75 | :fail 76 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 77 | rem the _cmd.exe /c_ return code! 78 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 79 | exit /b 1 80 | 81 | :mainEnd 82 | if "%OS%"=="Windows_NT" endlocal 83 | 84 | :omega 85 | -------------------------------------------------------------------------------- /vision-coprocessor/src/test/java/VisionPipelineTest.java: -------------------------------------------------------------------------------- 1 | import edu.wpi.first.networktables.NetworkTable; 2 | import edu.wpi.first.networktables.NetworkTableInstance; 3 | import edu.wpi.first.wpiutil.RuntimeLoader; 4 | import org.frcteam2910.c2019.vision.VisionTargetingPipeline; 5 | import org.frcteam2910.c2019.vision.drivers.Limelight; 6 | import org.frcteam2910.common.math.RigidTransform2; 7 | import org.frcteam2910.common.math.Vector2; 8 | import org.opencv.core.*; 9 | 10 | import java.io.IOException; 11 | import java.util.ArrayList; 12 | 13 | public class VisionPipelineTest { 14 | public static void main(String[] args) { 15 | // Initialize Networktables 16 | NetworkTableInstance instance = NetworkTableInstance.getDefault(); 17 | instance.startClientTeam(2910); 18 | NetworkTable table = instance.getTable("limelight-cargo"); 19 | 20 | // Load the native library 21 | try { 22 | RuntimeLoader loader = new RuntimeLoader<>(Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class); 23 | loader.loadLibraryHashed(); 24 | } catch (IOException ex) { 25 | ex.printStackTrace(); 26 | System.exit(1); 27 | } 28 | 29 | // Declare our object points, camera matrix, and distortion coefficients respectively 30 | double[][] objp = new double[][] { 31 | {5.9363, 2.9128, 0.0}, 32 | {4.0, 2.4120, 0.0}, 33 | {-4.0, 2.4120, 0.0}, 34 | {-5.9363, 2.9128, 0.0}, 35 | {-7.3134, -2.4120, 0.0}, 36 | {-5.3771, -2.9128, 0.0}, 37 | {5.3771, -2.9128, 0.0}, 38 | {7.3134, -2.4120, 0.0} 39 | }; 40 | 41 | double[][] mtx = new double[][] { 42 | {773.86723397, 0.0, 431.29426601}, 43 | {0.0, 777.62582296, 306.80364947}, 44 | {0.0, 0.0, 1.0} 45 | }; 46 | 47 | double[] dist = new double[] {3.15600348e-01, -1.17776987e+00, -9.30063427e-03, 1.46275541e-03, 1.61055001e+00}; 48 | 49 | // Create an instance of our vision targeting pipeline 50 | VisionTargetingPipeline pipeline = new VisionTargetingPipeline(new Limelight(table), objp, dist, mtx); 51 | RigidTransform2 transform; 52 | 53 | while (true) { 54 | try { 55 | transform = pipeline.getTranslation(); 56 | System.out.println(transform.toString()); 57 | } catch (Exception e) { 58 | e.printStackTrace(); 59 | } 60 | } 61 | } 62 | } -------------------------------------------------------------------------------- /vision-coprocessor/src/main/java/org/frcteam2910/c2019/vision/RobotStateEstimator.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision; 2 | 3 | import org.frcteam2910.common.math.RigidTransform2; 4 | import org.frcteam2910.common.math.Vector2; 5 | import org.frcteam2910.common.util.Interpolable; 6 | import org.frcteam2910.common.util.InterpolatingDouble; 7 | import org.frcteam2910.common.util.InterpolatingTreeMap; 8 | 9 | public class RobotStateEstimator { 10 | private static final int MAXIMUM_STATE_COUNT = 400; 11 | 12 | private static final Object stateLock = new Object(); 13 | private static InterpolatingTreeMap states = new InterpolatingTreeMap<>(MAXIMUM_STATE_COUNT); 14 | 15 | public static void addState(double timestamp, RigidTransform2 pose, Vector2 velocity, double rotationalVelocity) { 16 | InterpolatingDouble key = new InterpolatingDouble(timestamp); 17 | State state = new State(timestamp, pose, velocity, rotationalVelocity); 18 | 19 | synchronized (stateLock) { 20 | states.put(key, state); 21 | } 22 | } 23 | 24 | public static State estimateState(double timestamp) { 25 | InterpolatingDouble key = new InterpolatingDouble(timestamp); 26 | 27 | synchronized (stateLock) { 28 | return states.getInterpolated(key); 29 | } 30 | } 31 | 32 | public static class State implements Interpolable { 33 | private final double timestamp; 34 | private final RigidTransform2 pose; 35 | private final Vector2 velocity; 36 | private final double rotationalVelocity; 37 | 38 | public State(double timestamp, RigidTransform2 pose, Vector2 velocity, double rotationalVelocity) { 39 | this.timestamp = timestamp; 40 | this.pose = pose; 41 | this.velocity = velocity; 42 | this.rotationalVelocity = rotationalVelocity; 43 | } 44 | 45 | public double getTimestamp() { 46 | return timestamp; 47 | } 48 | 49 | public RigidTransform2 getPose() { 50 | return pose; 51 | } 52 | 53 | public Vector2 getVelocity() { 54 | return velocity; 55 | } 56 | 57 | @Override 58 | public State interpolate(State other, double t) { 59 | return new State( 60 | new InterpolatingDouble(timestamp).interpolate(new InterpolatingDouble(other.timestamp), t).value, 61 | pose.interpolate(other.pose, t), 62 | velocity.interpolate(other.velocity, t), 63 | new InterpolatingDouble(rotationalVelocity).interpolate(new InterpolatingDouble(other.rotationalVelocity), t).value 64 | ); 65 | } 66 | } 67 | } 68 | -------------------------------------------------------------------------------- /vision-coprocessor/build.gradle: -------------------------------------------------------------------------------- 1 | plugins { 2 | id 'application' 3 | id 'java' 4 | 5 | // This plugin is responsible for deploying the code to the raspberry pi 6 | id 'jaci.gradle.EmbeddedTools' 7 | 8 | // This plugin packages all the dependencies into a "fat jar" 9 | id 'com.github.johnrengelman.shadow' version '4.0.4' 10 | } 11 | 12 | sourceCompatibility = JavaVersion.VERSION_11 13 | targetCompatibility = JavaVersion.VERSION_11 14 | 15 | application { 16 | mainClassName = "org.frcteam2910.c2019.vision.Main" 17 | } 18 | 19 | repositories { 20 | mavenCentral() 21 | 22 | maven { 23 | url = 'http://first.wpi.edu/FRC/roborio/maven/release/' 24 | } 25 | } 26 | 27 | deploy { 28 | targets { 29 | target('raspberrypi') { 30 | directory = '/home/pi' 31 | 32 | locations { 33 | ssh { 34 | address = 'raspberrypi.local' 35 | user = 'pi' 36 | password = 'raspberry' 37 | } 38 | } 39 | } 40 | } 41 | 42 | artifacts { 43 | javaArtifact('visionapp') { 44 | jar = 'shadowJar' 45 | targets << 'raspberrypi' 46 | } 47 | 48 | fileArtifact('serviceFile') { 49 | file = file('pivision.service') // Set the file to deploy. Required. 50 | targets << 'raspberrypi' 51 | postdeploy << { 52 | // Move service file to appropriate location 53 | execute 'sudo mv /home/pi/pivision.service /etc/systemd/system/pivision.service' 54 | // Ensure the service is enabled 55 | execute 'sudo systemctl enable pivision.service' 56 | // Restart the service 57 | execute 'sudo systemctl restart pivision.service' 58 | } 59 | } 60 | } 61 | } 62 | 63 | 64 | dependencies { 65 | compile group: 'edu.wpi.first.wpiutil', name: 'wpiutil-java', version: '2019.3.1' 66 | 67 | compile group: 'edu.wpi.first.thirdparty.frc2019.opencv', name: 'opencv-java', version: '3.4.4-4' 68 | runtime group: 'edu.wpi.first.thirdparty.frc2019.opencv', name: 'opencv-jni', version: '3.4.4-4', classifier: 'linuxraspbian' 69 | 70 | // The desktop natives allow for testing locally before deploying 71 | runtime group: 'edu.wpi.first.thirdparty.frc2019.opencv', name: 'opencv-jni', version: '3.4.4-4', classifier: 'linuxx86-64' 72 | runtime group: 'edu.wpi.first.thirdparty.frc2019.opencv', name: 'opencv-jni', version: '3.4.4-4', classifier: 'osxx86-64' 73 | runtime group: 'edu.wpi.first.thirdparty.frc2019.opencv', name: 'opencv-jni', version: '3.4.4-4', classifier: 'windowsx86-64' 74 | 75 | compile group: 'edu.wpi.first.ntcore', name: 'ntcore-java', version: '2019.3.1' 76 | runtime group: 'edu.wpi.first.ntcore', name: 'ntcore-jni', version: '2019.3.1', classifier: 'all' 77 | 78 | compile project(':common') 79 | compile project('api') 80 | } 81 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/VisionTargetCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import edu.wpi.first.wpilibj.command.Command; 5 | import org.frcteam2910.c2019.Robot; 6 | import org.frcteam2910.c2019.subsystems.DrivetrainSubsystem; 7 | import org.frcteam2910.c2019.subsystems.VisionSubsystem; 8 | import org.frcteam2910.c2019.vision.api.Gamepiece; 9 | import org.frcteam2910.common.control.PidConstants; 10 | import org.frcteam2910.common.control.PidController; 11 | import org.frcteam2910.common.math.Rotation2; 12 | import org.frcteam2910.common.math.Vector2; 13 | import org.frcteam2910.common.robot.drivers.Limelight; 14 | 15 | public class VisionTargetCommand extends Command { 16 | private final PidController controller = new PidController(new PidConstants(0.9, 0.0, 1.0)); 17 | 18 | private final Limelight limelight; 19 | private final boolean flipRobotOriented; 20 | 21 | private double lastTime; 22 | 23 | public VisionTargetCommand(Gamepiece gamepiece) { 24 | limelight = VisionSubsystem.getInstance().getLimelight(gamepiece); 25 | flipRobotOriented = gamepiece == Gamepiece.HATCH_PANEL; 26 | 27 | requires(DrivetrainSubsystem.getInstance()); 28 | } 29 | 30 | @Override 31 | protected void initialize() { 32 | lastTime = Timer.getFPGATimestamp(); 33 | 34 | limelight.setCamMode(Limelight.CamMode.VISION); 35 | } 36 | 37 | @Override 38 | protected void execute() { 39 | double time = Timer.getFPGATimestamp(); 40 | double dt = time - lastTime; 41 | 42 | double scalar = 0.15; 43 | if (Robot.getOi().secondaryController.getRightTriggerAxis().get() > 0.1) { 44 | scalar = 0.3; 45 | } 46 | 47 | double forward = Robot.getOi().primaryController.getLeftYAxis().get(true) * scalar; 48 | double strafe; 49 | if (limelight.hasTarget() && Math.abs(Robot.getOi().primaryController.getLeftXAxis().get(false)) < 0.75) { 50 | strafe = controller.calculate(limelight.getTargetPosition().x, dt); 51 | } else { 52 | strafe = Robot.getOi().primaryController.getLeftXAxis().get(true) * scalar; 53 | } 54 | double rotation = Robot.getOi().primaryController.getRightXAxis().get(true); 55 | if (Robot.getOi().secondaryController.getRightTriggerAxis().get() > 0.1) { 56 | rotation = 0.0; 57 | DrivetrainSubsystem.getInstance().setSnapRotation(0.0); 58 | } 59 | 60 | Vector2 translation = new Vector2(forward, strafe); 61 | if (flipRobotOriented) { 62 | translation = translation.rotateBy(Rotation2.fromDegrees(180.0)); 63 | } 64 | 65 | DrivetrainSubsystem.getInstance().holonomicDrive(translation, rotation, false); 66 | } 67 | 68 | @Override 69 | protected void end() { 70 | limelight.setCamMode(Limelight.CamMode.DRIVER); 71 | } 72 | 73 | @Override 74 | protected boolean isFinished() { 75 | return false; 76 | } 77 | } 78 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/subsystems/ClimberSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.subsystems; 2 | 3 | import edu.wpi.first.wpilibj.Notifier; 4 | import edu.wpi.first.wpilibj.Solenoid; 5 | import org.frcteam2910.c2019.RobotMap; 6 | import org.frcteam2910.common.robot.subsystems.Subsystem; 7 | 8 | public class ClimberSubsystem extends Subsystem { 9 | private static final ClimberSubsystem instance = new ClimberSubsystem(); 10 | 11 | private Solenoid climberSolenoid = new Solenoid(RobotMap.CLIMBER_SOLENOID_MODULE, 12 | RobotMap.CLIMBER_SOLENOID_CHANNEL); 13 | private Solenoid kickstandSolenoid = new Solenoid(RobotMap.KICKSTAND_SOLENOID_MODULE, 14 | RobotMap.KICKSTAND_SOLENOID_CHANNEL); 15 | 16 | private final Object canLock = new Object(); 17 | private boolean climberStateChanged = false; 18 | private boolean climberExtended = false; 19 | private boolean kickstandStateChanged = true; 20 | private boolean kickstandExtended = false; 21 | 22 | private Notifier canUpdateThread = new Notifier(() -> { 23 | boolean localClimberStateChanged; 24 | boolean localClimberExtended; 25 | boolean localKickstandStateChanged; 26 | boolean localKickstandExtended; 27 | 28 | synchronized (canLock) { 29 | localClimberStateChanged = climberStateChanged; 30 | localClimberExtended = climberExtended; 31 | climberStateChanged = false; 32 | 33 | localKickstandStateChanged = kickstandStateChanged; 34 | localKickstandExtended = kickstandExtended; 35 | kickstandStateChanged = false; 36 | } 37 | 38 | if (localClimberStateChanged) { 39 | climberSolenoid.set(localClimberExtended); 40 | } 41 | 42 | // Kickstand is inverted 43 | if (localKickstandStateChanged) { 44 | kickstandSolenoid.set(!localKickstandExtended); 45 | } 46 | }); 47 | 48 | private ClimberSubsystem() { 49 | canUpdateThread.startPeriodic(0.02); 50 | } 51 | 52 | public static ClimberSubsystem getInstance() { 53 | return instance; 54 | } 55 | 56 | public void setClimberExtended(boolean extended) { 57 | synchronized (canLock) { 58 | climberStateChanged = climberExtended != extended; 59 | climberExtended = extended; 60 | } 61 | } 62 | 63 | public void extendKickstand() { 64 | synchronized (canLock) { 65 | kickstandStateChanged = !kickstandExtended; 66 | kickstandExtended = true; 67 | } 68 | } 69 | 70 | public void retractKickstand() { 71 | synchronized (canLock) { 72 | kickstandStateChanged = kickstandExtended; 73 | kickstandExtended = false; 74 | } 75 | } 76 | 77 | @Override 78 | public void outputToSmartDashboard() { 79 | 80 | } 81 | 82 | @Override 83 | public void zeroSensors() { 84 | 85 | } 86 | 87 | @Override 88 | public void stop() { 89 | 90 | } 91 | 92 | @Override 93 | public void initDefaultCommand() { 94 | 95 | } 96 | } 97 | -------------------------------------------------------------------------------- /vision-coprocessor/src/main/java/org/frcteam2910/c2019/vision/ImagePoint2019.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision; 2 | 3 | import org.opencv.core.Point; 4 | 5 | public class ImagePoint2019 extends ImagePoint { 6 | private static double MIN_ANGLE_DIFFERENCE = Math.toRadians(0.5); 7 | 8 | /** 9 | * Default constructor 10 | */ 11 | public ImagePoint2019() { 12 | this(0.0, new Point(0.0, 0.0)); 13 | } 14 | 15 | /** 16 | * Main constructor 17 | * @param angle Angle between this point and the horizontal 18 | * @param point The point represented in pixel coordinates 19 | */ 20 | public ImagePoint2019(double angle, Point point) { 21 | super(angle, point); 22 | } 23 | 24 | /** 25 | * Compares the current point with the other. If the instance of this point is just an ImagePoint, or if the angle between this point and the other ess than 0.5 degrees, then call the super compareTo(). 26 | * Otherwise, this means the angle of the two points are too close so we must compare them by there x components. This is done by finding which quadrant the points are in then returning either 1 or -1 depending on which point is which. 27 | * Thus, this compareTo() is simply a backup in case the angles are too close. 28 | * @param other The other point to compare to 29 | * @return 30 | */ 31 | @Override 32 | public int compareTo(ImagePoint other) { 33 | if (!(other instanceof ImagePoint2019) || (Math.abs(other.getAngle() - this.getAngle()) > MIN_ANGLE_DIFFERENCE)) { 34 | return super.compareTo(other); 35 | } 36 | if ((0.0 < this.getAngle()) && (this.getAngle() < (Math.PI / 2))) { 37 | if (this.getPoint().x < other.getPoint().x) { 38 | // "this" is point 2 39 | return 1; 40 | } 41 | if (this.getPoint().x > other.getPoint().x) { 42 | // "this" is point 1 43 | return -1; 44 | } 45 | return 0; 46 | } 47 | if (((Math.PI / 2) < this.getAngle()) && (this.getAngle() < Math.PI)) { 48 | if (this.getPoint().x < other.getPoint().x) { 49 | // "this" is point 4 50 | return 1; 51 | } 52 | if (this.getPoint().x > other.getPoint().x) { 53 | // "this" is point 3 54 | return -1; 55 | } 56 | return 0; 57 | } 58 | if ((Math.PI < this.getAngle()) && (this.getAngle() < ((3 * Math.PI) / 2))) { 59 | if (this.getPoint().x < other.getPoint().x) { 60 | // "this" is point 5 61 | return -1; 62 | } 63 | if (this.getPoint().x > other.getPoint().x) { 64 | // "this" is point 6 65 | return 1; 66 | } 67 | return 0; 68 | } 69 | if ((((3 * Math.PI) / 2) < this.getAngle()) && (this.getAngle() < (2 * Math.PI))) { 70 | if (this.getPoint().x < other.getPoint().x) { 71 | // "this" is point 7 72 | return -1; 73 | } 74 | if (this.getPoint().x > other.getPoint().x) { 75 | // "this" is point 8 76 | return 1; 77 | } 78 | return 0; 79 | } 80 | return 0; 81 | } 82 | } 83 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/CorrectPitchCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import edu.wpi.first.wpilibj.command.Command; 5 | import org.frcteam2910.c2019.subsystems.CargoArmSubsystem; 6 | import org.frcteam2910.c2019.subsystems.DrivetrainSubsystem; 7 | import org.frcteam2910.common.drivers.SwerveModule; 8 | 9 | import java.util.ArrayList; 10 | import java.util.List; 11 | 12 | public class CorrectPitchCommand extends Command { 13 | private static final double WHEEL_CONTACT_WAIT = 0.5; 14 | 15 | private static final double WHEEL_CONTACT_SPEED = 5.0; 16 | 17 | private final double targetPitch; 18 | private final boolean pitchOnlyDown; 19 | 20 | private final Timer wheelContactTimer = new Timer(); 21 | private boolean wheelContanctTimerRunning = false; 22 | 23 | private List frontModules = new ArrayList<>(); 24 | private double[] lastModuleDistances; 25 | private double lastExecuteTime = Timer.getFPGATimestamp(); 26 | 27 | 28 | public CorrectPitchCommand(double pitch, boolean pitchOnlyDown) { 29 | this.targetPitch = pitch; 30 | this.pitchOnlyDown = pitchOnlyDown; 31 | 32 | // Find all the swerve modules which are in the front and add them to the list 33 | for (SwerveModule module : DrivetrainSubsystem.getInstance().getSwerveModules()) { 34 | if (module.getModulePosition().x > 0.0) { 35 | frontModules.add(module); 36 | } 37 | } 38 | lastModuleDistances = new double[frontModules.size()]; 39 | 40 | requires(CargoArmSubsystem.getInstance()); 41 | } 42 | 43 | @Override 44 | protected void initialize() { 45 | wheelContactTimer.stop(); 46 | wheelContactTimer.reset(); 47 | wheelContanctTimerRunning = false; 48 | CargoArmSubsystem.getInstance().setTargetPitch(targetPitch, pitchOnlyDown); 49 | lastExecuteTime = Double.NaN; 50 | } 51 | 52 | @Override 53 | protected void execute() { 54 | double now = Timer.getFPGATimestamp(); 55 | if (Double.isNaN(lastExecuteTime)) { 56 | lastExecuteTime = now; 57 | return; 58 | } 59 | double dt = now - lastExecuteTime; 60 | lastExecuteTime = now; 61 | 62 | double averageSpeed = 0.0; 63 | for (int i = 0; i < frontModules.size(); i++) { 64 | double distance = frontModules.get(i).getCurrentDistance(); 65 | double speed = Math.abs((distance - lastModuleDistances[i]) / dt); 66 | lastModuleDistances[i] = distance; 67 | averageSpeed += speed; 68 | } 69 | averageSpeed /= frontModules.size(); 70 | 71 | if (averageSpeed < WHEEL_CONTACT_SPEED) { 72 | if (!wheelContanctTimerRunning) { 73 | wheelContactTimer.start(); 74 | wheelContanctTimerRunning = true; 75 | } 76 | } else { 77 | if (wheelContanctTimerRunning) { 78 | wheelContactTimer.stop(); 79 | wheelContactTimer.reset(); 80 | wheelContanctTimerRunning = false; 81 | } 82 | } 83 | } 84 | 85 | @Override 86 | protected void end() { 87 | wheelContactTimer.stop(); 88 | CargoArmSubsystem.getInstance().disable(); 89 | } 90 | 91 | @Override 92 | protected boolean isFinished() { 93 | return wheelContanctTimerRunning && wheelContactTimer.get() > WHEEL_CONTACT_WAIT; 94 | } 95 | } 96 | -------------------------------------------------------------------------------- /vision-coprocessor/src/main/java/org/frcteam2910/c2019/vision/VisionCalculationLoop.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision; 2 | 3 | import edu.wpi.first.networktables.NetworkTable; 4 | import edu.wpi.first.networktables.NetworkTableEntry; 5 | import edu.wpi.first.networktables.NetworkTableInstance; 6 | import org.frcteam2910.c2019.vision.api.Gamepiece; 7 | import org.frcteam2910.c2019.vision.drivers.Limelight; 8 | import org.frcteam2910.common.Logger; 9 | import org.frcteam2910.common.math.RigidTransform2; 10 | 11 | import java.util.HashMap; 12 | import java.util.Map; 13 | 14 | public class VisionCalculationLoop implements Runnable { 15 | private static final Logger LOGGER = new Logger(VisionTargetingPipeline.class); 16 | 17 | private NetworkTable table = NetworkTableInstance.getDefault().getTable("vision-coprocessor"); 18 | 19 | private static final String CARGO_LIMELIGHT_TABLE_NAME = "limelight-cargo"; 20 | private static final String HATCH_LIMELIGHT_TABLE_NAME = "limelight-hatch"; 21 | 22 | private Map limelights = new HashMap<>(); 23 | private Map pipelines = new HashMap<>(); 24 | 25 | public VisionCalculationLoop() { 26 | limelights.put(Gamepiece.CARGO, new Limelight(NetworkTableInstance.getDefault().getTable(CARGO_LIMELIGHT_TABLE_NAME))); 27 | limelights.put(Gamepiece.HATCH_PANEL, new Limelight(NetworkTableInstance.getDefault().getTable(HATCH_LIMELIGHT_TABLE_NAME))); 28 | 29 | pipelines.put(Gamepiece.CARGO, new VisionTargetingPipeline2019(limelights.get(Gamepiece.CARGO))); 30 | pipelines.put(Gamepiece.HATCH_PANEL, new VisionTargetingPipeline2019(limelights.get(Gamepiece.HATCH_PANEL))); 31 | } 32 | 33 | @Override 34 | public void run() { 35 | NetworkTableEntry activeEntry = table.getEntry("active"); 36 | NetworkTableEntry cameraEntry = table.getEntry("camera"); 37 | 38 | activeEntry.setBoolean(false); 39 | cameraEntry.setNumber(Gamepiece.CARGO.ordinal()); 40 | 41 | NetworkTableEntry xEntry = table.getEntry("x"); 42 | NetworkTableEntry yEntry = table.getEntry("y"); 43 | NetworkTableEntry angleEntry = table.getEntry("angle"); 44 | 45 | xEntry.setNumber(Double.NaN); 46 | yEntry.setNumber(Double.NaN); 47 | angleEntry.setNumber(Double.NaN); 48 | 49 | boolean lastActive = false; 50 | Gamepiece lastGamepiece = Gamepiece.CARGO; 51 | 52 | while (!Thread.interrupted()) { 53 | boolean active = activeEntry.getBoolean(false); 54 | 55 | Gamepiece gamepiece = Gamepiece.values()[cameraEntry.getNumber(0).intValue()]; 56 | 57 | if (gamepiece != lastGamepiece) { 58 | limelights.get(lastGamepiece).setCamMode(Limelight.CamMode.DRIVER); 59 | limelights.get(lastGamepiece).setLedMode(Limelight.LedMode.OFF); 60 | } 61 | 62 | Limelight limelight = limelights.get(gamepiece); 63 | VisionTargetingPipeline pipeline = pipelines.get(gamepiece); 64 | if (active) { 65 | if (gamepiece != lastGamepiece || !lastActive) { 66 | limelight.setCamMode(Limelight.CamMode.VISION); 67 | limelight.setLedMode(Limelight.LedMode.DEFAULT); 68 | LOGGER.info("Enabling vision for %s", gamepiece); 69 | } 70 | 71 | try { 72 | RigidTransform2 transform = pipeline.getTranslation(); 73 | 74 | xEntry.setNumber(transform.translation.y); 75 | yEntry.setNumber(transform.translation.x); 76 | angleEntry.setNumber(transform.rotation.toRadians()); 77 | } catch (Exception e) { 78 | LOGGER.error(e); 79 | } 80 | } else if (lastActive) { 81 | limelight.setCamMode(Limelight.CamMode.DRIVER); 82 | limelight.setLedMode(Limelight.LedMode.OFF); 83 | 84 | xEntry.setNumber(Double.NaN); 85 | yEntry.setNumber(Double.NaN); 86 | angleEntry.setNumber(Double.NaN); 87 | } 88 | 89 | lastActive = active; 90 | lastGamepiece = gamepiece; 91 | } 92 | } 93 | } 94 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/Robot.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019; 2 | 3 | import edu.wpi.first.wpilibj.TimedRobot; 4 | import edu.wpi.first.wpilibj.command.Command; 5 | import edu.wpi.first.wpilibj.command.Scheduler; 6 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 7 | import org.frcteam2910.c2019.autonomous.AutonomousSelector; 8 | import org.frcteam2910.c2019.autonomous.AutonomousTrajectories; 9 | import org.frcteam2910.c2019.subsystems.*; 10 | import org.frcteam2910.c2019.vision.api.Gamepiece; 11 | import org.frcteam2910.common.robot.drivers.Limelight; 12 | import org.frcteam2910.common.robot.drivers.NavX; 13 | import org.frcteam2910.common.robot.subsystems.SubsystemManager; 14 | 15 | public class Robot extends TimedRobot { 16 | private static final double UPDATE_DT = 5e-3; // 5 ms 17 | 18 | private final SubsystemManager subsystemManager = new SubsystemManager( 19 | ClimberSubsystem.getInstance(), 20 | DrivetrainSubsystem.getInstance(), 21 | CargoGrabberSubsystem.getInstance(), 22 | CargoArmSubsystem.getInstance(), 23 | HatchFloorGathererSubsystem.getInstance(), 24 | HatchPlacerSubsystem.getInstance(), 25 | VisionSubsystem.getInstance() 26 | ); 27 | 28 | private static final OI oi = new OI(); 29 | 30 | private AutonomousTrajectories autonomousTrajectories = new AutonomousTrajectories(DrivetrainSubsystem.CONSTRAINTS); 31 | private AutonomousSelector autonomousSelector = new AutonomousSelector(autonomousTrajectories); 32 | 33 | private Command autonomousCommand = null; 34 | 35 | public Robot() { 36 | oi.bindButtons(autonomousSelector); 37 | } 38 | 39 | public static OI getOi() { 40 | return oi; 41 | } 42 | 43 | @Override 44 | public void robotInit() { 45 | SmartDashboard.putBoolean("Limelight Calibration Mode", false); 46 | 47 | subsystemManager.enableKinematicLoop(UPDATE_DT); 48 | } 49 | 50 | @Override 51 | public void robotPeriodic() { 52 | subsystemManager.outputToSmartDashboard(); 53 | 54 | SmartDashboard.putNumber("Arm Angle", 55 | Math.toDegrees(CargoArmSubsystem.getInstance().getCurrentAngle())); 56 | SmartDashboard.putNumber("Robot Angle", 57 | Superstructure.getInstance().getGyroscope().getAngle().toDegrees()); 58 | SmartDashboard.putNumber("Gyro Pitch", 59 | Math.toDegrees(Superstructure.getInstance().getGyroscope().getAxis(NavX.Axis.ROLL))); 60 | SmartDashboard.putBoolean("Is Competition Bot", 61 | Superstructure.getInstance().isCompetitionBot()); 62 | SmartDashboard.putBoolean("Is Practice Bot", 63 | Superstructure.getInstance().isPracticeBot()); 64 | } 65 | 66 | @Override 67 | public void teleopInit() { 68 | // if (autonomousCommand != null) { 69 | // autonomousCommand.cancel(); 70 | // autonomousCommand = null; 71 | // } 72 | } 73 | 74 | @Override 75 | public void teleopPeriodic() { 76 | Scheduler.getInstance().run(); 77 | } 78 | 79 | @Override 80 | public void autonomousInit() { 81 | if (autonomousCommand != null) { 82 | autonomousCommand.cancel(); 83 | } 84 | 85 | autonomousCommand = autonomousSelector.getCommand(); 86 | autonomousCommand.start(); 87 | } 88 | 89 | @Override 90 | public void autonomousPeriodic() { 91 | Scheduler.getInstance().run(); 92 | } 93 | 94 | @Override 95 | public void disabledInit() { 96 | // if (autonomousCommand != null) { 97 | // autonomousCommand.cancel(); 98 | // autonomousCommand = null; 99 | // } 100 | // Scheduler.getInstance().removeAll(); 101 | } 102 | 103 | @Override 104 | public void disabledPeriodic() { 105 | boolean calibrationMode = SmartDashboard.getBoolean("Limelight Calibration Mode", false); 106 | 107 | Limelight.CamMode mode = calibrationMode ? Limelight.CamMode.VISION : Limelight.CamMode.DRIVER; 108 | VisionSubsystem.getInstance().getLimelight(Gamepiece.HATCH_PANEL).setCamMode(mode); 109 | VisionSubsystem.getInstance().getLimelight(Gamepiece.CARGO).setCamMode(mode); 110 | } 111 | } 112 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/subsystems/Superstructure.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.subsystems; 2 | 3 | import edu.wpi.first.wpilibj.SPI; 4 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 5 | import org.frcteam2910.common.Logger; 6 | import org.frcteam2910.common.robot.drivers.NavX; 7 | 8 | import java.io.IOException; 9 | import java.net.NetworkInterface; 10 | import java.util.ArrayList; 11 | import java.util.Arrays; 12 | import java.util.Enumeration; 13 | import java.util.List; 14 | 15 | public class Superstructure { 16 | private static final Logger LOGGER = new Logger(Superstructure.class); 17 | 18 | private static final byte[] COMPETITION_BOT_MAC_ADDRESS = new byte[] { 19 | 0x00, (byte) 0x80, 0x2f, 0x24, 0x19, (byte) 0xc0 20 | }; 21 | private static final byte[] PRACTICE_BOT_MAC_ADDRESS = new byte[] { 22 | 0x00, (byte) 0x80, 0x2f, 0x17, (byte) 0xe5, 0x18 23 | }; 24 | 25 | private static final Superstructure instance = new Superstructure(); 26 | 27 | private NavX navX = new NavX(SPI.Port.kMXP); 28 | 29 | private boolean competitionBot; 30 | private boolean practiceBot; 31 | 32 | private Superstructure() { 33 | navX.calibrate(); 34 | navX.setInverted(true); 35 | 36 | List macAddresses; 37 | try { 38 | macAddresses = getMacAddresses(); 39 | } catch (IOException e) { 40 | // Don't crash, just log the stacktrace and continue without any mac addresses. 41 | LOGGER.error(e); 42 | macAddresses = new ArrayList<>(); 43 | } 44 | 45 | for (byte[] macAddress : macAddresses) { 46 | // First check if we are the competition bot 47 | if (Arrays.compare(COMPETITION_BOT_MAC_ADDRESS, macAddress) == 0) { 48 | competitionBot = true; 49 | break; 50 | } 51 | 52 | // Next check if we are the practice bot 53 | if (Arrays.compare(PRACTICE_BOT_MAC_ADDRESS, macAddress) == 0) { 54 | practiceBot = true; 55 | break; 56 | } 57 | } 58 | 59 | if (!competitionBot && !practiceBot) { 60 | String[] macAddressStrings = macAddresses.stream() 61 | .map(Superstructure::macToString) 62 | .toArray(String[]::new); 63 | 64 | SmartDashboard.putStringArray("MAC Addresses", macAddressStrings); 65 | SmartDashboard.putString("Competition Bot MAC Address", macToString(COMPETITION_BOT_MAC_ADDRESS)); 66 | SmartDashboard.putString("Practice Bot MAC Address", macToString(PRACTICE_BOT_MAC_ADDRESS)); 67 | 68 | // If something goes terribly wrong we still want to use the competition bot stuff in competition. 69 | competitionBot = true; 70 | } 71 | } 72 | 73 | /** 74 | * Gets the MAC addresses of all present network adapters. 75 | * 76 | * @return the MAC addresses of all network adapters. 77 | */ 78 | private static List getMacAddresses() throws IOException { 79 | List macAddresses = new ArrayList<>(); 80 | 81 | Enumeration networkInterfaces = NetworkInterface.getNetworkInterfaces(); 82 | 83 | NetworkInterface networkInterface; 84 | while (networkInterfaces.hasMoreElements()) { 85 | networkInterface = networkInterfaces.nextElement(); 86 | 87 | byte[] address = networkInterface.getHardwareAddress(); 88 | if (address == null) { 89 | continue; 90 | } 91 | 92 | macAddresses.add(address); 93 | } 94 | 95 | return macAddresses; 96 | } 97 | 98 | private static String macToString(byte[] address) { 99 | StringBuilder builder = new StringBuilder(); 100 | for (int i = 0; i < address.length; i++) { 101 | if (i != 0) { 102 | builder.append(':'); 103 | } 104 | builder.append(String.format("%02X", address[i])); 105 | } 106 | return builder.toString(); 107 | } 108 | 109 | public static Superstructure getInstance() { 110 | return instance; 111 | } 112 | 113 | public NavX getGyroscope() { 114 | return navX; 115 | } 116 | 117 | public boolean isCompetitionBot() { 118 | return competitionBot; 119 | } 120 | 121 | public boolean isPracticeBot() { 122 | return practiceBot; 123 | } 124 | } 125 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/subsystems/HatchPlacerSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.subsystems; 2 | 3 | import edu.wpi.first.wpilibj.DigitalInput; 4 | import edu.wpi.first.wpilibj.Notifier; 5 | import edu.wpi.first.wpilibj.Solenoid; 6 | import org.frcteam2910.c2019.RobotMap; 7 | import org.frcteam2910.common.robot.subsystems.Subsystem; 8 | 9 | public class HatchPlacerSubsystem extends Subsystem { 10 | private static final double CAN_UPDATE_RATE = 50.0; 11 | 12 | private static final HatchPlacerSubsystem instance = new HatchPlacerSubsystem(); 13 | 14 | private Solenoid extenderSolenoid = new Solenoid(RobotMap.HATCH_EXTENDER_SOLENOID_MODULE, 15 | RobotMap.HATCH_EXTENDER_SOLENOID_CHANNEL); 16 | private Solenoid grabberSolenoid = new Solenoid(RobotMap.HATCH_GRABBER_SOLENOID_MODULE, 17 | RobotMap.HATCH_GRABBER_SOLENOID_CHANNEL); 18 | private Solenoid placerSolenoid = new Solenoid(RobotMap.HATCH_PLACER_SOLENOID_MODULE, 19 | RobotMap.HATCH_PLACER_SOLENOID_CHANNEL); 20 | 21 | private DigitalInput leftLimitSwitch = new DigitalInput(RobotMap.HATCH_GRABBER_LIMIT_SWITCH_LEFT); 22 | private DigitalInput rightLimitSwitch = new DigitalInput(RobotMap.HATCH_GRABBER_LIMIT_SWITCH_RIGHT); 23 | 24 | private final Object canLock = new Object(); 25 | private boolean extendedChanged = true; 26 | private boolean extended = false; 27 | private boolean releasedChanged = true; 28 | private boolean released = false; 29 | private boolean placingChanged = true; 30 | private boolean placing = false; 31 | 32 | private Notifier canUpdateThread = new Notifier(() -> { 33 | boolean localExtended; 34 | boolean localExtendedChanged; 35 | boolean localGrabbing; 36 | boolean localGrabbingChanged; 37 | boolean localPlacing; 38 | boolean localPlacingChanged; 39 | synchronized (canLock) { 40 | localExtended = extended; 41 | localExtendedChanged = extendedChanged; 42 | extendedChanged = false; 43 | 44 | localGrabbing = released; 45 | localGrabbingChanged = releasedChanged; 46 | releasedChanged = false; 47 | 48 | localPlacing = placing; 49 | localPlacingChanged = placingChanged; 50 | placingChanged = false; 51 | } 52 | 53 | if (localExtendedChanged) { 54 | extenderSolenoid.set(localExtended); 55 | } 56 | 57 | if (localGrabbingChanged) { 58 | grabberSolenoid.set(localGrabbing); 59 | } 60 | 61 | if (localPlacingChanged) { 62 | placerSolenoid.set(localPlacing); 63 | } 64 | }); 65 | 66 | 67 | private HatchPlacerSubsystem() { 68 | canUpdateThread.startPeriodic(1.0 / CAN_UPDATE_RATE); 69 | } 70 | 71 | public static HatchPlacerSubsystem getInstance() { 72 | return instance; 73 | } 74 | 75 | public void extend() { 76 | synchronized (canLock){ 77 | extendedChanged = !extended; 78 | extended = true; 79 | } 80 | } 81 | 82 | public void retract() { 83 | synchronized (canLock) { 84 | extendedChanged = extended; 85 | extended = false; 86 | } 87 | } 88 | 89 | public void grab() { 90 | synchronized (canLock) { 91 | releasedChanged = released; 92 | released = false; 93 | } 94 | } 95 | 96 | public void release() { 97 | synchronized (canLock) { 98 | releasedChanged = !released; 99 | released = true; 100 | } 101 | } 102 | 103 | public void retractPlacer() { 104 | synchronized (canLock) { 105 | placingChanged = placing; 106 | placing = false; 107 | } 108 | } 109 | 110 | public void extendPlacer() { 111 | synchronized (canLock) { 112 | placingChanged = !placing; 113 | placing = true; 114 | } 115 | } 116 | 117 | public boolean getRightLimitSwitch() { 118 | return !rightLimitSwitch.get(); 119 | } 120 | 121 | public boolean getLeftLimitSwitch() { 122 | return !leftLimitSwitch.get(); 123 | } 124 | 125 | public boolean hasHatch() { 126 | return getLeftLimitSwitch() || getRightLimitSwitch(); 127 | } 128 | 129 | @Override 130 | public void outputToSmartDashboard() { } 131 | 132 | @Override 133 | public void stop() { } 134 | 135 | @Override 136 | public void zeroSensors() { } 137 | 138 | @Override 139 | protected void initDefaultCommand() { } 140 | 141 | public boolean isReleased() { 142 | synchronized (canLock) { 143 | return released; 144 | } 145 | } 146 | } 147 | -------------------------------------------------------------------------------- /vision-coprocessor/src/main/java/org/frcteam2910/c2019/vision/drivers/Limelight.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision.drivers; 2 | 3 | import edu.wpi.first.networktables.NetworkTable; 4 | import edu.wpi.first.networktables.NetworkTableEntry; 5 | import org.frcteam2910.common.math.MathUtils; 6 | import org.frcteam2910.common.math.Vector2; 7 | 8 | public final class Limelight { 9 | private final NetworkTable table; 10 | 11 | private final NetworkTableEntry tv; 12 | private final NetworkTableEntry tx; 13 | private final NetworkTableEntry ty; 14 | private final NetworkTableEntry ta; 15 | private final NetworkTableEntry ts; 16 | private final NetworkTableEntry tl; 17 | 18 | private final NetworkTableEntry tcornx; 19 | private final NetworkTableEntry tcorny; 20 | private final NetworkTableEntry camtran; 21 | 22 | private final NetworkTableEntry ledMode; 23 | private final NetworkTableEntry camMode; 24 | private final NetworkTableEntry pipeline; 25 | private final NetworkTableEntry stream; 26 | private final NetworkTableEntry snapshot; 27 | 28 | public Limelight(NetworkTable table) { 29 | this.table = table; 30 | 31 | tv = table.getEntry("tv"); 32 | tx = table.getEntry("tx"); 33 | ty = table.getEntry("ty"); 34 | ta = table.getEntry("ta"); 35 | ts = table.getEntry("ts"); 36 | tl = table.getEntry("tl"); 37 | 38 | tcornx = table.getEntry("tcornx"); 39 | tcorny = table.getEntry("tcorny"); 40 | camtran = table.getEntry("camtran"); 41 | 42 | ledMode = table.getEntry("ledMode"); 43 | camMode = table.getEntry("camMode"); 44 | pipeline = table.getEntry("pipeline"); 45 | stream = table.getEntry("stream"); 46 | snapshot = table.getEntry("snapshot"); 47 | } 48 | 49 | public boolean hasTarget() { 50 | return MathUtils.epsilonEquals(tv.getDouble(0), 1); 51 | } 52 | 53 | public double getTargetArea() { 54 | return ta.getDouble(0); 55 | } 56 | 57 | public Vector2 getTargetPosition() { 58 | return new Vector2(Math.toRadians(tx.getDouble(0)), Math.toRadians(ty.getDouble(0))); 59 | } 60 | 61 | public double getTargetSkew() { 62 | return ts.getDouble(0); 63 | } 64 | 65 | public boolean getCorners(int numOfCorners, double[][] corners) { 66 | double[] x = tcornx.getDoubleArray(new double[]{0.0, 0.0}); 67 | double[] y = tcorny.getDoubleArray(new double[]{0.0, 0.0}); 68 | if (x.length != numOfCorners) { 69 | return false; 70 | } 71 | for (int i = 0; i < x.length; i++) { 72 | corners[i][0] = x[i]; 73 | corners[i][1] = y[i]; 74 | } 75 | return true; 76 | } 77 | 78 | public Number[] getPosition() { 79 | Number[] defaultValue = {0, 0}; 80 | return camtran.getNumberArray(defaultValue); 81 | } 82 | 83 | public void setCamMode(CamMode mode) { 84 | switch (mode) { 85 | case VISION: 86 | camMode.setNumber(0); 87 | break; 88 | case DRIVER: 89 | camMode.setNumber(1); 90 | } 91 | } 92 | 93 | public void setLedMode(LedMode mode) { 94 | switch (mode) { 95 | case DEFAULT: 96 | ledMode.setNumber(0); 97 | break; 98 | case OFF: 99 | ledMode.setNumber(1); 100 | break; 101 | case BLINK: 102 | ledMode.setNumber(2); 103 | break; 104 | case ON: 105 | ledMode.setNumber(3); 106 | break; 107 | } 108 | } 109 | 110 | public void setSnapshotsEnabled(boolean enabled) { 111 | if (enabled) { 112 | snapshot.setNumber(1); 113 | } else { 114 | snapshot.setNumber(0); 115 | } 116 | } 117 | 118 | public void setPipeline(int pipeline) { 119 | this.pipeline.setNumber(pipeline); 120 | } 121 | 122 | public void setStreamMode(StreamMode mode) { 123 | switch (mode) { 124 | case STANDARD: 125 | stream.setNumber(0); 126 | break; 127 | case PIP_MAIN: 128 | stream.setNumber(1); 129 | break; 130 | case PIP_SECONDARY: 131 | stream.setNumber(2); 132 | break; 133 | } 134 | } 135 | 136 | public enum CamMode { 137 | VISION, 138 | DRIVER 139 | } 140 | 141 | public enum LedMode { 142 | DEFAULT, 143 | ON, 144 | OFF, 145 | BLINK 146 | } 147 | 148 | public enum StreamMode { 149 | STANDARD, 150 | PIP_MAIN, 151 | PIP_SECONDARY 152 | } 153 | } 154 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/subsystems/CargoGrabberSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.subsystems; 2 | 3 | import com.ctre.phoenix.motorcontrol.ControlMode; 4 | import com.ctre.phoenix.motorcontrol.can.TalonSRX; 5 | import com.revrobotics.CANSparkMax; 6 | import com.revrobotics.CANSparkMaxLowLevel; 7 | import edu.wpi.first.wpilibj.Notifier; 8 | import edu.wpi.first.wpilibj.Timer; 9 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 10 | import org.frcteam2910.c2019.commands.CargoGrabberDefaultCommand; 11 | import org.frcteam2910.common.robot.subsystems.Subsystem; 12 | import org.frcteam2910.c2019.RobotMap; 13 | 14 | public class CargoGrabberSubsystem extends Subsystem { 15 | private static final CargoGrabberSubsystem instance = new CargoGrabberSubsystem(); 16 | 17 | private static final double CAN_THREAD_UPDATE_DURATION = 0.02; // 50Hz 18 | 19 | private final CANSparkMax topMotor = new CANSparkMax(RobotMap.GRABBER_TOP_MOTOR, CANSparkMaxLowLevel.MotorType.kBrushless); 20 | private final TalonSRX bottomMotor = new TalonSRX(RobotMap.GRABBER_BOTTOM_MOTOR); 21 | 22 | private double bottomMotorSpeed = 0; 23 | private double topMotorSpeed = 0; 24 | 25 | private final Object canLock = new Object(); 26 | private boolean leftCargoDetected = false; 27 | private double leftCargoDetectionTime = 0.0; 28 | private boolean rightCargoDetected = false; 29 | private double rightCargoDetectionTime = 0.0; 30 | private double topCurrent = 0.0; 31 | 32 | private final Notifier canThread = new Notifier(() -> { 33 | double localTopCurrent = topMotor.getOutputCurrent(); 34 | 35 | double bottomSpeed; 36 | double topSpeed; 37 | synchronized (canLock) { 38 | bottomSpeed = bottomMotorSpeed; 39 | topSpeed = topMotorSpeed; 40 | topCurrent = localTopCurrent; 41 | } 42 | bottomMotor.set(ControlMode.PercentOutput, bottomSpeed); 43 | topMotor.set(topSpeed); 44 | 45 | synchronized (canLock) { 46 | // Beam break sensor is normally closed 47 | leftCargoDetected = bottomMotor.getSensorCollection().isFwdLimitSwitchClosed(); 48 | if (leftCargoDetected) { 49 | leftCargoDetectionTime = Timer.getFPGATimestamp(); 50 | } 51 | 52 | rightCargoDetected = bottomMotor.getSensorCollection().isRevLimitSwitchClosed(); 53 | if (rightCargoDetected) { 54 | rightCargoDetectionTime = Timer.getFPGATimestamp(); 55 | } 56 | } 57 | }); 58 | 59 | private CargoGrabberSubsystem() { 60 | topMotor.setSmartCurrentLimit(40); 61 | topMotor.setInverted(true); 62 | 63 | bottomMotor.configFactoryDefault(); 64 | bottomMotor.configForwardSoftLimitEnable(false); 65 | bottomMotor.configReverseSoftLimitEnable(false); 66 | bottomMotor.overrideLimitSwitchesEnable(false); 67 | bottomMotor.setInverted(true); 68 | 69 | canThread.startPeriodic(CAN_THREAD_UPDATE_DURATION); 70 | } 71 | 72 | public static CargoGrabberSubsystem getInstance() { 73 | return instance; 74 | } 75 | 76 | @Override 77 | public void outputToSmartDashboard() { 78 | SmartDashboard.putBoolean("Cargo Detected", hasCargo()); 79 | double localTopCurrent; 80 | synchronized (canLock) { 81 | localTopCurrent = topCurrent; 82 | } 83 | SmartDashboard.putNumber("Top Current", localTopCurrent); 84 | } 85 | 86 | @Override 87 | public void stop() { 88 | setIntakeSpeed(0); 89 | } 90 | 91 | public void setIntakeSpeed(double speed) { 92 | setTopIntakeSpeed(speed); 93 | setBottomIntakeSpeed(speed); 94 | } 95 | 96 | public void setTopIntakeSpeed(double speed) { 97 | synchronized (canLock) { 98 | topMotorSpeed = speed; 99 | } 100 | } 101 | 102 | public void setBottomIntakeSpeed(double speed) { 103 | synchronized (canLock) { 104 | bottomMotorSpeed = speed; 105 | } 106 | } 107 | 108 | public boolean hasCargo() { 109 | return hasLeftCargo() && hasRightCargo(); 110 | } 111 | 112 | public boolean hasLeftCargo() { 113 | synchronized (canLock) { 114 | return leftCargoDetected || (Timer.getFPGATimestamp() - leftCargoDetectionTime) < 0.25; 115 | } 116 | } 117 | 118 | public boolean hasRightCargo() { 119 | synchronized (canLock) { 120 | return rightCargoDetected || (Timer.getFPGATimestamp() - rightCargoDetectionTime) < 0.25; 121 | } 122 | } 123 | 124 | @Override 125 | public void zeroSensors() { 126 | 127 | } 128 | 129 | @Override 130 | protected void initDefaultCommand() { 131 | setDefaultCommand(new CargoGrabberDefaultCommand()); 132 | } 133 | } 134 | -------------------------------------------------------------------------------- /vision-coprocessor/src/main/java/org/frcteam2910/c2019/vision/ControlHandler.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision; 2 | 3 | import edu.wpi.first.networktables.NetworkTable; 4 | import edu.wpi.first.networktables.NetworkTableInstance; 5 | import org.frcteam2910.c2019.vision.api.*; 6 | import org.frcteam2910.c2019.vision.drivers.Limelight; 7 | import org.frcteam2910.common.Logger; 8 | import org.frcteam2910.common.control.*; 9 | import org.frcteam2910.common.math.RigidTransform2; 10 | import org.frcteam2910.common.math.Rotation2; 11 | import org.frcteam2910.common.math.Vector2; 12 | 13 | import java.io.IOException; 14 | import java.util.HashMap; 15 | import java.util.Map; 16 | 17 | public class ControlHandler implements Runnable { 18 | private static final Logger LOGGER = new Logger(ControlHandler.class); 19 | 20 | private static final String CARGO_LIMELIGHT_TABLE_NAME = "limelight-cargo"; 21 | private static final String HATCH_LIMELIGHT_TABLE_NAME = "limelight-hatch"; 22 | 23 | private static final ITrajectoryConstraint[] TRAJECTORY_CONSTRAINTS = { 24 | new MaxVelocityConstraint(12.0 * 12.0), 25 | new MaxAccelerationConstraint(5.0 * 12.0), 26 | new FeedforwardConstraint(0.8, 1.0 / (14.0 * 12.0), 1.0 / (5.0 * 12.0), 0.1) 27 | }; 28 | 29 | private VisionServer server; 30 | 31 | private Map limelights = new HashMap<>(); 32 | private Map pipelines = new HashMap<>(); 33 | 34 | public ControlHandler() throws IOException { 35 | this.server = new VisionServer<>(5800); 36 | 37 | NetworkTableInstance instance = NetworkTableInstance.getDefault(); 38 | NetworkTable cargoTable = instance.getTable(CARGO_LIMELIGHT_TABLE_NAME); 39 | NetworkTable hatchTable = instance.getTable(HATCH_LIMELIGHT_TABLE_NAME); 40 | 41 | limelights.put(Gamepiece.CARGO, new Limelight(cargoTable)); 42 | limelights.put(Gamepiece.HATCH_PANEL, new Limelight(hatchTable)); 43 | 44 | pipelines.put(Gamepiece.CARGO, new VisionTargetingPipeline2019(limelights.get(Gamepiece.CARGO))); 45 | pipelines.put(Gamepiece.HATCH_PANEL, new VisionTargetingPipeline2019(limelights.get(Gamepiece.HATCH_PANEL))); 46 | } 47 | 48 | @Override 49 | public void run() { 50 | while (!Thread.interrupted() && server.getSocket().isBound()) { 51 | try (VisionClient connection = server.accept()) { 52 | while (!Thread.interrupted() && connection.getSocket().isConnected()) { 53 | Object object = connection.receivePacket(); 54 | 55 | if (object instanceof ConfigCoprocessorPacket) { 56 | // TODO: Config coprocessor 57 | ConfigCoprocessorPacket packet = (ConfigCoprocessorPacket) object; 58 | LOGGER.info("Setting %s to %s", packet.getGamepiece(), packet.isDriverMode()); 59 | } else if (object instanceof TrajectoryRequestPacket) { 60 | // TODO: Generate trajectory 61 | TrajectoryRequestPacket packet = (TrajectoryRequestPacket) object; 62 | 63 | VisionTargetingPipeline pipeline = pipelines.get(packet.getGamepiece()); 64 | 65 | try { 66 | RigidTransform2 relativePosition = pipeline.getTranslation(); 67 | 68 | RobotStateEstimator.State lastState = RobotStateEstimator.estimateState(packet.getTimestamp()); 69 | 70 | RigidTransform2 goalPose = lastState.getPose().transformBy(relativePosition); 71 | goalPose = goalPose.transformBy(new RigidTransform2(new Vector2(-12.0, 0.0), Rotation2.ZERO)); 72 | 73 | System.out.printf("%s -> %s%n", lastState.getPose(), goalPose); 74 | 75 | SplinePathGenerator generator = new SplinePathGenerator(); 76 | generator.setFitCheckEpsilon(1e-1); 77 | Path path = generator.generate( 78 | new Waypoint(lastState.getPose().translation, lastState.getVelocity().getAngle(), 79 | lastState.getPose().rotation), 80 | new Waypoint(goalPose.translation, goalPose.rotation, goalPose.rotation) 81 | ); 82 | Trajectory trajectory = new Trajectory(path, TRAJECTORY_CONSTRAINTS); 83 | 84 | connection.sendPacket(trajectory); 85 | } catch (Exception e) { 86 | // Didn't get a good result 87 | connection.sendPacket(e); 88 | LOGGER.error(e.getMessage()); 89 | } 90 | } else { 91 | LOGGER.warn("Got packet with unknown type: %s", object.getClass().getName()); 92 | } 93 | } 94 | } catch (IOException e) { 95 | LOGGER.error(e); 96 | } 97 | } 98 | } 99 | } 100 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/DoTheThingCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.command.*; 4 | import org.frcteam2910.c2019.subsystems.CargoArmSubsystem; 5 | import org.frcteam2910.c2019.subsystems.HatchPlacerSubsystem; 6 | import org.frcteam2910.common.control.ITrajectoryConstraint; 7 | import org.frcteam2910.common.control.MaxAccelerationConstraint; 8 | import org.frcteam2910.common.control.MaxVelocityConstraint; 9 | import org.frcteam2910.common.math.Rotation2; 10 | import org.frcteam2910.common.math.Vector2; 11 | 12 | public class DoTheThingCommand extends CommandGroup { 13 | private static final double RELEASE_DISTANCE = 21.5; 14 | private static final double RELEASE_HORIZ_DISTANCE = 1.25; 15 | 16 | public DoTheThingCommand(boolean placeOnRocket, boolean backupAfterPlace) { 17 | this.setRunWhenDisabled(true); 18 | // Setup 19 | addSequential(new ExtendHatchPlacerCommand()); 20 | Command setupHatchMechanism = new ConditionalCommand((Command) null, new ReleaseHatchPanelCommand()) { 21 | @Override 22 | protected boolean condition() { 23 | return HatchPlacerSubsystem.getInstance().hasHatch(); 24 | } 25 | }; 26 | setupHatchMechanism.setRunWhenDisabled(true); 27 | addSequential(setupHatchMechanism); 28 | addSequential(new SetArmAngleCommand(CargoArmSubsystem.VISION_TARGET_ANGLE)); 29 | 30 | CommandGroup placeGroup = new CommandGroup(); 31 | placeGroup.setRunWhenDisabled(true); 32 | placeGroup.addSequential(new ImprovedVisionPlaceCommand( 33 | (distanceFromTarget, horizDistance) -> distanceFromTarget < RELEASE_DISTANCE && Math.abs(horizDistance) < RELEASE_HORIZ_DISTANCE, 34 | placeOnRocket ? DoTheThingCommand::chooseRocketTarget : DoTheThingCommand::chooseCargoShipTarget, 35 | new ITrajectoryConstraint[]{ 36 | new MaxVelocityConstraint(12.0 * 12.0), 37 | new MaxAccelerationConstraint(7.5 * 12.0) 38 | } 39 | )); 40 | placeGroup.addSequential(new InstantCommand(() -> HatchPlacerSubsystem.getInstance().extendPlacer())); // TODO 41 | placeGroup.addSequential(new ReleaseHatchPanelCommand()); 42 | placeGroup.addSequential(new WaitCommand(0.2)); // TODO 43 | placeGroup.addSequential(new RetractHatchPlacerCommand()); 44 | placeGroup.addSequential(new InstantCommand(() -> HatchPlacerSubsystem.getInstance().retractPlacer())); // TODO 45 | if (backupAfterPlace) { 46 | placeGroup.addSequential(new DriveCommand(new Vector2(0.5, 0.0), 0.0, false), 0.25); 47 | } 48 | 49 | CommandGroup pickupGroup = new CommandGroup(); 50 | pickupGroup.setRunWhenDisabled(true); 51 | pickupGroup.addSequential(new ExtendHatchPlacerCommand()); 52 | pickupGroup.addSequential(new ReleaseHatchPanelCommand()); 53 | pickupGroup.addSequential(new ImprovedVisionPlaceCommand( 54 | (a, b) -> HatchPlacerSubsystem.getInstance().hasHatch(), 55 | placeOnRocket ? DoTheThingCommand::chooseRocketTarget : DoTheThingCommand::chooseCargoShipTarget, 56 | new ITrajectoryConstraint[]{ 57 | new MaxVelocityConstraint(12.0 * 12.0), 58 | new MaxAccelerationConstraint(11.0 * 12.0) 59 | } 60 | )); 61 | pickupGroup.addSequential(new GrabHatchPanelCommand()); 62 | pickupGroup.addSequential(new WaitCommand(0.25)); // TODO 63 | pickupGroup.addSequential(new RetractHatchPlacerCommand()); 64 | 65 | // Target 66 | Command cleanupHatchMechanism = new ConditionalCommand(placeGroup, pickupGroup) { 67 | @Override 68 | protected boolean condition() { 69 | return HatchPlacerSubsystem.getInstance().hasHatch(); 70 | } 71 | }; 72 | addSequential(cleanupHatchMechanism); 73 | } 74 | 75 | private static Rotation2 chooseCargoShipTarget(Rotation2 currentAngle) { 76 | if (!HatchPlacerSubsystem.getInstance().hasHatch()) { 77 | return Rotation2.ZERO; 78 | } 79 | 80 | double angle = currentAngle.toRadians(); 81 | if (angle < Math.toRadians(135.0)) { 82 | return Rotation2.fromDegrees(90.0); 83 | } else if (angle < Math.toRadians(225.0)) { 84 | return Rotation2.fromDegrees(180.0); 85 | } else { 86 | return Rotation2.fromDegrees(270.0); 87 | } 88 | } 89 | 90 | private static Rotation2 chooseRocketTarget(Rotation2 currentAngle) { 91 | if (!HatchPlacerSubsystem.getInstance().hasHatch()) { 92 | return Rotation2.ZERO; 93 | } 94 | 95 | double angle = currentAngle.toRadians(); 96 | if (angle < Math.toRadians(90.0)) { 97 | return Rotation2.fromDegrees(32.0); 98 | } else if (angle < Math.toRadians(180.0)) { 99 | return Rotation2.fromDegrees(148.0); 100 | } else if (angle < Math.toRadians(270.0)) { 101 | return Rotation2.fromDegrees(212.0); 102 | } else { 103 | return Rotation2.fromDegrees(328.0); 104 | } 105 | } 106 | } 107 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/drivers/Mk2SwerveModule.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.drivers; 2 | 3 | import com.revrobotics.CANEncoder; 4 | import com.revrobotics.CANSparkMax; 5 | import com.revrobotics.CANSparkMaxLowLevel; 6 | import edu.wpi.first.wpilibj.AnalogInput; 7 | import edu.wpi.first.wpilibj.Notifier; 8 | import edu.wpi.first.wpilibj.RobotController; 9 | import edu.wpi.first.wpilibj.Spark; 10 | import org.frcteam2910.common.control.PidConstants; 11 | import org.frcteam2910.common.control.PidController; 12 | import org.frcteam2910.common.drivers.SwerveModule; 13 | import org.frcteam2910.common.math.Vector2; 14 | 15 | import java.util.concurrent.atomic.AtomicLong; 16 | 17 | public class Mk2SwerveModule extends SwerveModule { 18 | private static final PidConstants ANGLE_CONSTANTS = new PidConstants(0.5, 0.0, 0.0001); 19 | private static final double DRIVE_TICKS_PER_INCH = 1.0 / (4.0 * Math.PI / 60.0 * 15.0 / 20.0 * 24.0 / 38.0 * 18.0); // 0.707947 20 | 21 | private static final double CAN_UPDATE_RATE = 200.0; 22 | 23 | private final double angleOffset; 24 | 25 | private Spark angleMotor; 26 | private AnalogInput angleEncoder; 27 | private CANSparkMax driveMotor; 28 | private CANEncoder driveEncoder; 29 | 30 | private final Object canLock = new Object(); 31 | private double driveEncoderTicks = 0.0; 32 | private double drivePercentOutput = 0.0; 33 | private double driveVelocityRpm = 0.0; 34 | private double driveCurrent = 0.0; 35 | 36 | private Notifier canUpdateNotifier = new Notifier(() -> { 37 | double driveEncoderTicks = driveEncoder.getPosition(); 38 | synchronized (canLock) { 39 | Mk2SwerveModule.this.driveEncoderTicks = driveEncoderTicks; 40 | } 41 | 42 | double driveVelocityRpm = driveEncoder.getVelocity(); 43 | synchronized (canLock) { 44 | Mk2SwerveModule.this.driveVelocityRpm = driveVelocityRpm; 45 | } 46 | 47 | double localDriveCurrent = driveMotor.getOutputCurrent(); 48 | synchronized (canLock) { 49 | driveCurrent = localDriveCurrent; 50 | } 51 | 52 | double drivePercentOutput; 53 | synchronized (canLock) { 54 | drivePercentOutput = Mk2SwerveModule.this.drivePercentOutput; 55 | } 56 | driveMotor.set(drivePercentOutput); 57 | }); 58 | 59 | private PidController angleController = new PidController(ANGLE_CONSTANTS); 60 | 61 | public Mk2SwerveModule(Vector2 modulePosition, double angleOffset, 62 | Spark angleMotor, CANSparkMax driveMotor, AnalogInput angleEncoder) { 63 | super(modulePosition); 64 | this.angleOffset = angleOffset; 65 | this.angleMotor = angleMotor; 66 | this.angleEncoder = angleEncoder; 67 | this.driveMotor = driveMotor; 68 | this.driveEncoder = new CANEncoder(driveMotor); 69 | 70 | driveMotor.setSmartCurrentLimit(60); 71 | driveMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 500); 72 | driveMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, 3); 73 | 74 | angleController.setInputRange(0.0, 2.0 * Math.PI); 75 | angleController.setContinuous(true); 76 | angleController.setOutputRange(-0.5, 0.5); 77 | 78 | canUpdateNotifier.startPeriodic(1.0 / CAN_UPDATE_RATE); 79 | } 80 | 81 | @Override 82 | protected double readAngle() { 83 | double angle = (1.0 - angleEncoder.getVoltage() / RobotController.getVoltage5V()) * 2.0 * Math.PI + angleOffset; 84 | angle %= 2.0 * Math.PI; 85 | if (angle < 0.0) { 86 | angle += 2.0 * Math.PI; 87 | } 88 | 89 | return angle; 90 | } 91 | 92 | @Override 93 | protected double readDistance() { 94 | double driveEncoderTicks; 95 | synchronized (canLock) { 96 | driveEncoderTicks = this.driveEncoderTicks; 97 | } 98 | 99 | return driveEncoderTicks / DRIVE_TICKS_PER_INCH; 100 | } 101 | 102 | protected double readVelocity() { 103 | double driveVelocityRpm; 104 | synchronized (canLock) { 105 | driveVelocityRpm = this.driveVelocityRpm; 106 | } 107 | 108 | return driveVelocityRpm * (1.0 / 60.0) / DRIVE_TICKS_PER_INCH; 109 | } 110 | 111 | protected double readDriveCurrent() { 112 | double localDriveCurrent; 113 | synchronized (canLock) { 114 | localDriveCurrent = driveCurrent; 115 | } 116 | 117 | return localDriveCurrent; 118 | } 119 | 120 | @Override 121 | public double getCurrentVelocity() { 122 | return readVelocity(); 123 | } 124 | 125 | @Override 126 | public double getDriveCurrent() { 127 | return readDriveCurrent(); 128 | } 129 | 130 | @Override 131 | protected void setTargetAngle(double angle) { 132 | angleController.setSetpoint(angle); 133 | } 134 | 135 | @Override 136 | protected void setDriveOutput(double output) { 137 | synchronized (canLock) { 138 | this.drivePercentOutput = output; 139 | } 140 | } 141 | 142 | @Override 143 | public void updateState(double dt) { 144 | super.updateState(dt); 145 | 146 | angleMotor.set(angleController.calculate(getCurrentAngle(), dt)); 147 | } 148 | } 149 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/CargoGrabberDefaultCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import edu.wpi.first.wpilibj.command.Command; 5 | import org.frcteam2910.c2019.Robot; 6 | import org.frcteam2910.c2019.subsystems.CargoArmSubsystem; 7 | import org.frcteam2910.c2019.subsystems.CargoGrabberSubsystem; 8 | import org.frcteam2910.c2019.subsystems.Superstructure; 9 | import org.frcteam2910.common.math.MathUtils; 10 | 11 | public class CargoGrabberDefaultCommand extends Command { 12 | private static final double EJECT_SPEED_TOP = -1.0; 13 | private static final double EJECT_SPEED_BOTTOM = 0.0; 14 | 15 | private static final double EJECT_FRONT_CARGO_SHIP_SPEED_TOP = -0.6; 16 | private static final double EJECT_FRONT_CARGO_SHIP_SPEED_BOTTOM = 0.0; 17 | 18 | private static final double EJECT_ROCKET_SPEED_TOP = -0.2; 19 | private static final double EJECT_ROCKET_SPEED_BOTTOM = 1.0; 20 | 21 | private static final double INTAKE_AFTER_EJECT_TIME = 2.5; 22 | 23 | private static final double INTAKE_AFTER_EJECT_SPEED_TOP = 0.7; 24 | private static final double INTAKE_AFTER_EJECT_SPEED_BOTTOM = 0.0; 25 | 26 | private static final double INTAKE_SPEED_TOP = 0.7; 27 | private static final double INTAKE_SPEED_BOTTOM = 0.5; 28 | 29 | private static final double CENTERING_INTAKE_SPEED_TOP = 0.6; 30 | private static final double CENTERING_INTAKE_SPEED_BOTTOM = 0.5; 31 | 32 | private static final double SOFT_INTAKE_SPEED_TOP = 0.06; 33 | private static final double SOFT_INTAKE_SPEED_BOTTOM = 0.0; 34 | 35 | private final Timer timer = new Timer(); 36 | 37 | public CargoGrabberDefaultCommand() { 38 | requires(CargoGrabberSubsystem.getInstance()); 39 | } 40 | 41 | @Override 42 | protected void initialize() { 43 | timer.start(); 44 | } 45 | 46 | @Override 47 | protected void execute() { 48 | if (Robot.getOi().primaryController.getLeftTriggerAxis().get() > 0.1) { 49 | double angle = Superstructure.getInstance().getGyroscope().getAngle().toRadians(); 50 | 51 | if (MathUtils.epsilonEquals(CargoArmSubsystem.getInstance().getCurrentAngle(), 52 | CargoArmSubsystem.ROCKET_SCORE_ANGLE, Math.toRadians(5.0))) { 53 | // Placing in the rocket uses different speeds because sometimes when we eject cargo it rides up the 54 | // side of the rocket. 55 | CargoGrabberSubsystem.getInstance().setTopIntakeSpeed(EJECT_ROCKET_SPEED_TOP); 56 | CargoGrabberSubsystem.getInstance().setBottomIntakeSpeed(EJECT_ROCKET_SPEED_BOTTOM); 57 | } else if (angle < Math.toRadians(30.0) || angle > Math.toRadians(360.0 - 30.0)) { 58 | // When we are scoring in the front of the cargo ship, the ejection speed is different so the cargo 59 | // doesn't launch as far. 60 | CargoGrabberSubsystem.getInstance().setTopIntakeSpeed(EJECT_FRONT_CARGO_SHIP_SPEED_TOP); 61 | CargoGrabberSubsystem.getInstance().setBottomIntakeSpeed(EJECT_FRONT_CARGO_SHIP_SPEED_BOTTOM); 62 | } else { 63 | CargoGrabberSubsystem.getInstance().setTopIntakeSpeed(EJECT_SPEED_TOP); 64 | CargoGrabberSubsystem.getInstance().setBottomIntakeSpeed(EJECT_SPEED_BOTTOM); 65 | } 66 | 67 | timer.reset(); 68 | } else if (CargoArmSubsystem.getInstance().isWithinTargetAngleRange(CargoArmSubsystem.BOTTOM_ANGLE)) { 69 | CargoGrabberSubsystem.getInstance().setTopIntakeSpeed(INTAKE_SPEED_TOP); 70 | CargoGrabberSubsystem.getInstance().setBottomIntakeSpeed(INTAKE_SPEED_BOTTOM); 71 | } else if (CargoGrabberSubsystem.getInstance().hasLeftCargo() || 72 | CargoGrabberSubsystem.getInstance().hasRightCargo()) { 73 | if (!CargoArmSubsystem.getInstance().isWithinTargetAngleRange(CargoArmSubsystem.getInstance().getTargetAngle())) { 74 | CargoGrabberSubsystem.getInstance().setTopIntakeSpeed(INTAKE_SPEED_TOP); 75 | CargoGrabberSubsystem.getInstance().setBottomIntakeSpeed(INTAKE_SPEED_BOTTOM); 76 | } else if (CargoGrabberSubsystem.getInstance().hasCargo()) { 77 | CargoGrabberSubsystem.getInstance().setTopIntakeSpeed(SOFT_INTAKE_SPEED_TOP); 78 | CargoGrabberSubsystem.getInstance().setBottomIntakeSpeed(SOFT_INTAKE_SPEED_BOTTOM); 79 | } else { 80 | CargoGrabberSubsystem.getInstance().setTopIntakeSpeed(CENTERING_INTAKE_SPEED_TOP); 81 | CargoGrabberSubsystem.getInstance().setBottomIntakeSpeed(CENTERING_INTAKE_SPEED_BOTTOM); 82 | } 83 | } else if (Robot.getOi().secondaryController.getLeftTriggerAxis().get() > 0.1) { 84 | CargoGrabberSubsystem.getInstance().setTopIntakeSpeed(INTAKE_SPEED_TOP); 85 | CargoGrabberSubsystem.getInstance().setBottomIntakeSpeed(INTAKE_SPEED_BOTTOM); 86 | } else if (timer.get() < INTAKE_AFTER_EJECT_TIME) { 87 | CargoGrabberSubsystem.getInstance().setTopIntakeSpeed(INTAKE_AFTER_EJECT_SPEED_TOP); 88 | CargoGrabberSubsystem.getInstance().setBottomIntakeSpeed(INTAKE_AFTER_EJECT_SPEED_BOTTOM); 89 | } else { 90 | CargoGrabberSubsystem.getInstance().setIntakeSpeed(0.0); 91 | } 92 | } 93 | 94 | @Override 95 | protected void end() { 96 | timer.stop(); 97 | } 98 | 99 | @Override 100 | protected boolean isFinished() { 101 | return false; 102 | } 103 | } 104 | -------------------------------------------------------------------------------- /gradlew: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | 3 | ############################################################################## 4 | ## 5 | ## Gradle start up script for UN*X 6 | ## 7 | ############################################################################## 8 | 9 | # Attempt to set APP_HOME 10 | # Resolve links: $0 may be a link 11 | PRG="$0" 12 | # Need this for relative symlinks. 13 | while [ -h "$PRG" ] ; do 14 | ls=`ls -ld "$PRG"` 15 | link=`expr "$ls" : '.*-> \(.*\)$'` 16 | if expr "$link" : '/.*' > /dev/null; then 17 | PRG="$link" 18 | else 19 | PRG=`dirname "$PRG"`"/$link" 20 | fi 21 | done 22 | SAVED="`pwd`" 23 | cd "`dirname \"$PRG\"`/" >/dev/null 24 | APP_HOME="`pwd -P`" 25 | cd "$SAVED" >/dev/null 26 | 27 | APP_NAME="Gradle" 28 | APP_BASE_NAME=`basename "$0"` 29 | 30 | # Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 31 | DEFAULT_JVM_OPTS='"-Xmx64m"' 32 | 33 | # Use the maximum available, or set MAX_FD != -1 to use that value. 34 | MAX_FD="maximum" 35 | 36 | warn () { 37 | echo "$*" 38 | } 39 | 40 | die () { 41 | echo 42 | echo "$*" 43 | echo 44 | exit 1 45 | } 46 | 47 | # OS specific support (must be 'true' or 'false'). 48 | cygwin=false 49 | msys=false 50 | darwin=false 51 | nonstop=false 52 | case "`uname`" in 53 | CYGWIN* ) 54 | cygwin=true 55 | ;; 56 | Darwin* ) 57 | darwin=true 58 | ;; 59 | MINGW* ) 60 | msys=true 61 | ;; 62 | NONSTOP* ) 63 | nonstop=true 64 | ;; 65 | esac 66 | 67 | CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar 68 | 69 | # Determine the Java command to use to start the JVM. 70 | if [ -n "$JAVA_HOME" ] ; then 71 | if [ -x "$JAVA_HOME/jre/sh/java" ] ; then 72 | # IBM's JDK on AIX uses strange locations for the executables 73 | JAVACMD="$JAVA_HOME/jre/sh/java" 74 | else 75 | JAVACMD="$JAVA_HOME/bin/java" 76 | fi 77 | if [ ! -x "$JAVACMD" ] ; then 78 | die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME 79 | 80 | Please set the JAVA_HOME variable in your environment to match the 81 | location of your Java installation." 82 | fi 83 | else 84 | JAVACMD="java" 85 | which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 86 | 87 | Please set the JAVA_HOME variable in your environment to match the 88 | location of your Java installation." 89 | fi 90 | 91 | # Increase the maximum file descriptors if we can. 92 | if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then 93 | MAX_FD_LIMIT=`ulimit -H -n` 94 | if [ $? -eq 0 ] ; then 95 | if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then 96 | MAX_FD="$MAX_FD_LIMIT" 97 | fi 98 | ulimit -n $MAX_FD 99 | if [ $? -ne 0 ] ; then 100 | warn "Could not set maximum file descriptor limit: $MAX_FD" 101 | fi 102 | else 103 | warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" 104 | fi 105 | fi 106 | 107 | # For Darwin, add options to specify how the application appears in the dock 108 | if $darwin; then 109 | GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" 110 | fi 111 | 112 | # For Cygwin, switch paths to Windows format before running java 113 | if $cygwin ; then 114 | APP_HOME=`cygpath --path --mixed "$APP_HOME"` 115 | CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` 116 | JAVACMD=`cygpath --unix "$JAVACMD"` 117 | 118 | # We build the pattern for arguments to be converted via cygpath 119 | ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` 120 | SEP="" 121 | for dir in $ROOTDIRSRAW ; do 122 | ROOTDIRS="$ROOTDIRS$SEP$dir" 123 | SEP="|" 124 | done 125 | OURCYGPATTERN="(^($ROOTDIRS))" 126 | # Add a user-defined pattern to the cygpath arguments 127 | if [ "$GRADLE_CYGPATTERN" != "" ] ; then 128 | OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" 129 | fi 130 | # Now convert the arguments - kludge to limit ourselves to /bin/sh 131 | i=0 132 | for arg in "$@" ; do 133 | CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` 134 | CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option 135 | 136 | if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition 137 | eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` 138 | else 139 | eval `echo args$i`="\"$arg\"" 140 | fi 141 | i=$((i+1)) 142 | done 143 | case $i in 144 | (0) set -- ;; 145 | (1) set -- "$args0" ;; 146 | (2) set -- "$args0" "$args1" ;; 147 | (3) set -- "$args0" "$args1" "$args2" ;; 148 | (4) set -- "$args0" "$args1" "$args2" "$args3" ;; 149 | (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; 150 | (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; 151 | (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; 152 | (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; 153 | (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; 154 | esac 155 | fi 156 | 157 | # Escape application args 158 | save () { 159 | for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done 160 | echo " " 161 | } 162 | APP_ARGS=$(save "$@") 163 | 164 | # Collect all arguments for the java command, following the shell quoting and substitution rules 165 | eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" 166 | 167 | # by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong 168 | if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then 169 | cd "$(dirname "$0")" 170 | fi 171 | 172 | exec "$JAVACMD" "$@" 173 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/OI.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019; 2 | 3 | import edu.wpi.first.wpilibj.DriverStation; 4 | import edu.wpi.first.wpilibj.GenericHID; 5 | import edu.wpi.first.wpilibj.command.*; 6 | import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; 7 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 8 | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; 9 | import org.frcteam2910.c2019.autonomous.AutonomousSelector; 10 | import org.frcteam2910.c2019.commands.*; 11 | import org.frcteam2910.c2019.subsystems.*; 12 | import org.frcteam2910.common.robot.commands.ZeroFieldOrientedCommand; 13 | import org.frcteam2910.common.robot.input.DPadButton; 14 | import org.frcteam2910.common.robot.input.XboxController; 15 | 16 | public class OI { 17 | public final XboxController primaryController = new XboxController(0); 18 | 19 | public final XboxController secondaryController = new XboxController(1); 20 | 21 | public OI() { 22 | primaryController.getLeftXAxis().setInverted(true); 23 | primaryController.getRightXAxis().setInverted(true); 24 | 25 | primaryController.getRightXAxis().setScale(0.45); 26 | } 27 | 28 | public void bindButtons(AutonomousSelector autonomousSelector) { 29 | primaryController.getRightTriggerAxis().whenPressed(new ExtendHatchPlacerCommand()); 30 | { 31 | CommandGroup group = new CommandGroup(); 32 | group.addSequential(new ReleaseHatchPanelCommand()); 33 | group.addSequential(new InstantCommand(() -> HatchPlacerSubsystem.getInstance().extendPlacer())); 34 | group.addSequential(new WaitCommand(0.25)); 35 | group.addSequential(new RetractHatchPlacerCommand()); 36 | group.addSequential(new InstantCommand(() -> HatchPlacerSubsystem.getInstance().retractPlacer())); 37 | primaryController.getRightTriggerAxis().whenReleased(group); 38 | } 39 | 40 | primaryController.getAButton().whileHeld(new SetRobotPitchCommand(Math.toRadians(12.0))); 41 | 42 | { 43 | Command doTheThingCommand = new DoTheThingCommand(true, true); 44 | primaryController.getRightBumperButton().whenPressed(doTheThingCommand); 45 | primaryController.getRightBumperButton().whenReleased(new RetractHatchPlacerCommand()); 46 | primaryController.getRightBumperButton().whenReleased(new InstantCommand(doTheThingCommand::cancel)); 47 | primaryController.getRightBumperButton().whenReleased(new ConditionalCommand(new InstantCommand(() -> { 48 | autonomousSelector.getHybridQueue().remove().start(); 49 | })) { 50 | @Override 51 | protected boolean condition() { 52 | System.out.printf("Checking %d%n", autonomousSelector.getHybridQueue().size()); 53 | return DriverStation.getInstance().isAutonomous() && !autonomousSelector.getHybridQueue().isEmpty(); 54 | } 55 | }); 56 | } 57 | { 58 | Command doTheThingCommand = new DoTheThingCommand(false, true); 59 | primaryController.getLeftBumperButton().whenPressed(doTheThingCommand); 60 | primaryController.getLeftBumperButton().whenReleased(new RetractHatchPlacerCommand()); 61 | primaryController.getLeftBumperButton().whenReleased(new InstantCommand(doTheThingCommand::cancel)); 62 | primaryController.getLeftBumperButton().whenReleased(new ConditionalCommand(new InstantCommand(() -> { 63 | autonomousSelector.getHybridQueue().remove().start(); 64 | })) { 65 | @Override 66 | protected boolean condition() { 67 | System.out.printf("Checking %d%n", autonomousSelector.getHybridQueue().size()); 68 | return DriverStation.getInstance().isAutonomous() && !autonomousSelector.getHybridQueue().isEmpty(); 69 | } 70 | }); 71 | } 72 | 73 | primaryController.getDPadButton(DPadButton.Direction.UP).whenPressed( 74 | new SetHatchFloorGathererAngleCommand(HatchFloorGathererSubsystem.getInstance().getMaxAngle())); 75 | primaryController.getDPadButton(DPadButton.Direction.RIGHT).whenPressed(new SetHatchFloorGathererAngleCommand(Math.toRadians(90.0))); 76 | primaryController.getDPadButton(DPadButton.Direction.DOWN).whenPressed(new SetHatchFloorGathererAngleCommand(0.0)); 77 | primaryController.getDPadButton(DPadButton.Direction.LEFT).whenPressed(new RetractKickstandCommand()); 78 | 79 | // Field oriented zero 80 | primaryController.getBackButton().whenPressed(new ZeroFieldOrientedCommand(DrivetrainSubsystem.getInstance())); 81 | 82 | // Climbing 83 | primaryController.getStartButton().whenPressed(new InstantCommand(new Runnable() { 84 | private SendableChooser climbModeSendable = new SendableChooser<>(); 85 | 86 | { 87 | ShuffleboardTab climbTab = Shuffleboard.getTab("Climbing"); 88 | 89 | climbModeSendable.setName("Climb Mode"); 90 | climbModeSendable.setDefaultOption("Normal", 0); 91 | climbModeSendable.addOption("Overhang", 1); 92 | climbModeSendable.addOption("MadTown", 2); 93 | climbTab.add(climbModeSendable); 94 | } 95 | 96 | @Override 97 | public void run() { 98 | switch (climbModeSendable.getSelected()) { 99 | case 0: 100 | new ClimbCommand().start(); 101 | break; 102 | case 1: 103 | new OverhangCommand(Math.toRadians(80.0), false).start(); 104 | break; 105 | case 2: 106 | new OverhangCommand(CargoArmSubsystem.getInstance().getMaxAngle(), true).start(); 107 | break; 108 | } 109 | } 110 | })); 111 | primaryController.getYButton().whenPressed(new AbortClimbCommand()); 112 | 113 | 114 | // Cargo arm top position 115 | secondaryController.getDPadButton(DPadButton.Direction.UP).whenPressed( 116 | new SetArmAngleCommand(CargoArmSubsystem.CARGO_SHIP_SCORE_ANGLE)); 117 | // Cargo arm cargo ship height 118 | secondaryController.getDPadButton(DPadButton.Direction.LEFT).whenPressed( 119 | new SetArmAngleCommand(CargoArmSubsystem.CARGO_SHIP_SCORE_ANGLE)); 120 | // Cargo arm rocket & climb height 121 | secondaryController.getDPadButton(DPadButton.Direction.RIGHT).whenPressed( 122 | new SetArmAngleCommand(CargoArmSubsystem.ROCKET_SCORE_ANGLE)); 123 | // Cargo arm bottom position 124 | secondaryController.getDPadButton(DPadButton.Direction.DOWN).whenPressed(new SetArmAngleCommand(CargoArmSubsystem.BOTTOM_ANGLE)); 125 | 126 | { 127 | CommandGroup group = new CommandGroup(); 128 | group.addSequential(new ReleaseHatchPanelCommand()); 129 | group.addSequential(new ExtendHatchPlacerCommand()); 130 | secondaryController.getRightTriggerAxis().whenPressed(group); 131 | 132 | } 133 | 134 | { 135 | CommandGroup group = new CommandGroup(); 136 | group.addParallel(new GrabOnHasHatchPanelCommand()); 137 | group.addSequential(new RumbleWhileHasHatchPanelCommand()); 138 | secondaryController.getRightTriggerAxis().whileHeld(group); 139 | 140 | } 141 | { 142 | CommandGroup group = new CommandGroup(); 143 | group.addSequential(new GrabHatchPanelCommand()); 144 | group.addSequential(new WaitCommand(0.1)); 145 | group.addSequential(new RetractHatchPlacerCommand()); 146 | secondaryController.getRightTriggerAxis().whenReleased(group); 147 | } 148 | 149 | secondaryController.getRightBumperButton().whileHeld(new GrabHatchFromFloorPart1Command()); 150 | secondaryController.getRightBumperButton().whenReleased(new GrabHatchFromFloorPart2Command()); 151 | 152 | secondaryController.getLeftTriggerAxis().whileHeld(new Command() { 153 | @Override 154 | protected void execute() { 155 | if (CargoGrabberSubsystem.getInstance().hasLeftCargo() || CargoGrabberSubsystem.getInstance().hasRightCargo()) { 156 | secondaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kLeftRumble, 1.0); 157 | } else { 158 | secondaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kLeftRumble, 0.0); 159 | } 160 | } 161 | 162 | @Override 163 | protected void end() { 164 | secondaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kLeftRumble, 0.0); 165 | } 166 | 167 | @Override 168 | protected boolean isFinished() { 169 | return false; 170 | } 171 | }); 172 | 173 | secondaryController.getAButton().whenPressed(new SetHatchFloorGathererAngleCommand( 174 | HatchFloorGathererSubsystem.getInstance().getMaxAngle())); 175 | } 176 | } 177 | 178 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/commands/ImprovedVisionPlaceCommand.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.commands; 2 | 3 | import edu.wpi.first.wpilibj.GenericHID; 4 | import edu.wpi.first.wpilibj.command.Command; 5 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 6 | import org.frcteam2910.c2019.Robot; 7 | import org.frcteam2910.c2019.subsystems.DrivetrainSubsystem; 8 | import org.frcteam2910.c2019.subsystems.HatchPlacerSubsystem; 9 | import org.frcteam2910.c2019.subsystems.Superstructure; 10 | import org.frcteam2910.c2019.subsystems.VisionSubsystem; 11 | import org.frcteam2910.c2019.vision.api.Gamepiece; 12 | import org.frcteam2910.common.control.ITrajectoryConstraint; 13 | import org.frcteam2910.common.control.Path; 14 | import org.frcteam2910.common.control.PathLineSegment; 15 | import org.frcteam2910.common.control.Trajectory; 16 | import org.frcteam2910.common.math.RigidTransform2; 17 | import org.frcteam2910.common.math.Rotation2; 18 | import org.frcteam2910.common.math.Vector2; 19 | import org.frcteam2910.common.robot.drivers.Limelight; 20 | import org.frcteam2910.common.util.MovingAverage; 21 | 22 | import java.util.function.BiFunction; 23 | import java.util.function.Function; 24 | 25 | import static org.frcteam2910.common.robot.Utilities.deadband; 26 | 27 | public class ImprovedVisionPlaceCommand extends Command { 28 | private static final boolean USE_HIGH_RESOLUTION_MODE = false; 29 | 30 | private static final double PLACEMENT_DISTANCE = 20.0; 31 | private static final double OUTER_PLACEMENT_DISTANCE = 36 + PLACEMENT_DISTANCE; 32 | private static final double MAX_PLACEMENT_DISTANCE_CORRECTION = 5; 33 | private static final double INTERPOLATION_RATIO = MAX_PLACEMENT_DISTANCE_CORRECTION / (OUTER_PLACEMENT_DISTANCE - PLACEMENT_DISTANCE); 34 | 35 | private static final double HORIZONTAL_CORRECTION_MAX = 4; 36 | private static final double HORIZONTAL_CORRECTION_FACTOR = 0.25; 37 | private static final double HORIZONTAL_CORRECTION_DEADBAND_BUFFER = 0.5; 38 | 39 | private static final int FINISH_SUCCESSES_NEEDED = 4; 40 | 41 | private static final int HIGH_RESOLUTION_HEIGHT = 720; 42 | private static final int LOW_RESOLUTION_HEIGHT = 240; 43 | 44 | private static final int HIGH_RESOLUTION_PIPELINE = 9; 45 | private static final int LOW_RESOLUTION_PIPELINE = 8; 46 | 47 | private Limelight limelight = VisionSubsystem.getInstance().getLimelight(Gamepiece.HATCH_PANEL); 48 | 49 | private final BiFunction finishCondition; 50 | private final Function chooseTargetAngleFunction; 51 | private final ITrajectoryConstraint[] trajectoryConstraints; 52 | private final Vector2 targetOffset; 53 | 54 | 55 | private Rotation2 targetAngle; 56 | private Vector2 robotOrientedPositionOffset; 57 | private double distance = Double.POSITIVE_INFINITY; 58 | private double previousDistance; 59 | private Vector2 previousRobotOrientedPositionOffset; 60 | 61 | private int successesNeeded; 62 | 63 | public ImprovedVisionPlaceCommand(BiFunction finishCondition, 64 | Function chooseTargetAngleFunction, 65 | ITrajectoryConstraint[] trajectoryConstraints) { 66 | this(finishCondition, chooseTargetAngleFunction, trajectoryConstraints, 67 | new Vector2(PLACEMENT_DISTANCE, 0.0)); 68 | } 69 | 70 | public ImprovedVisionPlaceCommand(BiFunction finishCondition, 71 | Function chooseTargetAngleFunction, 72 | ITrajectoryConstraint[] trajectoryConstraints, 73 | Vector2 targetOffset) { 74 | this.finishCondition = finishCondition; 75 | this.chooseTargetAngleFunction = chooseTargetAngleFunction; 76 | this.trajectoryConstraints = trajectoryConstraints; 77 | this.targetOffset = targetOffset; 78 | 79 | requires(DrivetrainSubsystem.getInstance()); 80 | 81 | this.setRunWhenDisabled(true); 82 | } 83 | 84 | @Override 85 | protected void initialize() { 86 | limelight.setCamMode(Limelight.CamMode.VISION); 87 | if (USE_HIGH_RESOLUTION_MODE) { 88 | limelight.setPipeline(HIGH_RESOLUTION_PIPELINE); 89 | } else { 90 | limelight.setPipeline(LOW_RESOLUTION_PIPELINE); 91 | } 92 | 93 | distance = Double.POSITIVE_INFINITY; 94 | successesNeeded = FINISH_SUCCESSES_NEEDED; 95 | 96 | // If we don't have a hatch panel we know the target is the loading station which has an angle of 97 | targetAngle = chooseTargetAngleFunction.apply(Superstructure.getInstance().getGyroscope().getAngle()); 98 | } 99 | 100 | @Override 101 | protected void execute() { 102 | Robot.getOi().primaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kLeftRumble, 1.0); 103 | 104 | if (!limelight.hasTarget()) { 105 | DrivetrainSubsystem.getInstance().setSnapRotation(targetAngle.toRadians()); 106 | // We don't know where the target should be 107 | DrivetrainSubsystem.getInstance().holonomicDrive( 108 | new Vector2( 109 | Robot.getOi().primaryController.getLeftYAxis().get(true), 110 | Robot.getOi().primaryController.getLeftXAxis().get(true) 111 | ), 112 | Robot.getOi().primaryController.getRightXAxis().get(true), 113 | true 114 | ); 115 | } else { 116 | // We can see a target 117 | { 118 | double theta; 119 | if (USE_HIGH_RESOLUTION_MODE) { 120 | theta = limelight.getTable().getEntry("tvert").getDouble(0.0) * Math.toRadians(49.7) / HIGH_RESOLUTION_HEIGHT; 121 | } else { 122 | theta = limelight.getTable().getEntry("tvert").getDouble(0.0) * Math.toRadians(49.7) / LOW_RESOLUTION_HEIGHT; 123 | } 124 | distance = 5.83 / (2 * Math.tan(theta / 2)); 125 | 126 | robotOrientedPositionOffset = Vector2.fromAngle(Rotation2.fromRadians(limelight.getTargetPosition().x)).scale(distance); 127 | } 128 | 129 | if (distance != previousDistance || robotOrientedPositionOffset.x != previousRobotOrientedPositionOffset.x || robotOrientedPositionOffset.y != previousRobotOrientedPositionOffset.y) { 130 | previousDistance = distance; 131 | previousRobotOrientedPositionOffset = robotOrientedPositionOffset; 132 | 133 | double unboundedHorizontalError = Math.abs(robotOrientedPositionOffset.y) * HORIZONTAL_CORRECTION_FACTOR; 134 | double boundedHorizontalError = Math.min(unboundedHorizontalError, HORIZONTAL_CORRECTION_MAX); 135 | double horizontalError = deadband(boundedHorizontalError, HORIZONTAL_CORRECTION_DEADBAND_BUFFER); 136 | Vector2 newTargetOffset = targetOffset.add(interpolate(distance) + horizontalError, 0); 137 | 138 | SmartDashboard.putNumber("Target distance", robotOrientedPositionOffset.length); 139 | 140 | RigidTransform2 currentPose = new RigidTransform2( 141 | DrivetrainSubsystem.getInstance().getKinematicPosition(), 142 | Superstructure.getInstance().getGyroscope().getAngle() 143 | ); 144 | 145 | RigidTransform2 targetPose = currentPose.transformBy( 146 | new RigidTransform2( 147 | robotOrientedPositionOffset.subtract(newTargetOffset).multiply(-1.0, 1.0), 148 | Rotation2.ZERO 149 | ) 150 | ); 151 | 152 | Path path = new Path(targetAngle); 153 | path.addSegment( 154 | new PathLineSegment( 155 | DrivetrainSubsystem.getInstance().getKinematicPosition(), 156 | targetPose.translation 157 | ) 158 | ); 159 | 160 | double startingVelocity = DrivetrainSubsystem.getInstance().getKinematicVelocity().length; 161 | Trajectory.Segment lastSegment = DrivetrainSubsystem.getInstance().getFollower().getLastSegment(); 162 | if (lastSegment != null) { 163 | startingVelocity = lastSegment.velocity; 164 | } 165 | 166 | Trajectory trajectory = new Trajectory( 167 | startingVelocity, 168 | 0.0 * 12.0, 169 | path, 170 | trajectoryConstraints 171 | ); 172 | 173 | DrivetrainSubsystem.getInstance().getFollower().follow(trajectory); 174 | } 175 | } 176 | } 177 | 178 | @Override 179 | protected boolean isFinished() { 180 | if (robotOrientedPositionOffset == null) { 181 | return false; 182 | } 183 | 184 | if (finishCondition.apply(distance, robotOrientedPositionOffset.y)) { 185 | successesNeeded--; 186 | } else { 187 | successesNeeded = FINISH_SUCCESSES_NEEDED; 188 | } 189 | 190 | return successesNeeded == 0; 191 | } 192 | 193 | @Override 194 | protected void end() { 195 | DrivetrainSubsystem.getInstance().stop(); 196 | 197 | DrivetrainSubsystem.getInstance().getFollower().cancel(); 198 | 199 | Robot.getOi().primaryController.getRawJoystick().setRumble(GenericHID.RumbleType.kLeftRumble, 0.0); 200 | limelight.setCamMode(Limelight.CamMode.DRIVER); 201 | } 202 | 203 | private double interpolate(double toInterpolate) { 204 | if (toInterpolate <= PLACEMENT_DISTANCE) { 205 | return 0; 206 | } else if (toInterpolate >= OUTER_PLACEMENT_DISTANCE) { 207 | return MAX_PLACEMENT_DISTANCE_CORRECTION; 208 | } else { 209 | return INTERPOLATION_RATIO * (toInterpolate - PLACEMENT_DISTANCE); 210 | } 211 | } 212 | } 213 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/subsystems/CargoArmSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.subsystems; 2 | 3 | import edu.wpi.first.wpilibj.AnalogInput; 4 | import edu.wpi.first.wpilibj.RobotController; 5 | import edu.wpi.first.wpilibj.Spark; 6 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 7 | import org.frcteam2910.common.control.MotionProfileFollower; 8 | import org.frcteam2910.common.control.PidConstants; 9 | import org.frcteam2910.common.control.PidController; 10 | import org.frcteam2910.common.math.MathUtils; 11 | import org.frcteam2910.common.motion.MotionProfile; 12 | import org.frcteam2910.common.motion.TrapezoidalMotionProfile; 13 | import org.frcteam2910.common.robot.drivers.NavX; 14 | import org.frcteam2910.common.robot.subsystems.Subsystem; 15 | import org.frcteam2910.c2019.RobotMap; 16 | 17 | import java.util.Optional; 18 | 19 | public class CargoArmSubsystem extends Subsystem { 20 | public static final double CARGO_SHIP_SCORE_ANGLE = Math.toRadians(100.0); 21 | public static final double ROCKET_SCORE_ANGLE = Math.toRadians(63.0); 22 | public static final double BOTTOM_ANGLE = Math.toRadians(2.0); 23 | public static final double VISION_TARGET_ANGLE = Math.toRadians(95.0); 24 | 25 | private static final double ANGLE_OFFSET_COMPETITION = Math.toRadians(-221.75); 26 | private static final double ANGLE_OFFSET_PRACTICE = Math.toRadians(-212.11148939808933); 27 | 28 | // These really shouldn't be different but it is good to have so we can make sure we don't run into the hard stops. 29 | private static final double MAX_ANGLE_COMPETITION = Math.toRadians(108.25736028138388); 30 | private static final double MAX_ANGLE_PRACTICE = Math.toRadians(110.41971277880431); 31 | 32 | private static final double ENCODER_GEAR_RATIO = 24.0 / 54.0; 33 | private static final double ALLOWABLE_TARGET_ANGLE_ERROR = Math.toRadians(4.0); // Allowable error range of 2 degrees 34 | 35 | private static final MotionProfile.Constraints UPWARDS_MOTION_CONSTRAINTS = new MotionProfile.Constraints( 36 | Math.toRadians(180.0), 37 | Math.toRadians(550.0) 38 | ); 39 | private static final MotionProfile.Constraints DOWNWARDS_MOTION_CONSTRAINTS = new MotionProfile.Constraints( 40 | Math.toRadians(180.0), 41 | Math.toRadians(800.0) 42 | ); 43 | private static final double MOTION_KV = 1.0 / Math.toRadians(180.0); 44 | private static final double MOTION_KA = 1.0 / Math.toRadians(4800.0); 45 | 46 | private static final double ANGLE_FEEDFORWARD = 0.03; 47 | 48 | private static final CargoArmSubsystem instance = new CargoArmSubsystem(); 49 | 50 | private final Spark[] motors = { 51 | new Spark(RobotMap.ARM_MOTOR_A), 52 | new Spark(RobotMap.ARM_MOTOR_B) 53 | }; 54 | 55 | private final double angleOffset; 56 | private final double maxAngle; 57 | 58 | private final PidConstants followerPidConstants = new PidConstants(2.0, 0.0, 0.0); 59 | private final PidConstants pitchPidConstants = new PidConstants(1.0 / Math.toRadians(10.0), 0.0, 0.0); 60 | 61 | private PidController followerController = new PidController(followerPidConstants); 62 | private MotionProfileFollower follower = new MotionProfileFollower(followerController, MOTION_KV, MOTION_KA); 63 | private PidController pitchController = new PidController(pitchPidConstants); 64 | 65 | private final Object sensorLock = new Object(); 66 | private AnalogInput angleEncoder = new AnalogInput(RobotMap.CARGO_ARM_ENCODER_PORT); 67 | private double currentAngle = Math.toRadians(0); 68 | 69 | private final Object stateLock = new Object(); 70 | private State currentState = State.DISABLED; 71 | // private double targetAngle = 0.0; 72 | private double targetPitch = 0.0; 73 | private boolean pitchOnlyDown = false; 74 | 75 | private double previousTimestamp = 0; 76 | 77 | private CargoArmSubsystem() { 78 | if (Superstructure.getInstance().isPracticeBot()) { 79 | angleOffset = ANGLE_OFFSET_PRACTICE; 80 | maxAngle = MAX_ANGLE_PRACTICE; 81 | } else { 82 | angleOffset = ANGLE_OFFSET_COMPETITION; 83 | maxAngle = MAX_ANGLE_COMPETITION; 84 | } 85 | 86 | followerController.setInputRange(0.0, 2.0 * Math.PI); 87 | followerController.setContinuous(true); 88 | followerController.setOutputRange(-1.0, 1.0); 89 | followerController.setIntegralRange(Math.toRadians(10.0)); 90 | } 91 | 92 | public static CargoArmSubsystem getInstance() { 93 | return instance; 94 | } 95 | 96 | public double getMaxAngle() { 97 | return maxAngle; 98 | } 99 | 100 | @Override 101 | public void outputToSmartDashboard() { 102 | SmartDashboard.putNumber("Cargo Arm Angle", Math.toDegrees(getCurrentAngle())); 103 | SmartDashboard.putNumber("Cargo Arm Target Angle", Math.toDegrees(getTargetAngle())); 104 | } 105 | 106 | public double getCurrentAngle() { 107 | synchronized (sensorLock) { 108 | return currentAngle; 109 | } 110 | } 111 | 112 | public double getTargetAngle() { 113 | MotionProfile profile = follower.getCurrentMotionProfile(); 114 | if (profile == null) { 115 | return getCurrentAngle(); 116 | } else { 117 | return profile.getEnd().position; 118 | } 119 | } 120 | 121 | public void setTargetAngle(double angle) { 122 | angle = MathUtils.clamp(angle, 0.0, maxAngle); 123 | 124 | double currentAngle = getCurrentAngle(); 125 | if (currentAngle > Math.toRadians(180.0)) { 126 | currentAngle -= Math.toRadians(360.0); 127 | } 128 | 129 | MotionProfile.Constraints constraints; 130 | if (angle - currentAngle > 0.0) { 131 | constraints = UPWARDS_MOTION_CONSTRAINTS; 132 | } else { 133 | constraints = DOWNWARDS_MOTION_CONSTRAINTS; 134 | } 135 | 136 | MotionProfile profile = new TrapezoidalMotionProfile( 137 | new MotionProfile.Goal(currentAngle, 0.0), 138 | new MotionProfile.Goal(angle, 0.0), 139 | constraints 140 | ); 141 | 142 | follower.follow(profile); 143 | synchronized (stateLock) { 144 | currentState = State.POSITION; 145 | } 146 | } 147 | 148 | public double getTargetPitch() { 149 | synchronized (stateLock) { 150 | return targetPitch; 151 | } 152 | } 153 | 154 | public void setTargetPitch(double pitch, boolean pitchOnlyDown) { 155 | synchronized (stateLock) { 156 | targetPitch = pitch; 157 | this.pitchOnlyDown = pitchOnlyDown; 158 | currentState = State.PITCH; 159 | } 160 | } 161 | 162 | public void disable() { 163 | synchronized (stateLock) { 164 | currentState = State.DISABLED; 165 | } 166 | } 167 | 168 | @Override 169 | public void updateKinematics(double timestamp) { 170 | State localCurrentState; 171 | synchronized (stateLock) { 172 | localCurrentState = currentState; 173 | } 174 | 175 | double dt = timestamp - previousTimestamp; 176 | previousTimestamp = timestamp; 177 | 178 | updateSensors(); 179 | 180 | double output = 0.0; 181 | 182 | double currentAngle = getCurrentAngle(); 183 | 184 | switch (localCurrentState) { 185 | case DISABLED: 186 | output = 0.0; 187 | break; 188 | case PITCH: 189 | double localTargetPitch; 190 | synchronized (stateLock) { 191 | localTargetPitch = targetPitch; 192 | } 193 | 194 | double currentPitch = Superstructure.getInstance().getGyroscope().getAxis(NavX.Axis.ROLL); 195 | 196 | pitchController.setSetpoint(localTargetPitch); 197 | output = pitchController.calculate(currentPitch, dt); 198 | 199 | // If we are close to the bottom of the range of travel, force the arm down. 200 | if (MathUtils.epsilonEquals(currentAngle, 0.0, Math.toRadians(5.0)) && pitchOnlyDown) { // TODO: Make constant 201 | output = -0.4; // TODO: Make constant 202 | } 203 | 204 | if (currentAngle < Math.toRadians(65.0) && currentAngle > Math.toRadians(355.0) && pitchOnlyDown) { 205 | output = MathUtils.clamp(output, -1.0, 0.0); 206 | } else if (!pitchOnlyDown) { 207 | // Don't go above 100 degrees 208 | if (currentAngle > Math.toRadians(100.0)) { 209 | output = 0.0; 210 | } else { 211 | output = MathUtils.clamp(output, 0.0, 1.0); 212 | } 213 | } 214 | break; 215 | case POSITION: 216 | // double localTargetAngle; 217 | // synchronized (stateLock) { 218 | // localTargetAngle = targetAngle; 219 | // } 220 | 221 | // positionController.setSetpoint(targetAngle); 222 | // output = positionController.calculate(currentAngle, dt); 223 | 224 | output = follower.update(currentAngle, timestamp, dt); 225 | Optional lastState = follower.getLastState(); 226 | if (lastState.isPresent()) { 227 | SmartDashboard.putNumber("Target Arm Angle", Math.toDegrees(lastState.get().position)); 228 | output += ANGLE_FEEDFORWARD * Math.cos(lastState.get().position); 229 | } 230 | 231 | SmartDashboard.putNumber("Follower output", output); 232 | break; 233 | } 234 | 235 | motors[0].set(output); 236 | motors[1].set(output); 237 | } 238 | 239 | public boolean isWithinTargetAngleRange(double targetAngle) { 240 | double currentAngle = getCurrentAngle(); 241 | return MathUtils.epsilonEquals(currentAngle, targetAngle, ALLOWABLE_TARGET_ANGLE_ERROR); 242 | } 243 | 244 | private void updateSensors() { 245 | double encoderRotations = angleEncoder.getVoltage() / RobotController.getVoltage5V(); 246 | double armRotations = ENCODER_GEAR_RATIO * encoderRotations; 247 | double armUnadjustedAngle = armRotations * 2.0 * Math.PI; 248 | double armAngle = 2.0 * Math.PI - armUnadjustedAngle; 249 | armAngle += angleOffset; 250 | armAngle %= 2.0 * Math.PI; 251 | if (armAngle < 0.0) { 252 | armAngle += 2.0 * Math.PI; 253 | } 254 | 255 | synchronized (sensorLock) { 256 | currentAngle = armAngle; 257 | } 258 | } 259 | 260 | @Override 261 | public void stop() { 262 | 263 | } 264 | 265 | @Override 266 | public void zeroSensors() { 267 | 268 | } 269 | 270 | @Override 271 | protected void initDefaultCommand() { 272 | 273 | } 274 | 275 | 276 | public enum State { 277 | DISABLED, 278 | POSITION, 279 | PITCH 280 | } 281 | } 282 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/subsystems/DrivetrainSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.subsystems; 2 | 3 | import com.revrobotics.CANSparkMax; 4 | import com.revrobotics.CANSparkMaxLowLevel; 5 | import edu.wpi.first.wpilibj.AnalogInput; 6 | import edu.wpi.first.wpilibj.Spark; 7 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 8 | import org.frcteam2910.c2019.RobotMap; 9 | import org.frcteam2910.c2019.commands.HolonomicDriveCommand; 10 | import org.frcteam2910.c2019.drivers.Mk2SwerveModule; 11 | import org.frcteam2910.common.control.*; 12 | import org.frcteam2910.common.drivers.Gyroscope; 13 | import org.frcteam2910.common.drivers.SwerveModule; 14 | import org.frcteam2910.common.math.RigidTransform2; 15 | import org.frcteam2910.common.math.Vector2; 16 | import org.frcteam2910.common.robot.subsystems.SwerveDrivetrain; 17 | import org.frcteam2910.common.util.DrivetrainFeedforwardConstants; 18 | import org.frcteam2910.common.util.HolonomicDriveSignal; 19 | import org.frcteam2910.common.util.HolonomicFeedforward; 20 | 21 | import java.util.Optional; 22 | 23 | public class DrivetrainSubsystem extends SwerveDrivetrain { 24 | private static final double TRACKWIDTH = 19.5; 25 | private static final double WHEELBASE = 23.5; 26 | 27 | private static final double MAX_VELOCITY = 12.0 * 12.0; 28 | 29 | public static final ITrajectoryConstraint[] CONSTRAINTS = { 30 | new MaxVelocityConstraint(MAX_VELOCITY), 31 | new MaxAccelerationConstraint(13.0 * 12.0), 32 | new CentripetalAccelerationConstraint(25.0 * 12.0) 33 | }; 34 | 35 | private static final double FRONT_LEFT_ANGLE_OFFSET_COMPETITION = Math.toRadians(-154.3); 36 | private static final double FRONT_RIGHT_ANGLE_OFFSET_COMPETITION = Math.toRadians(-329.0); 37 | private static final double BACK_LEFT_ANGLE_OFFSET_COMPETITION = Math.toRadians(-218.10); 38 | private static final double BACK_RIGHT_ANGLE_OFFSET_COMPETITION = Math.toRadians(-268.9); 39 | private static final double FRONT_LEFT_ANGLE_OFFSET_PRACTICE = Math.toRadians(-170.2152486947372); 40 | private static final double FRONT_RIGHT_ANGLE_OFFSET_PRACTICE = Math.toRadians(-43.55619048306742); 41 | private static final double BACK_LEFT_ANGLE_OFFSET_PRACTICE = Math.toRadians(-237.47063008637048); 42 | private static final double BACK_RIGHT_ANGLE_OFFSET_PRACTICE = Math.toRadians(-336.70093128378477); 43 | 44 | private static final PidConstants FOLLOWER_TRANSLATION_CONSTANTS = new PidConstants(0.05, 0.01, 0.0); 45 | private static final PidConstants FOLLOWER_ROTATION_CONSTANTS = new PidConstants(0.2, 0.01, 0.0); 46 | private static final HolonomicFeedforward FOLLOWER_FEEDFORWARD_CONSTANTS = new HolonomicFeedforward( 47 | new DrivetrainFeedforwardConstants(1.0 / (14.0 * 12.0), 0.0, 0.0) 48 | ); 49 | 50 | private static final PidConstants SNAP_ROTATION_CONSTANTS = new PidConstants(0.3, 0.01, 0.0); 51 | 52 | private static final DrivetrainSubsystem instance = new DrivetrainSubsystem(); 53 | 54 | private Mk2SwerveModule[] swerveModules; 55 | 56 | private HolonomicMotionProfiledTrajectoryFollower follower = new HolonomicMotionProfiledTrajectoryFollower( 57 | FOLLOWER_TRANSLATION_CONSTANTS, 58 | FOLLOWER_ROTATION_CONSTANTS, 59 | FOLLOWER_FEEDFORWARD_CONSTANTS 60 | ); 61 | 62 | private PidController snapRotationController = new PidController(SNAP_ROTATION_CONSTANTS); 63 | private double snapRotation = Double.NaN; 64 | 65 | private double lastTimestamp = 0; 66 | 67 | private final Object lock = new Object(); 68 | private HolonomicDriveSignal signal = new HolonomicDriveSignal(Vector2.ZERO, 0.0, false); 69 | private Trajectory.Segment segment = null; 70 | 71 | private DrivetrainSubsystem() { 72 | double frontLeftAngleOffset = FRONT_LEFT_ANGLE_OFFSET_COMPETITION; 73 | double frontRightAngleOffset = FRONT_RIGHT_ANGLE_OFFSET_COMPETITION; 74 | double backLeftAngleOffset = BACK_LEFT_ANGLE_OFFSET_COMPETITION; 75 | double backRightAngleOffset = BACK_RIGHT_ANGLE_OFFSET_COMPETITION; 76 | if (Superstructure.getInstance().isPracticeBot()) { 77 | frontLeftAngleOffset = FRONT_LEFT_ANGLE_OFFSET_PRACTICE; 78 | frontRightAngleOffset = FRONT_RIGHT_ANGLE_OFFSET_PRACTICE; 79 | backLeftAngleOffset = BACK_LEFT_ANGLE_OFFSET_PRACTICE; 80 | backRightAngleOffset = BACK_RIGHT_ANGLE_OFFSET_PRACTICE; 81 | } 82 | 83 | Mk2SwerveModule frontLeftModule = new Mk2SwerveModule( 84 | new Vector2(-TRACKWIDTH / 2.0, WHEELBASE / 2.0), 85 | frontLeftAngleOffset, 86 | new Spark(RobotMap.DRIVETRAIN_FRONT_LEFT_ANGLE_MOTOR), 87 | new CANSparkMax(RobotMap.DRIVETRAIN_FRONT_LEFT_DRIVE_MOTOR, CANSparkMaxLowLevel.MotorType.kBrushless), 88 | new AnalogInput(RobotMap.DRIVETRAIN_FRONT_LEFT_ANGLE_ENCODER) 89 | ); 90 | frontLeftModule.setName("Front Left"); 91 | 92 | Mk2SwerveModule frontRightModule = new Mk2SwerveModule( 93 | new Vector2(TRACKWIDTH / 2.0, WHEELBASE / 2.0), 94 | frontRightAngleOffset, 95 | new Spark(RobotMap.DRIVETRAIN_FRONT_RIGHT_ANGLE_MOTOR), 96 | new CANSparkMax(RobotMap.DRIVETRAIN_FRONT_RIGHT_DRIVE_MOTOR, CANSparkMaxLowLevel.MotorType.kBrushless), 97 | new AnalogInput(RobotMap.DRIVETRAIN_FRONT_RIGHT_ANGLE_ENCODER) 98 | ); 99 | frontRightModule.setName("Front Right"); 100 | 101 | Mk2SwerveModule backLeftModule = new Mk2SwerveModule( 102 | new Vector2(-TRACKWIDTH / 2.0, -WHEELBASE / 2.0), 103 | backLeftAngleOffset, 104 | new Spark(RobotMap.DRIVETRAIN_BACK_LEFT_ANGLE_MOTOR), 105 | new CANSparkMax(RobotMap.DRIVETRAIN_BACK_LEFT_DRIVE_MOTOR, CANSparkMaxLowLevel.MotorType.kBrushless), 106 | new AnalogInput(RobotMap.DRIVETRAIN_BACK_LEFT_ANGLE_ENCODER) 107 | ); 108 | backLeftModule.setName("Back Left"); 109 | 110 | Mk2SwerveModule backRightModule = new Mk2SwerveModule( 111 | new Vector2(TRACKWIDTH / 2.0, -WHEELBASE / 2.0), 112 | backRightAngleOffset, 113 | new Spark(RobotMap.DRIVETRAIN_BACK_RIGHT_ANGLE_MOTOR), 114 | new CANSparkMax(RobotMap.DRIVETRAIN_BACK_RIGHT_DRIVE_MOTOR, CANSparkMaxLowLevel.MotorType.kBrushless), 115 | new AnalogInput(RobotMap.DRIVETRAIN_BACK_RIGHT_ANGLE_ENCODER) 116 | ); 117 | backRightModule.setName("Back Right"); 118 | 119 | swerveModules = new Mk2SwerveModule[]{ 120 | frontLeftModule, 121 | frontRightModule, 122 | backLeftModule, 123 | backRightModule, 124 | }; 125 | 126 | snapRotationController.setInputRange(0.0, 2.0 * Math.PI); 127 | snapRotationController.setContinuous(true); 128 | snapRotationController.setOutputRange(-0.5, 0.5); 129 | } 130 | 131 | public void setSnapRotation(double snapRotation) { 132 | synchronized (lock) { 133 | this.snapRotation = snapRotation; 134 | } 135 | } 136 | 137 | public void stopSnap() { 138 | synchronized (lock) { 139 | this.snapRotation = Double.NaN; 140 | } 141 | } 142 | 143 | @Override 144 | public void holonomicDrive(Vector2 translation, double rotation, boolean fieldOriented) { 145 | synchronized (lock) { 146 | this.signal = new HolonomicDriveSignal(translation, rotation, fieldOriented); 147 | } 148 | } 149 | 150 | @Override 151 | public synchronized void updateKinematics(double timestamp) { 152 | super.updateKinematics(timestamp); 153 | 154 | double dt = timestamp - lastTimestamp; 155 | lastTimestamp = timestamp; 156 | 157 | double localSnapRotation; 158 | synchronized (lock) { 159 | localSnapRotation = snapRotation; 160 | } 161 | RigidTransform2 currentPose = new RigidTransform2( 162 | getKinematicPosition(), 163 | getGyroscope().getAngle() 164 | ); 165 | 166 | Optional optSignal = follower.update(currentPose, getKinematicVelocity(), 167 | getGyroscope().getRate(), timestamp, dt); 168 | HolonomicDriveSignal localSignal; 169 | 170 | if (optSignal.isPresent()) { 171 | localSignal = optSignal.get(); 172 | 173 | synchronized (lock) { 174 | segment = follower.getLastSegment(); 175 | } 176 | } else { 177 | synchronized (lock) { 178 | localSignal = this.signal; 179 | } 180 | } 181 | 182 | if (Math.abs(localSignal.getRotation()) < 0.1 && Double.isFinite(localSnapRotation)) { 183 | snapRotationController.setSetpoint(localSnapRotation); 184 | 185 | localSignal = new HolonomicDriveSignal(localSignal.getTranslation(), 186 | snapRotationController.calculate(getGyroscope().getAngle().toRadians(), dt), 187 | localSignal.isFieldOriented()); 188 | } else { 189 | synchronized (lock) { 190 | snapRotation = Double.NaN; 191 | } 192 | } 193 | 194 | super.holonomicDrive(localSignal.getTranslation(), localSignal.getRotation(), localSignal.isFieldOriented()); 195 | } 196 | 197 | @Override 198 | public void outputToSmartDashboard() { 199 | super.outputToSmartDashboard(); 200 | 201 | HolonomicDriveSignal localSignal; 202 | Trajectory.Segment localSegment; 203 | synchronized (lock) { 204 | localSignal = signal; 205 | localSegment = segment; 206 | } 207 | 208 | SmartDashboard.putNumber("Drivetrain Follower Forwards", localSignal.getTranslation().x); 209 | SmartDashboard.putNumber("Drivetrain Follower Strafe", localSignal.getTranslation().y); 210 | SmartDashboard.putNumber("Drivetrain Follower Rotation", localSignal.getRotation()); 211 | SmartDashboard.putBoolean("Drivetrain Follower Field Oriented", localSignal.isFieldOriented()); 212 | 213 | if (follower.getCurrentTrajectory().isPresent() && localSegment != null) { 214 | SmartDashboard.putNumber("Drivetrain Follower Target Angle", localSegment.rotation.toDegrees()); 215 | 216 | Vector2 position = getKinematicPosition(); 217 | 218 | SmartDashboard.putNumber("Drivetrain Follower X Error", localSegment.translation.x - position.x); 219 | SmartDashboard.putNumber("Drivetrain Follower Y Error", localSegment.translation.y - position.y); 220 | SmartDashboard.putNumber("Drivetrain Follower Angle Error", localSegment.rotation.toDegrees() - getGyroscope().getAngle().toDegrees()); 221 | } 222 | 223 | for (Mk2SwerveModule module : swerveModules) { 224 | SmartDashboard.putNumber(String.format("%s Module Drive Current Draw", module.getName()), module.getDriveCurrent()); 225 | } 226 | } 227 | 228 | public static DrivetrainSubsystem getInstance() { 229 | return instance; 230 | } 231 | 232 | @Override 233 | public SwerveModule[] getSwerveModules() { 234 | return swerveModules; 235 | } 236 | 237 | @Override 238 | public Gyroscope getGyroscope() { 239 | return Superstructure.getInstance().getGyroscope(); 240 | } 241 | 242 | @Override 243 | public double getMaximumVelocity() { 244 | return 0; 245 | } 246 | 247 | @Override 248 | public double getMaximumAcceleration() { 249 | return 0; 250 | } 251 | 252 | @Override 253 | protected void initDefaultCommand() { 254 | setDefaultCommand(new HolonomicDriveCommand()); 255 | } 256 | 257 | @Override 258 | public void stop() { 259 | super.stop(); 260 | synchronized (lock) { 261 | snapRotation = Double.NaN; 262 | } 263 | } 264 | 265 | public HolonomicMotionProfiledTrajectoryFollower getFollower() { 266 | return follower; 267 | } 268 | } 269 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/subsystems/HatchFloorGathererSubsystem.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.subsystems; 2 | 3 | import com.ctre.phoenix.motorcontrol.ControlMode; 4 | import com.ctre.phoenix.motorcontrol.FeedbackDevice; 5 | import com.ctre.phoenix.motorcontrol.NeutralMode; 6 | import com.ctre.phoenix.motorcontrol.can.TalonSRX; 7 | import com.revrobotics.CANEncoder; 8 | import com.revrobotics.CANSparkMax; 9 | import com.revrobotics.CANSparkMaxLowLevel; 10 | import edu.wpi.first.wpilibj.DigitalInput; 11 | import edu.wpi.first.wpilibj.Notifier; 12 | import edu.wpi.first.wpilibj.Timer; 13 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 14 | import org.frcteam2910.c2019.RobotMap; 15 | import org.frcteam2910.common.math.MathUtils; 16 | import org.frcteam2910.common.robot.Constants; 17 | import org.frcteam2910.common.robot.subsystems.Subsystem; 18 | 19 | public class HatchFloorGathererSubsystem extends Subsystem { 20 | public static final double CAN_UPDATE_RATE = 50.0; 21 | 22 | private static final double ANGLE_ENCODER_TICKS_PER_REVOLUTION = 4096.0; 23 | 24 | private static final double ANGLE_OFFSET_COMPETITION = Math.toRadians(-178.066); 25 | private static final double ANGLE_OFFSET_PRACTICE = Math.toRadians(-159.52); 26 | 27 | private static final double MAX_ANGLE_COMPETITION = Math.toRadians(140.5); 28 | private static final double MAX_ANGLE_PRACTICE = Math.toRadians(135.0); 29 | 30 | private static final double ANGLE_PID_P = 3.0; 31 | private static final double ANGLE_PID_I = 0.0075; 32 | private static final double ANGLE_PID_D = 10.0; 33 | private static final double ANGLE_PID_F = 1.0; 34 | 35 | private static final double ALLOWABLE_TARGET_ANGLE_ERROR = Math.toRadians(2.0); // Allowable error range of 2 degrees 36 | 37 | private static final double HATCH_DETECTION_DEBOUNCE_TIME = 0.5; 38 | private static final double INTAKE_SOFT_INTAKE_SPEED = 0.3; 39 | 40 | private static final HatchFloorGathererSubsystem instance = new HatchFloorGathererSubsystem(); 41 | 42 | private final double angleOffset; 43 | private final double maxAngle; 44 | 45 | private TalonSRX angleMotor = new TalonSRX(RobotMap.HATCH_FLOOR_GATHERER_ARM_MOTOR); 46 | 47 | private CANSparkMax intakeMotor = new CANSparkMax(RobotMap.HATCH_FLOOR_GATHERER_FLOOR_MOTOR, 48 | CANSparkMaxLowLevel.MotorType.kBrushed); 49 | private CANEncoder intakeEncoder = intakeMotor.getEncoder(); 50 | 51 | private DigitalInput hatchDetector = new DigitalInput(4); 52 | 53 | private final Object canLock = new Object(); 54 | private int angleEncoderTicks = 0; 55 | private double intakeRpm = 0.0; 56 | 57 | private Notifier canUpdateThread; 58 | 59 | private final Object sensorLock = new Object(); 60 | private double currentAngle = 0.0; 61 | private double lastHatchDetectionTime = 0.0; 62 | private boolean hatchDetected = false; 63 | 64 | private final Object targetLock = new Object(); 65 | private double targetAngle; 66 | private double targetIntakeSpeed = 0.0; 67 | 68 | private final Object outputLock = new Object(); 69 | private double angleTicksOutput = 0.0; 70 | private double intakeOutput = 0.0; 71 | 72 | private HatchFloorGathererSubsystem() { 73 | if (Superstructure.getInstance().isPracticeBot()) { 74 | angleOffset = ANGLE_OFFSET_PRACTICE; 75 | maxAngle = MAX_ANGLE_PRACTICE; 76 | } else { 77 | angleOffset = ANGLE_OFFSET_COMPETITION; 78 | maxAngle = MAX_ANGLE_COMPETITION; 79 | } 80 | targetAngle = maxAngle; 81 | 82 | angleMotor.configFactoryDefault(Constants.CAN_TIMEOUT_MS); 83 | 84 | angleMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, Constants.CAN_TIMEOUT_MS); 85 | angleMotor.setInverted(false); 86 | angleMotor.setSensorPhase(false); 87 | angleMotor.configFeedbackNotContinuous(true, Constants.CAN_TIMEOUT_MS); 88 | 89 | angleMotor.config_kP(0, ANGLE_PID_P, Constants.CAN_TIMEOUT_MS); 90 | angleMotor.config_kI(0, ANGLE_PID_I, Constants.CAN_TIMEOUT_MS); 91 | angleMotor.config_kD(0, ANGLE_PID_D, Constants.CAN_TIMEOUT_MS); 92 | angleMotor.config_kF(0, ANGLE_PID_F, Constants.CAN_TIMEOUT_MS); 93 | 94 | angleMotor.configAllowableClosedloopError(0, 0, Constants.CAN_TIMEOUT_MS); 95 | angleMotor.config_IntegralZone(0, 100, Constants.CAN_TIMEOUT_MS); 96 | 97 | angleMotor.setNeutralMode(NeutralMode.Brake); 98 | 99 | angleMotor.configForwardSoftLimitThreshold(convertAngleToEncoderTicks(0.0), Constants.CAN_TIMEOUT_MS); 100 | angleMotor.configForwardSoftLimitEnable(false, Constants.CAN_TIMEOUT_MS); 101 | 102 | angleMotor.configReverseSoftLimitThreshold(convertAngleToEncoderTicks(maxAngle), Constants.CAN_TIMEOUT_MS); 103 | angleMotor.configReverseSoftLimitEnable(false, Constants.CAN_TIMEOUT_MS); 104 | 105 | angleMotor.configMotionCruiseVelocity(5000, Constants.CAN_TIMEOUT_MS); 106 | angleMotor.configMotionAcceleration(1500, Constants.CAN_TIMEOUT_MS); 107 | 108 | intakeMotor.setInverted(true); 109 | intakeMotor.setSmartCurrentLimit(20); 110 | 111 | canUpdateThread = new Notifier(() -> { 112 | int angleTicks = angleMotor.getSelectedSensorPosition(); 113 | 114 | synchronized (canLock) { 115 | angleEncoderTicks = angleTicks; 116 | } 117 | 118 | double intakeRpm = intakeEncoder.getVelocity(); 119 | synchronized (canLock) { 120 | HatchFloorGathererSubsystem.this.intakeRpm = intakeRpm; 121 | } 122 | 123 | double angleTicksOutput; 124 | double intakeOutput; 125 | 126 | synchronized (outputLock) { 127 | angleTicksOutput = this.angleTicksOutput; 128 | intakeOutput = this.intakeOutput; 129 | } 130 | 131 | angleMotor.set(ControlMode.MotionMagic, angleTicksOutput); 132 | intakeMotor.set(intakeOutput); 133 | }); 134 | canUpdateThread.startPeriodic(1.0 / CAN_UPDATE_RATE); 135 | } 136 | 137 | private int convertAngleToEncoderTicks(double angle) { 138 | double unoffsettedAngle = angle - angleOffset; 139 | unoffsettedAngle = 2.0 * Math.PI - unoffsettedAngle; 140 | unoffsettedAngle %= 2.0 * Math.PI; 141 | if (unoffsettedAngle < 0.0) { 142 | unoffsettedAngle += 2.0 * Math.PI; 143 | } 144 | 145 | return (int) (unoffsettedAngle * (ANGLE_ENCODER_TICKS_PER_REVOLUTION / (2.0 * Math.PI))); 146 | } 147 | 148 | private double convertEncoderTicksToAngle(int ticks) { 149 | double revolutions = ticks / ANGLE_ENCODER_TICKS_PER_REVOLUTION; 150 | double unoffsettedAngle = 2.0 * Math.PI * revolutions; 151 | unoffsettedAngle = 2.0 * Math.PI - unoffsettedAngle; 152 | double angle = unoffsettedAngle + angleOffset; 153 | angle %= 2.0 * Math.PI; 154 | if (angle < 0.0) { 155 | angle += 2.0 * Math.PI; 156 | } 157 | 158 | return angle; 159 | } 160 | 161 | public static HatchFloorGathererSubsystem getInstance() { 162 | return instance; 163 | } 164 | 165 | public double getMaxAngle() { 166 | return maxAngle; 167 | } 168 | 169 | public double getCurrentAngle() { 170 | synchronized (sensorLock) { 171 | return currentAngle; 172 | } 173 | } 174 | 175 | public double getIntakeRpm() { 176 | synchronized (canLock) { 177 | return intakeRpm; 178 | } 179 | } 180 | 181 | public boolean hasHatchPanel() { 182 | double localLastHatchDetectionTime; 183 | boolean localHatchDetected; 184 | synchronized (sensorLock) { 185 | localLastHatchDetectionTime = lastHatchDetectionTime; 186 | localHatchDetected = hatchDetected; 187 | } 188 | 189 | double deltaTime = Timer.getFPGATimestamp() - localLastHatchDetectionTime; 190 | 191 | return localHatchDetected || deltaTime < HATCH_DETECTION_DEBOUNCE_TIME; 192 | } 193 | 194 | public boolean isAtBottomOfTravel() { 195 | return getCurrentAngle() <= 0.0; 196 | } 197 | 198 | public boolean isAtTopOfTravel() { 199 | return getCurrentAngle() >= maxAngle; 200 | } 201 | 202 | public double getTargetAngle() { 203 | synchronized (targetLock) { 204 | return targetAngle; 205 | } 206 | } 207 | 208 | public boolean isAtTargetAngle() { 209 | return MathUtils.epsilonEquals(getCurrentAngle(), getTargetAngle(), ALLOWABLE_TARGET_ANGLE_ERROR); 210 | } 211 | 212 | public void setTargetAngle(double angle) { 213 | angle = MathUtils.clamp(angle, 0.0, maxAngle); 214 | 215 | synchronized (targetLock) { 216 | targetAngle = angle; 217 | } 218 | } 219 | 220 | public double getTargetIntakeSpeed() { 221 | return targetIntakeSpeed; 222 | } 223 | 224 | public void setTargetIntakeSpeed(double speed) { 225 | speed = MathUtils.clamp(speed, -1.0, 1.0); 226 | 227 | synchronized (targetLock) { 228 | targetIntakeSpeed = speed; 229 | } 230 | } 231 | 232 | private void updateSensors(double time) { 233 | int angleEncoderTicks; 234 | synchronized (canLock) { 235 | angleEncoderTicks = this.angleEncoderTicks; 236 | } 237 | 238 | double angle = convertEncoderTicksToAngle(angleEncoderTicks); 239 | 240 | // Hatch detector is normally on 241 | boolean localHatchDetected = !hatchDetector.get(); 242 | synchronized (sensorLock) { 243 | currentAngle = angle; 244 | hatchDetected = localHatchDetected; 245 | if (localHatchDetected) { 246 | lastHatchDetectionTime = time; 247 | } 248 | } 249 | } 250 | 251 | @Override 252 | public void updateKinematics(double timestamp) { 253 | updateSensors(timestamp); 254 | 255 | double targetAngle; 256 | double targetIntakeSpeed; 257 | 258 | synchronized (targetLock) { 259 | targetAngle = this.targetAngle; 260 | targetIntakeSpeed = this.targetIntakeSpeed; 261 | } 262 | 263 | // If we have a hatch panel and we want to be either intaking or stopped, soft intake 264 | if (hasHatchPanel() && targetIntakeSpeed >= 0.0) { 265 | targetIntakeSpeed = Math.max(targetIntakeSpeed, INTAKE_SOFT_INTAKE_SPEED); 266 | } 267 | 268 | double targetAngleTicks = convertAngleToEncoderTicks(targetAngle); 269 | 270 | synchronized (outputLock) { 271 | angleTicksOutput = targetAngleTicks; 272 | intakeOutput = targetIntakeSpeed; 273 | } 274 | } 275 | 276 | @Override 277 | public void outputToSmartDashboard() { 278 | double currentAngle = getCurrentAngle(); 279 | double targetAngle = getTargetAngle(); 280 | 281 | SmartDashboard.putNumber("Floor Intake Arm Angle", Math.toDegrees(currentAngle)); 282 | SmartDashboard.putNumber("Floor Intake Arm Angle Ticks", angleEncoderTicks); 283 | 284 | SmartDashboard.putNumber("Floor Intake Arm Target Angle", Math.toDegrees(targetAngle)); 285 | SmartDashboard.putNumber("Floor Intake Arm Target Angle Ticks", convertAngleToEncoderTicks(targetAngle)); 286 | 287 | SmartDashboard.putNumber("Floor Intake Target Speed", getTargetIntakeSpeed()); 288 | SmartDashboard.putNumber("Floor Intake RPM", getIntakeRpm()); 289 | SmartDashboard.putBoolean("Floor Intake Detects Hatch Panel", hasHatchPanel()); 290 | 291 | SmartDashboard.putNumber("Floor Intake Angle Error", angleMotor.getClosedLoopError()); 292 | 293 | SmartDashboard.putBoolean("Floor Intake Hatch Detected", hasHatchPanel()); 294 | } 295 | 296 | @Override 297 | public void stop() { 298 | 299 | } 300 | 301 | @Override 302 | public void zeroSensors() { 303 | 304 | } 305 | 306 | @Override 307 | protected void initDefaultCommand() { 308 | 309 | } 310 | } 311 | -------------------------------------------------------------------------------- /src/main/java/org/frcteam2910/c2019/autonomous/AutonomousSelector.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.autonomous; 2 | 3 | import edu.wpi.first.networktables.NetworkTableEntry; 4 | import edu.wpi.first.wpilibj.command.Command; 5 | import edu.wpi.first.wpilibj.command.CommandGroup; 6 | import edu.wpi.first.wpilibj.command.InstantCommand; 7 | import edu.wpi.first.wpilibj.command.WaitCommand; 8 | import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; 9 | import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; 10 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 11 | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; 12 | import org.frcteam2910.c2019.Robot; 13 | import org.frcteam2910.c2019.commands.DoTheThingCommand; 14 | import org.frcteam2910.c2019.commands.FollowTrajectoryCommand; 15 | import org.frcteam2910.c2019.commands.ImprovedVisionPlaceCommand; 16 | import org.frcteam2910.c2019.commands.SetArmAngleCommand; 17 | import org.frcteam2910.c2019.subsystems.CargoArmSubsystem; 18 | import org.frcteam2910.c2019.subsystems.CargoGrabberSubsystem; 19 | import org.frcteam2910.c2019.subsystems.DrivetrainSubsystem; 20 | import org.frcteam2910.common.control.ITrajectoryConstraint; 21 | import org.frcteam2910.common.control.MaxAccelerationConstraint; 22 | import org.frcteam2910.common.control.MaxVelocityConstraint; 23 | import org.frcteam2910.common.math.MathUtils; 24 | import org.frcteam2910.common.math.Rotation2; 25 | import org.frcteam2910.common.math.Vector2; 26 | import org.frcteam2910.common.util.Side; 27 | 28 | import java.util.LinkedList; 29 | import java.util.Queue; 30 | 31 | public class AutonomousSelector { 32 | private final AutonomousTrajectories trajectories; 33 | 34 | private static SendableChooser sideChooser; 35 | private static SendableChooser orientationChooser; 36 | private static SendableChooser autonomousModeChooser; 37 | private static NetworkTableEntry onHab2Entry; 38 | private static NetworkTableEntry placeThirdPanelEntry; 39 | private static NetworkTableEntry placeFourthPanelEntry; 40 | private static NetworkTableEntry rocketAutoEntry; 41 | 42 | private Queue hybridCommandQueue = new LinkedList<>(); 43 | 44 | static { 45 | ShuffleboardTab sandstormTab = Shuffleboard.getTab("Sandstorm settings"); 46 | 47 | sideChooser = new SendableChooser<>(); 48 | sideChooser.addOption("Left", Side.LEFT); 49 | sideChooser.setDefaultOption("Right", Side.RIGHT); 50 | sandstormTab.add("Starting Side", sideChooser); 51 | 52 | orientationChooser = new SendableChooser<>(); 53 | orientationChooser.setDefaultOption("Forward", Rotation2.ZERO); 54 | orientationChooser.addOption("Backwards", Rotation2.fromDegrees(180.0)); 55 | orientationChooser.addOption("Left", Rotation2.fromDegrees(90.0)); 56 | orientationChooser.addOption("Right", Rotation2.fromDegrees(270.0)); 57 | sandstormTab.add("Starting Orientation", orientationChooser); 58 | 59 | autonomousModeChooser = new SendableChooser<>(); 60 | autonomousModeChooser.setDefaultOption("Driven", AutonomousMode.DRIVEN); 61 | autonomousModeChooser.addOption("Hybrid", AutonomousMode.HYBRID); 62 | autonomousModeChooser.addOption("Autonomous", AutonomousMode.AUTONOMOUS); 63 | sandstormTab.add("Mode", autonomousModeChooser); 64 | 65 | onHab2Entry = sandstormTab.add("On HAB 2", false) 66 | .withWidget(BuiltInWidgets.kToggleButton) 67 | .getEntry(); 68 | placeThirdPanelEntry = sandstormTab.add("Place 3rd Hatch", false) 69 | .withWidget(BuiltInWidgets.kToggleButton) 70 | .getEntry(); 71 | placeFourthPanelEntry = sandstormTab.add("Place 4th Hatch", false) 72 | .withWidget(BuiltInWidgets.kToggleButton) 73 | .getEntry(); 74 | rocketAutoEntry = sandstormTab.add("Rocket Auto", false) 75 | .withWidget(BuiltInWidgets.kToggleButton) 76 | .getEntry(); 77 | } 78 | 79 | public AutonomousSelector(AutonomousTrajectories trajectories) { 80 | this.trajectories = trajectories; 81 | } 82 | 83 | private Command getFillRocketBottomCommand() { 84 | // First leave the hab and drive to the far rocket 85 | AutonomousMode mode = autonomousModeChooser.getSelected(); 86 | Rotation2 startingOrientation = orientationChooser.getSelected(); 87 | Side startingSide = sideChooser.getSelected(); 88 | boolean onHab2 = onHab2Entry.getBoolean(false); 89 | 90 | CommandGroup group = new CommandGroup(); 91 | 92 | if (onHab2) { 93 | group.addSequential(new FollowTrajectoryCommand( 94 | trajectories.getHab2ToRocketFarTrajectory(startingSide) 95 | )); 96 | } else { 97 | group.addSequential(new FollowTrajectoryCommand( 98 | trajectories.getHab1ToRocketFarTrajectory(startingSide) 99 | )); 100 | } 101 | 102 | group.addSequential(new DoTheThingCommand(true, false)); 103 | 104 | // Go back to the loading station and grab the next hatch 105 | group.addSequential(new FollowTrajectoryCommand( 106 | trajectories.getRocketFarToLoadingStationTrajectory(startingSide) 107 | )); 108 | group.addSequential(new DoTheThingCommand(true, false)); 109 | 110 | // Go to the near rocket and place 111 | group.addSequential(new FollowTrajectoryCommand( 112 | trajectories.getLoadingStationToRocketNearTrajectory(startingSide) 113 | )); 114 | group.addSequential(new DoTheThingCommand(true, false)); 115 | 116 | // Put the arm down and start intaking, drive to the depot 117 | group.addParallel(new SetArmAngleCommand(CargoArmSubsystem.BOTTOM_ANGLE)); 118 | group.addSequential(new FollowTrajectoryCommand( 119 | trajectories.getRocketNearToDepotTrajectory(startingSide) 120 | )); 121 | 122 | // Put the arm up and drive to the rocket cargo bay 123 | group.addParallel(new SetArmAngleCommand(CargoArmSubsystem.ROCKET_SCORE_ANGLE)); 124 | group.addSequential(new FollowTrajectoryCommand( 125 | trajectories.getDepotToRocketCargoTrajectory(startingSide) 126 | )); 127 | 128 | return group; 129 | } 130 | 131 | public Command getCommand() { 132 | AutonomousMode mode = autonomousModeChooser.getSelected(); 133 | Rotation2 startingOrientation = orientationChooser.getSelected(); 134 | Side startingSide = sideChooser.getSelected(); 135 | boolean onHab2 = onHab2Entry.getBoolean(false); 136 | 137 | CommandGroup group = new CommandGroup(); 138 | group.setRunWhenDisabled(true); 139 | 140 | // Set the gyro angle to the correct starting angle 141 | group.addSequential(new InstantCommand(() -> { 142 | DrivetrainSubsystem.getInstance().getGyroscope().setAdjustmentAngle( 143 | DrivetrainSubsystem.getInstance().getGyroscope().getUnadjustedAngle().rotateBy(startingOrientation) 144 | ); 145 | })); 146 | 147 | // If we want to manually drive the robot, return now. 148 | if (mode == AutonomousMode.DRIVEN) { 149 | return group; 150 | } 151 | 152 | group.addParallel(new SetArmAngleCommand(CargoArmSubsystem.VISION_TARGET_ANGLE)); 153 | 154 | if (rocketAutoEntry.getBoolean(false)) { 155 | group.addSequential(getFillRocketBottomCommand()); 156 | return group; 157 | } 158 | 159 | // Drive to the first target 160 | // If we are on hab 2, leave hab 2 in a (semi) repeatable manner 161 | if (onHab2) { 162 | group.addSequential(new FollowTrajectoryCommand(trajectories.getHab2ToCargoSideNearTrajectory(startingSide))); 163 | } else { 164 | group.addSequential(new FollowTrajectoryCommand(trajectories.getHab1ToCargoSideNearTrajectory(startingSide))); 165 | } 166 | // Enqueue the next trajectories 167 | hybridCommandQueue.clear(); 168 | // First we want to go to the loading station 169 | CommandGroup loadingStationPickup1 = new CommandGroup(); 170 | loadingStationPickup1.setRunWhenDisabled(true); 171 | loadingStationPickup1.addSequential(new FollowTrajectoryCommand(trajectories.getCargoSideNearToLoadingStationTrajectory(startingSide))); 172 | if (mode == AutonomousMode.AUTONOMOUS) { 173 | group.addSequential(new DoTheThingCommand(false, false)); 174 | group.addSequential(loadingStationPickup1); 175 | group.addSequential(new DoTheThingCommand(false, false)); 176 | } else { 177 | hybridCommandQueue.add(loadingStationPickup1); 178 | } 179 | // Next we want to go to the mid cargo ship 180 | CommandGroup cargoMidPlace = new CommandGroup(); 181 | cargoMidPlace.setRunWhenDisabled(true); 182 | cargoMidPlace.addSequential(new FollowTrajectoryCommand(trajectories.getLoadingStationToCargoSideMidTrajectory(startingSide))); 183 | if (mode == AutonomousMode.AUTONOMOUS) { 184 | group.addSequential(cargoMidPlace); 185 | group.addSequential(new DoTheThingCommand(false, false)); 186 | } else { 187 | hybridCommandQueue.add(cargoMidPlace); 188 | } 189 | // Finally, drive back to the loading station 190 | Command loadingStationPickup2 = new FollowTrajectoryCommand(trajectories.getCargoSideMidToLoadingStationTrajectory(startingSide)); 191 | loadingStationPickup2.setRunWhenDisabled(true); 192 | if (mode == AutonomousMode.AUTONOMOUS) { 193 | group.addSequential(loadingStationPickup2); 194 | } else { 195 | hybridCommandQueue.add(loadingStationPickup2); 196 | } 197 | 198 | // Place a 3rd hatch panel (Only will work if a hatch can be loaded in sandstorm.) 199 | if (placeThirdPanelEntry.getBoolean(false)) { 200 | Command placeThirdPanel = new FollowTrajectoryCommand(trajectories.getLoadingStationToRocketNearTrajectory(startingSide)); 201 | placeThirdPanel.setRunWhenDisabled(true); 202 | if (mode == AutonomousMode.AUTONOMOUS) { 203 | group.addSequential(new Command() { 204 | @Override 205 | protected boolean isFinished() { 206 | return Robot.getOi().primaryController.getLeftJoystickButton().get(); 207 | } 208 | }); 209 | group.addSequential(new DoTheThingCommand(false, false)); 210 | group.addSequential(placeThirdPanel); 211 | group.addSequential(new DoTheThingCommand(true, false)); 212 | } else { 213 | hybridCommandQueue.add(placeThirdPanel); 214 | } 215 | 216 | // Pickup a 4th hatch because why not 217 | if (placeFourthPanelEntry.getBoolean(false)) { 218 | Command pickupFourthPanel = new FollowTrajectoryCommand(trajectories.getRocketNearToLoadingStationTrajectory(startingSide)); 219 | pickupFourthPanel.setRunWhenDisabled(true); 220 | if (mode == AutonomousMode.AUTONOMOUS) { 221 | group.addSequential(pickupFourthPanel); 222 | group.addSequential(new DoTheThingCommand(false, false)); 223 | } else { 224 | hybridCommandQueue.add(pickupFourthPanel); 225 | } 226 | 227 | Command placeFourthPanel = new FollowTrajectoryCommand(trajectories.getLoadingStationToRocketFarTrajectory(startingSide)); 228 | placeFourthPanel.setRunWhenDisabled(true); 229 | if (mode == AutonomousMode.AUTONOMOUS) { 230 | group.addSequential(placeFourthPanel); 231 | group.addSequential(new DoTheThingCommand(true, true)); 232 | } else { 233 | hybridCommandQueue.add(placeFourthPanel); 234 | } 235 | } 236 | } 237 | 238 | return group; 239 | } 240 | 241 | public Queue getHybridQueue() { 242 | return hybridCommandQueue; 243 | } 244 | 245 | private enum AutonomousMode { 246 | DRIVEN, 247 | HYBRID, 248 | AUTONOMOUS 249 | } 250 | } 251 | -------------------------------------------------------------------------------- /vision-coprocessor/src/main/java/org/frcteam2910/c2019/vision/VisionTargetingPipeline.java: -------------------------------------------------------------------------------- 1 | package org.frcteam2910.c2019.vision; 2 | 3 | import edu.wpi.first.networktables.NetworkTable; 4 | import org.frcteam2910.c2019.vision.drivers.Limelight; 5 | import org.frcteam2910.common.math.RigidTransform2; 6 | import org.frcteam2910.common.math.Rotation2; 7 | import org.frcteam2910.common.math.Vector2; 8 | import org.opencv.core.*; 9 | 10 | import java.util.ArrayList; 11 | import java.util.Collections; 12 | 13 | import static org.opencv.calib3d.Calib3d.Rodrigues; 14 | import static org.opencv.calib3d.Calib3d.solvePnP; 15 | import static org.opencv.core.Core.PCACompute2; 16 | 17 | public class VisionTargetingPipeline { 18 | private MatOfPoint3f objectPoints; 19 | private int numberOfPoints; 20 | private MatOfDouble distortionCoefficients; 21 | private Mat cameraMatrix; 22 | 23 | private Point horizontal = new Point(1, 0); 24 | private Limelight limelight; 25 | 26 | /** 27 | * Constructor for the pipeline 28 | * @param objp Object points for the target 29 | * @param dist Distortion Coefficients for the camera 30 | * @param mtx Camera Matrix for the camera 31 | */ 32 | public VisionTargetingPipeline(Limelight limelight, double[][] objp, double[] dist, double[][] mtx) { 33 | this.limelight = limelight; 34 | 35 | objectPoints = doubleArrayToMatOfPoint3f(objp); 36 | numberOfPoints = objp.length; 37 | distortionCoefficients = new MatOfDouble(dist); 38 | cameraMatrix = doubleArrayToMat(mtx); 39 | } 40 | 41 | /** 42 | * Finds how far away the target is and its rotation with respect to the robot 43 | * @return Displacement left/right and forward/back as a Vector2 44 | * @throws Exception when there is no target 45 | */ 46 | public RigidTransform2 getTranslation() throws Exception { 47 | double[][] points = new double[numberOfPoints][2]; 48 | limelight.getCorners(numberOfPoints, points); 49 | if (limelight.hasTarget()) { 50 | Mat corners = doubleArrayToMat(points); 51 | 52 | // Get our principal axes 53 | Mat midPoint = new Mat(); 54 | Mat x = new Mat(1, 2, CvType.CV_32F); 55 | Mat y = new Mat(1, 2, CvType.CV_32F); 56 | double skew = getPrincipalAxes(corners, midPoint, x, y); 57 | 58 | // Sort the image points 59 | sortImgPts(corners, skew, midPoint); 60 | 61 | // solvePnP, get our distance and angle to target 62 | Mat rotationVectors = new Mat(); 63 | Mat translationVectors = new Mat(); 64 | solvePnP(objectPoints, matToMatOfPoint2f(corners), cameraMatrix, distortionCoefficients, rotationVectors, translationVectors); 65 | skew = getAngleToTarget(rotationVectors); 66 | 67 | // Prepare and return the RigidTransformation2 68 | Vector2 translation = new Vector2(translationVectors.get(0, 0)[0], translationVectors.get(2, 0)[0]); 69 | Rotation2 rotation = Rotation2.fromRadians(skew); 70 | return new RigidTransform2(translation, rotation); 71 | } 72 | throw new Exception("Target not available"); 73 | } 74 | 75 | /** 76 | * Gets the principle axes and midpoint of the given data points 77 | * @param corners The image points obtained from the target 78 | * @param mean The origin of the target in pixel coordinates 79 | * @param x The right principal axis 80 | * @param y The up principal axis 81 | * @return Returns the angle the principle axes are rotated at 82 | */ 83 | private double getPrincipalAxes(Mat corners, Mat mean, Mat x, Mat y) { 84 | Mat eigenVectors = new Mat(); 85 | Mat eigenValues = new Mat(); 86 | PCACompute2(corners, mean, eigenVectors, eigenValues); 87 | double[] right = {eigenVectors.get(0, 0)[0], eigenVectors.get(1, 0)[0]}; 88 | x.put(0, 0, right); 89 | double[] up = {eigenVectors.get(0, 1)[0], eigenVectors.get(1, 1)[0]}; 90 | y.put(0, 0, up); 91 | return getAngle(horizontal, matToPoint(x), false); 92 | } 93 | 94 | /** 95 | * Sorts the given image points. Primarily sorts by angle from least to greatest, but will sort by comparing the x components of the points if the angles are too close. 96 | * @param corners The image points, an input/output parameter 97 | * @param rotation The angle that the target is rotated at. @see getPrincipalAxes() 98 | * @param midPoint The origin of the target in pixel coordinates 99 | */ 100 | private void sortImgPts(Mat corners, double rotation, Mat midPoint) { 101 | double[][] imgpts = matToDoubleArray(corners); 102 | ArrayList points = new ArrayList(); 103 | for (int i = 0; i < numberOfPoints; i++) { 104 | // Prepare the point 105 | Point temp = new Point(imgpts[i][0] - midPoint.get(0, 0)[0], midPoint.get(0, 1)[0] - imgpts[i][1]); 106 | rotatePoint(temp, -1 * rotation); 107 | norm(temp); 108 | 109 | // Get the angle 110 | double angle = getAngle(horizontal, temp, true); 111 | ImagePoint2019 pt = new ImagePoint2019(angle, new Point(imgpts[i][0], imgpts[i][1])); 112 | points.add(pt); 113 | } 114 | // Now sort the arrays 115 | Collections.sort(points); 116 | 117 | // Lastly put modify the original corners matrix 118 | for (int i = 0; i < numberOfPoints; i++) { 119 | corners.put(i, 0, pointToDoubleArray(points.get(i).getPoint())); 120 | } 121 | } 122 | 123 | /** 124 | * This returns the angle to the target from the rotation vectors which was returned by solvePnP() 125 | * @param rotationVectors The rotation vectors calculated from solvePnP() 126 | * @return returns our angle to the target 127 | */ 128 | private double getAngleToTarget(Mat rotationVectors) { 129 | Mat dst = new Mat(); 130 | Rodrigues(rotationVectors, dst); 131 | dst = dst.row(2); 132 | Point pt = new Point(dst.get(0, 0)[0], dst.get(0, 2)[0]); 133 | norm(pt); 134 | double angle = Math.PI - Math.acos(pt.dot(new Point(0, 1))); 135 | 136 | Point3 crossProduct = new Point3(); 137 | crossProduct(new Point3(0, 0, 1), matToPoint3(dst), crossProduct); 138 | if (crossProduct.y < 0.0) { 139 | angle *= -1; 140 | } 141 | return angle; 142 | } 143 | 144 | /** 145 | * Gets the angle between two vectors 146 | * @param a The first vector 147 | * @param b The second vector 148 | * @param mode Switches between returning a angle from +/- 0-180 degrees or from 0-360 degrees. This only works given the right principal axis of the set of points this vector came from is is (1, 0). In other words, this will only work if Vector a is (1, 0). 149 | * @return Returns the angle between vector a and b 150 | */ 151 | private static double getAngle(Point a, Point b, boolean mode) { 152 | double rotation = Math.acos(a.dot(b) / getLength(a) * getLength(b)); 153 | if (b.y < 0) { 154 | if (mode) { 155 | rotation = (2 * Math.PI) - rotation; 156 | } else { 157 | rotation *= -1; 158 | } 159 | } 160 | return rotation; 161 | } 162 | 163 | /** 164 | * Rotates a point around (0, 0) 165 | * @param point The point to rotate 166 | * @param angle The angle to rotate it 167 | */ 168 | private static void rotatePoint(Point point, double angle) { 169 | double[] result = new double[2]; 170 | result[0] = (point.x * Math.cos(angle)) - (point.y * Math.sin(angle)); 171 | result[1] = (point.y * Math.cos(angle)) + (point.x * Math.sin(angle)); 172 | point.set(result); 173 | } 174 | 175 | /** 176 | * Returns the length of a vector 177 | * @param pt The vector to get the length of 178 | * @return The length of the vector as a double 179 | */ 180 | private static double getLength(Point pt) { 181 | return Math.sqrt(Math.pow(pt.x, 2) + Math.pow(pt.y, 2)); 182 | } 183 | 184 | /** 185 | * Normalizes a vector 186 | * @param pt The vector to normalize 187 | */ 188 | private static void norm(Point pt) { 189 | double length = getLength(pt); 190 | pt.x/=length; 191 | pt.y/=length; 192 | } 193 | 194 | /** 195 | * Returns the cross product of two vectors. Remember the resulting vector differs depending on the order of the operation 196 | * @param a Vector A 197 | * @param b Vector B 198 | * @param result A vector where the result will be stored 199 | */ 200 | private static void crossProduct(Point3 a, Point3 b, Point3 result) { 201 | result.x = a.y * b.z - a.z * b.y; 202 | result.y = a.z * b.x - a.x * b.z; 203 | result.z = a.x * b.y - a.y * b.x; 204 | } 205 | 206 | /** 207 | * Converts a matrix to a point. The matrix, of course, must represent a 2D point 208 | * @param mat The matrix to convert 209 | * @return The Point representation of the Matrix 210 | */ 211 | private static Point matToPoint(Mat mat) { 212 | Point result = new Point(); 213 | result.x = mat.get(0, 0)[0]; 214 | result.y = mat.get(0, 1)[0]; 215 | return result; 216 | } 217 | 218 | /** 219 | * Converts a matrix to a point. The matrix, of course, must represent a 3D point 220 | * @param mat The matrix to convert 221 | * @return The Point3 representation of the matrix 222 | */ 223 | private static Point3 matToPoint3(Mat mat) { 224 | Point3 result = new Point3(); 225 | result.x = mat.get(0, 0)[0]; 226 | result.y = mat.get(0, 1)[0]; 227 | result.z = mat.get(0, 2)[0]; 228 | return result; 229 | } 230 | 231 | /** 232 | * Converts a 2 dimensional double array to a matrix 233 | * @param data The 2 dimensional double array to convert 234 | * @return The Matrix representation of the 2 dimensional array 235 | */ 236 | private static Mat doubleArrayToMat(double[][] data) { 237 | Mat result = new Mat(data.length, data[0].length, CvType.CV_32F); 238 | for (int x = 0; x < data.length; x++) { 239 | result.put(x, 0, data[x]); 240 | } 241 | return result; 242 | } 243 | 244 | /** 245 | * Converts a matrix to a 2 dimensional double array 246 | * @param data The matrix to convert 247 | * @return The 2 dimensional double array of the Matrix 248 | */ 249 | private static double[][] matToDoubleArray(Mat data) { 250 | double[][] result = new double[(int)data.size().height][2]; 251 | for (int i = 0; i < data.size().height; i++) { 252 | result[i][0] = data.get(i, 0)[0]; 253 | result[i][1] = data.get(i, 1)[0]; 254 | } 255 | return result; 256 | } 257 | 258 | /** 259 | * Converts a 2D point to a double array 260 | * @param pt The point to convert 261 | * @return The double array representation of the point 262 | */ 263 | private static double[] pointToDoubleArray(Point pt) { 264 | double[] result = new double[2]; 265 | result[0] = pt.x; 266 | result[1] = pt.y; 267 | return result; 268 | } 269 | 270 | /** 271 | * Converts a matrix to a Matrix of 2D Points 272 | * @param mat The matrix to convert 273 | * @return The Matrix of 2D points 274 | */ 275 | private static MatOfPoint2f matToMatOfPoint2f(Mat mat) { 276 | ArrayList points = new ArrayList(); 277 | for (int i = 0; i < mat.size().height; i++) { 278 | points.add(new Point(mat.get(i, 0)[0], mat.get(i, 1)[0])); 279 | } 280 | return new MatOfPoint2f(points.toArray(new Point[0])); 281 | } 282 | 283 | // New undocumented things below 284 | 285 | /** 286 | * Converts a 2 dimensional double array to a MatOfPoint2f 287 | * @param corners The 2 dimensional double array to be converted 288 | * @return returns a MatOfPoint2f 289 | */ 290 | private static MatOfPoint2f doubleArrayToMatOfPoint2f(double[][] corners) { 291 | ArrayList points = new ArrayList(); 292 | for (int i = 0; i < 8; i++) { 293 | points.add(new Point(corners[i][0], corners[i][1])); 294 | } 295 | return new MatOfPoint2f(points.toArray(new Point[0])); 296 | } 297 | 298 | /** 299 | * Converts a 2 dimensional double array into a MatOfPoint3f 300 | * @param corners The 2 dimensional double array to be converted 301 | * @return returns a MatOfPoint3f 302 | */ 303 | private static MatOfPoint3f doubleArrayToMatOfPoint3f(double[][] corners) { 304 | ArrayList points = new ArrayList(); 305 | for (int i = 0; i < 8; i++) { 306 | points.add(new Point3(corners[i][0], corners[i][1], 0)); 307 | } 308 | return new MatOfPoint3f(points.toArray(new Point3[0])); 309 | } 310 | } --------------------------------------------------------------------------------