From 3de740290ca0e8d1f172e38d5c623f4b6209aa21 Mon Sep 17 00:00:00 2001 From: Mixmix00 Date: Wed, 20 Mar 2024 11:15:08 -0700 Subject: [PATCH 1/3] trap auton code --- .../team766/ViSIONbase/ScoringPosition.java | 4 ++- .../reva/VisionUtil/ScoringPositions.java | 10 ++++-- .../reva/procedures/DriveToAndScoreAt.java | 36 ++++++++++++------- .../com/team766/robot/swerveandshoot/OI.java | 8 ++--- .../swerveandshoot/ScoringPositions.java | 4 +-- 5 files changed, 40 insertions(+), 22 deletions(-) diff --git a/src/main/java/com/team766/ViSIONbase/ScoringPosition.java b/src/main/java/com/team766/ViSIONbase/ScoringPosition.java index 76ddff66b..b3befe4cc 100644 --- a/src/main/java/com/team766/ViSIONbase/ScoringPosition.java +++ b/src/main/java/com/team766/ViSIONbase/ScoringPosition.java @@ -2,6 +2,7 @@ public class ScoringPosition { public final double speed, angle, x_position, y_position, swerve_angle; + public final int tagId; /* * @param speed the speed to set the shooter, in motor shaft rps. @@ -11,11 +12,12 @@ public class ScoringPosition { * @param swerve_angle the angle the robot should be facing relative to the AprilTag (where 0 degrees is flush) */ public ScoringPosition( - double speed, double angle, double x_position, double y_position, double swerve_angle) { + double speed, double angle, double x_position, double y_position, double swerve_angle, int tagId) { this.speed = speed; this.angle = angle; this.x_position = x_position; this.y_position = y_position; this.swerve_angle = swerve_angle; + this.tagId = tagId; } } diff --git a/src/main/java/com/team766/robot/reva/VisionUtil/ScoringPositions.java b/src/main/java/com/team766/robot/reva/VisionUtil/ScoringPositions.java index 00e2e2358..2f2bd3b8c 100644 --- a/src/main/java/com/team766/robot/reva/VisionUtil/ScoringPositions.java +++ b/src/main/java/com/team766/robot/reva/VisionUtil/ScoringPositions.java @@ -3,6 +3,12 @@ import com.team766.ViSIONbase.ScoringPosition; public class ScoringPositions { - public static final ScoringPosition MAKER_SPACE_1L = new ScoringPosition(0, 0, 2.88, -0.92, 0); - public static final ScoringPosition MAKER_SPACE_1R = new ScoringPosition(0, 0, 2.9, 0.80, 0); + public static final ScoringPosition RED_ALLIANCE_STAGE_LEFT = new ScoringPosition(0, 0, 0, 0, 0, 11); + public static final ScoringPosition RED_ALLIANCE_STAGE_RIGHT = new ScoringPosition(0, 0, 0, 0, 0, 12); + public static final ScoringPosition RED_ALLIANCE_CENTERSTAGE = new ScoringPosition(0, 0, 0, 0, 0, 13); + + public static final ScoringPosition BLUE_ALLIANCE_CENTERSTAGE = new ScoringPosition(0, 0, 0, 0, 0, 14); + public static final ScoringPosition BLUE_ALLIANCE_STAGE_LEFT = new ScoringPosition(0, 0, 0, 0, 0, 15); + public static final ScoringPosition BLUE_ALLIANCE_STAGE_RIGHT = new ScoringPosition(0, 0, 0, 0, 0, 16); } + diff --git a/src/main/java/com/team766/robot/reva/procedures/DriveToAndScoreAt.java b/src/main/java/com/team766/robot/reva/procedures/DriveToAndScoreAt.java index d639a6765..8fe3109cf 100644 --- a/src/main/java/com/team766/robot/reva/procedures/DriveToAndScoreAt.java +++ b/src/main/java/com/team766/robot/reva/procedures/DriveToAndScoreAt.java @@ -16,9 +16,11 @@ public class DriveToAndScoreAt extends VisionPIDProcedure { private double lastY; private double timeLastSeen = -1; + private int tagId; public DriveToAndScoreAt(ScoringPosition score) { this.score = score; + tagId = score.tagId; } // button needs to be held down @@ -26,11 +28,15 @@ public void run(final Context context) { context.takeOwnership(Robot.drive); context.takeOwnership(Robot.intake); context.takeOwnership(Robot.shooter); + context.takeOwnership(Robot.shoulder); yPID.setSetpoint(score.y_position); xPID.setSetpoint(score.x_position); - while (Math.abs(xPID.getOutput()) + Math.abs(yPID.getOutput()) != 0) { + Robot.shoulder.rotate(score.angle); + + + while (Math.abs(xPID.getOutput()) + Math.abs(yPID.getOutput()) > 0.05) { context.yield(); Transform3d robotToTag; @@ -49,7 +55,7 @@ public void run(final Context context) { // TODO: Turn this into PID? // If it is more that four degrees off... - if (Math.abs(robotToTag.getRotation().getZ()) > 4) { + if (Math.abs(robotToTag.getRotation().getZ()) > 3) { if (robotToTag.getRotation().getZ() < 0) { turnConstant = -0.02; } else { @@ -77,6 +83,20 @@ public void run(final Context context) { } Robot.shooter.shoot(score.speed); + + context.waitFor(Robot.shoulder::isFinished); + context.waitFor(Robot.shooter::isCloseToExpectedSpeed); + + Robot.intake.runIntake(); + + context.waitForSeconds(1.5); + + Robot.intake.stop(); + + context.releaseOwnership(Robot.drive); + context.releaseOwnership(Robot.shooter); + context.releaseOwnership(Robot.intake); + context.releaseOwnership(Robot.shoulder); } /** @@ -92,16 +112,6 @@ public void run(final Context context) { private Transform3d getTransform3dOfRobotToTag() throws AprilTagGeneralCheckedException { GrayScaleCamera toUse = Robot.forwardApriltagCamera.getCamera(); - Transform3d robotToTag = toUse.getBestTargetTransform3d(toUse.getBestTrackedTarget()); - - int tagId = toUse.getTagIdOfBestTarget(); - - // this is the tag we will be using for testing in the time being. later we will need to set - // based on alliance color - if (tagId == 5) { - return robotToTag; - } - - throw new AprilTagGeneralCheckedException("Could not find tag with the correct tagId"); + return GrayScaleCamera.getBestTargetTransform3d(toUse.getTrackedTargetWithID(tagId)); } } diff --git a/src/main/java/com/team766/robot/swerveandshoot/OI.java b/src/main/java/com/team766/robot/swerveandshoot/OI.java index 694bf502e..0e9366040 100644 --- a/src/main/java/com/team766/robot/swerveandshoot/OI.java +++ b/src/main/java/com/team766/robot/swerveandshoot/OI.java @@ -63,8 +63,8 @@ public void run(final Context context) { */ if (joystick0.getButtonPressed(1)) { // Robot.speakerShooter.goToAndScore(SpeakerShooterPowerCalculator.makerSpace1R); - visionProcedure = - context.startAsync(new DriveToAndScoreAt(ScoringPositions.makerSpace1R)); + //visionProcedure = + //context.startAsync(new DriveToAndScoreAt(ScoringPositions.makerSpace1R)); } if (joystick0.getButtonReleased(1)) { @@ -77,8 +77,8 @@ public void run(final Context context) { */ if (joystick0.getButtonPressed(2)) { // Robot.speakerShooter.goToAndScore(SpeakerShooterPowerCalculator.makerSpace1R); - visionProcedure = - context.startAsync(new DriveToAndScoreAt(ScoringPositions.makerSpace1L)); + //visionProcedure = + //context.startAsync(new DriveToAndScoreAt(ScoringPositions.makerSpace1L)); } if (joystick0.getButtonReleased(2)) { diff --git a/src/main/java/com/team766/robot/swerveandshoot/ScoringPositions.java b/src/main/java/com/team766/robot/swerveandshoot/ScoringPositions.java index 961c4bc32..a31dd60f1 100644 --- a/src/main/java/com/team766/robot/swerveandshoot/ScoringPositions.java +++ b/src/main/java/com/team766/robot/swerveandshoot/ScoringPositions.java @@ -3,6 +3,6 @@ import com.team766.ViSIONbase.ScoringPosition; public class ScoringPositions { - public static ScoringPosition makerSpace1L = new ScoringPosition(0, 0, 2.88, -0.92, 0); - public static ScoringPosition makerSpace1R = new ScoringPosition(0, 0, 2.9, 0.80, 0); + //public static ScoringPosition makerSpace1L = new ScoringPosition(0, 0, 2.88, -0.92, 0); + //public static ScoringPosition makerSpace1R = new ScoringPosition(0, 0, 2.9, 0.80, 0); } From 8cdc43d3c49b944214cda3d69e99f01df7b93573 Mon Sep 17 00:00:00 2001 From: Mixmix00 Date: Thu, 28 Mar 2024 13:57:19 -0700 Subject: [PATCH 2/3] some more --- .../reva/VisionUtil/ScoringPositions.java | 12 +++++ .../robot/reva/procedures/ShootTrap.java | 44 +++++++++++++++++++ 2 files changed, 56 insertions(+) create mode 100644 src/main/java/com/team766/robot/reva/procedures/ShootTrap.java diff --git a/src/main/java/com/team766/robot/reva/VisionUtil/ScoringPositions.java b/src/main/java/com/team766/robot/reva/VisionUtil/ScoringPositions.java index 2f2bd3b8c..c87e061f1 100644 --- a/src/main/java/com/team766/robot/reva/VisionUtil/ScoringPositions.java +++ b/src/main/java/com/team766/robot/reva/VisionUtil/ScoringPositions.java @@ -1,5 +1,6 @@ package com.team766.robot.reva.VisionUtil; +import java.util.ArrayList; import com.team766.ViSIONbase.ScoringPosition; public class ScoringPositions { @@ -10,5 +11,16 @@ public class ScoringPositions { public static final ScoringPosition BLUE_ALLIANCE_CENTERSTAGE = new ScoringPosition(0, 0, 0, 0, 0, 14); public static final ScoringPosition BLUE_ALLIANCE_STAGE_LEFT = new ScoringPosition(0, 0, 0, 0, 0, 15); public static final ScoringPosition BLUE_ALLIANCE_STAGE_RIGHT = new ScoringPosition(0, 0, 0, 0, 0, 16); + + public static final ArrayList trapScoringPositions = new ArrayList(){ + { + add(RED_ALLIANCE_STAGE_LEFT); + add(RED_ALLIANCE_STAGE_RIGHT); + add(RED_ALLIANCE_CENTERSTAGE); + add(BLUE_ALLIANCE_CENTERSTAGE); + add(BLUE_ALLIANCE_STAGE_LEFT); + add(BLUE_ALLIANCE_STAGE_RIGHT); + } + }; } diff --git a/src/main/java/com/team766/robot/reva/procedures/ShootTrap.java b/src/main/java/com/team766/robot/reva/procedures/ShootTrap.java new file mode 100644 index 000000000..ba9f3024a --- /dev/null +++ b/src/main/java/com/team766/robot/reva/procedures/ShootTrap.java @@ -0,0 +1,44 @@ +package com.team766.robot.reva.procedures; + +import com.team766.robot.reva.Robot; +import com.team766.robot.reva.VisionUtil.VisionPIDProcedure; +import edu.wpi.first.math.geometry.Transform3d; +import com.team766.ViSIONbase.AprilTagGeneralCheckedException; +import com.team766.ViSIONbase.GrayScaleCamera; +import com.team766.ViSIONbase.ScoringPosition; +import com.team766.framework.Context; +import com.team766.logging.LoggerExceptionUtils; +import com.team766.robot.reva.VisionUtil.ScoringPositions; + +public class ShootTrap extends VisionPIDProcedure { + + int tagId; + + + public void run(Context context){ + + while (true){ + context.yield(); + try{ + tagId = getTagId(); + break; + } catch (AprilTagGeneralCheckedException e){ + continue; + } + } + + for(int i = 0; i < ScoringPositions.trapScoringPositions.size(); i++){ + if(ScoringPositions.trapScoringPositions.get(i).tagId == tagId){ + new DriveToAndScoreAt(ScoringPositions.trapScoringPositions.get(i)).run(context); + break; + } + } + + } + + private int getTagId() throws AprilTagGeneralCheckedException{ + GrayScaleCamera toUse = Robot.forwardApriltagCamera.getCamera(); + + return toUse.getTagIdOfBestTarget(); + } +} From b4f29bfdf76650f9be59d16f455f46560f77c9b7 Mon Sep 17 00:00:00 2001 From: Mixmix00 Date: Thu, 28 Mar 2024 14:01:21 -0700 Subject: [PATCH 3/3] lint --- .../team766/ViSIONbase/ScoringPosition.java | 7 +- .../reva/VisionUtil/ScoringPositions.java | 44 +++++++------ .../reva/procedures/DriveToAndScoreAt.java | 1 - .../robot/reva/procedures/ShootTrap.java | 65 +++++++++---------- .../com/team766/robot/swerveandshoot/OI.java | 8 +-- .../swerveandshoot/ScoringPositions.java | 6 +- 6 files changed, 67 insertions(+), 64 deletions(-) diff --git a/src/main/java/com/team766/ViSIONbase/ScoringPosition.java b/src/main/java/com/team766/ViSIONbase/ScoringPosition.java index b3befe4cc..6d2a4f7b4 100644 --- a/src/main/java/com/team766/ViSIONbase/ScoringPosition.java +++ b/src/main/java/com/team766/ViSIONbase/ScoringPosition.java @@ -12,7 +12,12 @@ public class ScoringPosition { * @param swerve_angle the angle the robot should be facing relative to the AprilTag (where 0 degrees is flush) */ public ScoringPosition( - double speed, double angle, double x_position, double y_position, double swerve_angle, int tagId) { + double speed, + double angle, + double x_position, + double y_position, + double swerve_angle, + int tagId) { this.speed = speed; this.angle = angle; this.x_position = x_position; diff --git a/src/main/java/com/team766/robot/reva/VisionUtil/ScoringPositions.java b/src/main/java/com/team766/robot/reva/VisionUtil/ScoringPositions.java index c87e061f1..948abc0c6 100644 --- a/src/main/java/com/team766/robot/reva/VisionUtil/ScoringPositions.java +++ b/src/main/java/com/team766/robot/reva/VisionUtil/ScoringPositions.java @@ -1,26 +1,32 @@ package com.team766.robot.reva.VisionUtil; -import java.util.ArrayList; import com.team766.ViSIONbase.ScoringPosition; +import java.util.ArrayList; public class ScoringPositions { - public static final ScoringPosition RED_ALLIANCE_STAGE_LEFT = new ScoringPosition(0, 0, 0, 0, 0, 11); - public static final ScoringPosition RED_ALLIANCE_STAGE_RIGHT = new ScoringPosition(0, 0, 0, 0, 0, 12); - public static final ScoringPosition RED_ALLIANCE_CENTERSTAGE = new ScoringPosition(0, 0, 0, 0, 0, 13); - - public static final ScoringPosition BLUE_ALLIANCE_CENTERSTAGE = new ScoringPosition(0, 0, 0, 0, 0, 14); - public static final ScoringPosition BLUE_ALLIANCE_STAGE_LEFT = new ScoringPosition(0, 0, 0, 0, 0, 15); - public static final ScoringPosition BLUE_ALLIANCE_STAGE_RIGHT = new ScoringPosition(0, 0, 0, 0, 0, 16); + public static final ScoringPosition RED_ALLIANCE_STAGE_LEFT = + new ScoringPosition(0, 0, 0, 0, 0, 11); + public static final ScoringPosition RED_ALLIANCE_STAGE_RIGHT = + new ScoringPosition(0, 0, 0, 0, 0, 12); + public static final ScoringPosition RED_ALLIANCE_CENTERSTAGE = + new ScoringPosition(0, 0, 0, 0, 0, 13); - public static final ArrayList trapScoringPositions = new ArrayList(){ - { - add(RED_ALLIANCE_STAGE_LEFT); - add(RED_ALLIANCE_STAGE_RIGHT); - add(RED_ALLIANCE_CENTERSTAGE); - add(BLUE_ALLIANCE_CENTERSTAGE); - add(BLUE_ALLIANCE_STAGE_LEFT); - add(BLUE_ALLIANCE_STAGE_RIGHT); - } - }; -} + public static final ScoringPosition BLUE_ALLIANCE_CENTERSTAGE = + new ScoringPosition(0, 0, 0, 0, 0, 14); + public static final ScoringPosition BLUE_ALLIANCE_STAGE_LEFT = + new ScoringPosition(0, 0, 0, 0, 0, 15); + public static final ScoringPosition BLUE_ALLIANCE_STAGE_RIGHT = + new ScoringPosition(0, 0, 0, 0, 0, 16); + public static final ArrayList trapScoringPositions = + new ArrayList() { + { + add(RED_ALLIANCE_STAGE_LEFT); + add(RED_ALLIANCE_STAGE_RIGHT); + add(RED_ALLIANCE_CENTERSTAGE); + add(BLUE_ALLIANCE_CENTERSTAGE); + add(BLUE_ALLIANCE_STAGE_LEFT); + add(BLUE_ALLIANCE_STAGE_RIGHT); + } + }; +} diff --git a/src/main/java/com/team766/robot/reva/procedures/DriveToAndScoreAt.java b/src/main/java/com/team766/robot/reva/procedures/DriveToAndScoreAt.java index 8fe3109cf..49f949ced 100644 --- a/src/main/java/com/team766/robot/reva/procedures/DriveToAndScoreAt.java +++ b/src/main/java/com/team766/robot/reva/procedures/DriveToAndScoreAt.java @@ -35,7 +35,6 @@ public void run(final Context context) { Robot.shoulder.rotate(score.angle); - while (Math.abs(xPID.getOutput()) + Math.abs(yPID.getOutput()) > 0.05) { context.yield(); diff --git a/src/main/java/com/team766/robot/reva/procedures/ShootTrap.java b/src/main/java/com/team766/robot/reva/procedures/ShootTrap.java index ba9f3024a..5bce1f92c 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ShootTrap.java +++ b/src/main/java/com/team766/robot/reva/procedures/ShootTrap.java @@ -1,44 +1,39 @@ package com.team766.robot.reva.procedures; -import com.team766.robot.reva.Robot; -import com.team766.robot.reva.VisionUtil.VisionPIDProcedure; -import edu.wpi.first.math.geometry.Transform3d; import com.team766.ViSIONbase.AprilTagGeneralCheckedException; import com.team766.ViSIONbase.GrayScaleCamera; -import com.team766.ViSIONbase.ScoringPosition; import com.team766.framework.Context; -import com.team766.logging.LoggerExceptionUtils; +import com.team766.robot.reva.Robot; import com.team766.robot.reva.VisionUtil.ScoringPositions; +import com.team766.robot.reva.VisionUtil.VisionPIDProcedure; public class ShootTrap extends VisionPIDProcedure { - - int tagId; - - - public void run(Context context){ - - while (true){ - context.yield(); - try{ - tagId = getTagId(); - break; - } catch (AprilTagGeneralCheckedException e){ - continue; - } - } - - for(int i = 0; i < ScoringPositions.trapScoringPositions.size(); i++){ - if(ScoringPositions.trapScoringPositions.get(i).tagId == tagId){ - new DriveToAndScoreAt(ScoringPositions.trapScoringPositions.get(i)).run(context); - break; - } - } - - } - - private int getTagId() throws AprilTagGeneralCheckedException{ - GrayScaleCamera toUse = Robot.forwardApriltagCamera.getCamera(); - - return toUse.getTagIdOfBestTarget(); - } + + int tagId; + + public void run(Context context) { + + while (true) { + context.yield(); + try { + tagId = getTagId(); + break; + } catch (AprilTagGeneralCheckedException e) { + continue; + } + } + + for (int i = 0; i < ScoringPositions.trapScoringPositions.size(); i++) { + if (ScoringPositions.trapScoringPositions.get(i).tagId == tagId) { + new DriveToAndScoreAt(ScoringPositions.trapScoringPositions.get(i)).run(context); + break; + } + } + } + + private int getTagId() throws AprilTagGeneralCheckedException { + GrayScaleCamera toUse = Robot.forwardApriltagCamera.getCamera(); + + return toUse.getTagIdOfBestTarget(); + } } diff --git a/src/main/java/com/team766/robot/swerveandshoot/OI.java b/src/main/java/com/team766/robot/swerveandshoot/OI.java index 0e9366040..d6a6b7dc5 100644 --- a/src/main/java/com/team766/robot/swerveandshoot/OI.java +++ b/src/main/java/com/team766/robot/swerveandshoot/OI.java @@ -63,8 +63,8 @@ public void run(final Context context) { */ if (joystick0.getButtonPressed(1)) { // Robot.speakerShooter.goToAndScore(SpeakerShooterPowerCalculator.makerSpace1R); - //visionProcedure = - //context.startAsync(new DriveToAndScoreAt(ScoringPositions.makerSpace1R)); + // visionProcedure = + // context.startAsync(new DriveToAndScoreAt(ScoringPositions.makerSpace1R)); } if (joystick0.getButtonReleased(1)) { @@ -77,8 +77,8 @@ public void run(final Context context) { */ if (joystick0.getButtonPressed(2)) { // Robot.speakerShooter.goToAndScore(SpeakerShooterPowerCalculator.makerSpace1R); - //visionProcedure = - //context.startAsync(new DriveToAndScoreAt(ScoringPositions.makerSpace1L)); + // visionProcedure = + // context.startAsync(new DriveToAndScoreAt(ScoringPositions.makerSpace1L)); } if (joystick0.getButtonReleased(2)) { diff --git a/src/main/java/com/team766/robot/swerveandshoot/ScoringPositions.java b/src/main/java/com/team766/robot/swerveandshoot/ScoringPositions.java index a31dd60f1..5f5322de1 100644 --- a/src/main/java/com/team766/robot/swerveandshoot/ScoringPositions.java +++ b/src/main/java/com/team766/robot/swerveandshoot/ScoringPositions.java @@ -1,8 +1,6 @@ package com.team766.robot.swerveandshoot; -import com.team766.ViSIONbase.ScoringPosition; - public class ScoringPositions { - //public static ScoringPosition makerSpace1L = new ScoringPosition(0, 0, 2.88, -0.92, 0); - //public static ScoringPosition makerSpace1R = new ScoringPosition(0, 0, 2.9, 0.80, 0); + // public static ScoringPosition makerSpace1L = new ScoringPosition(0, 0, 2.88, -0.92, 0); + // public static ScoringPosition makerSpace1R = new ScoringPosition(0, 0, 2.9, 0.80, 0); }