-
Notifications
You must be signed in to change notification settings - Fork 4
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
f42bf5f
commit 12a2e7d
Showing
4 changed files
with
258 additions
and
67 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
79 changes: 79 additions & 0 deletions
79
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Blinker.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,79 @@ | ||
| package org.firstinspires.ftc.teamcode.util; | ||
|
|
||
| import com.qualcomm.hardware.lynx.LynxModule; | ||
| import com.qualcomm.robotcore.hardware.Blinker.Step; | ||
|
|
||
| import java.util.ArrayList; | ||
| import java.util.List; | ||
| import java.util.concurrent.TimeUnit; | ||
|
|
||
| /** | ||
| * Control the status LED built-in to REV Control Hubs and REV Expansion Hubs | ||
| */ | ||
| public class Blinker { | ||
| private final LynxModule module; | ||
|
|
||
| /** | ||
| * Create a Blinker wrapper | ||
| * @param module the REV Hub to use | ||
| */ | ||
| public Blinker(LynxModule module) { | ||
| this.module = module; | ||
| } | ||
|
|
||
| /** | ||
| * @param pattern the pattern to display | ||
| * @see Pattern | ||
| */ | ||
| public void displayPattern(Pattern pattern) { | ||
| module.setPattern(pattern.steps); | ||
| } | ||
|
|
||
| /** | ||
| * Display the idle pattern | ||
| * <br/> | ||
| * <ul> | ||
| * <li>Expansion Hub</li> | ||
| * <ul> | ||
| * <li>GREEN for 5 seconds</li> | ||
| * <li>OFF for .5 seconds</li> | ||
| * <li>BLUE flashs corresponding to the ID (.5 ea. followed by .5 OFF)</li> | ||
| * </ul> | ||
| * <li>Control Hub: GREEN</li> | ||
| * </ul> | ||
| */ | ||
| public void idle() { | ||
| module.setPattern(new LynxModule.CountModuleAddressBlinkerPolicy().getIdlePattern(module)); | ||
| } | ||
|
|
||
| /** | ||
| * Display a pattern that makes the module easily identifiable | ||
| * <br/> | ||
| * <ul> | ||
| * <li>CYAN for 150ms</li> | ||
| * <li>OFF for 75ms</li> | ||
| * <li>MAGENTA for 150ms</li> | ||
| * <li>OFF for 75ms</li> | ||
| * </ul> | ||
| */ | ||
| public void identify() { | ||
| module.setPattern(new LynxModule.BreathingBlinkerPolicy().getVisuallyIdentifyPattern(module)); | ||
| } | ||
|
|
||
| public static class Pattern { | ||
| public static final int MAX_STEPS = 16; | ||
|
|
||
| private List<com.qualcomm.robotcore.hardware.Blinker.Step> steps = new ArrayList<>(); | ||
|
|
||
| /** | ||
| * Add a step to the pattern. You can have a maximum of 16 steps. | ||
| * @param color the color to display (in Android's int color format) | ||
| * @param duration the duration in ms for this step to last | ||
| * @see android.graphics.Color | ||
| */ | ||
| public Pattern addStep(int color, int duration) { | ||
| steps.add(new Step(color, duration, TimeUnit.MILLISECONDS)); | ||
| return this; | ||
| } | ||
| } | ||
| } |
30 changes: 30 additions & 0 deletions
30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagCamera.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,30 @@ | ||
| package org.firstinspires.ftc.teamcode.vision; | ||
|
|
||
| import com.acmerobotics.roadrunner.geometry.Pose2d; | ||
|
|
||
| import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; | ||
|
|
||
| public class AprilTagCamera { | ||
| public WebcamName webcamName; | ||
| private final double offset; | ||
| private final double angle; | ||
|
|
||
| public AprilTagCamera(WebcamName webcamName, double offset, double angle) { | ||
| this.webcamName = webcamName; | ||
| this.offset = offset; | ||
| this.angle = angle; | ||
| } | ||
|
|
||
| public Pose2d translatePose(Pose2d inputPose) { | ||
| double offsetX = offset * Math.cos(inputPose.getHeading()); | ||
| double offsetY = offset * Math.sin(inputPose.getHeading()); | ||
|
|
||
| android.util.Log.i("AprilTag Test", "Camera offset: " + offsetX + ", " + offsetY); | ||
|
|
||
| double robotX = inputPose.getX() - offsetX; | ||
| double robotY = inputPose.getY() - offsetY; | ||
| double robotHeading = inputPose.getHeading() - angle; | ||
|
|
||
| return new Pose2d(robotX, robotY, robotHeading); | ||
| } | ||
| } |
Oops, something went wrong.