eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'bumblebee_dev' of https://github.com/IronEaglesRobotics/CenterStage into bumblebee_dev

This commit is contained in:
Thomas 2023-11-17 10:02:32 -06:00
parent 4301b8fbe8 94c9deae5b
commit acb43f0940
13 changed files with 229 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

148
TeamCode/src/main/java/opmodes/AutoBase.java Normal file
View File

@ -0,0 +1,148 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG;
import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Center;
import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Left;
import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Right;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import org.firstinspires.ftc.teamcode.vision.Detection;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.opencv.core.Point;
@Config
public abstract class AutoBase extends LinearOpMode {
public static int DEPOSIT_HEIGHT = 100;
protected Robot robot;
protected FtcDashboard dashboard;
protected Telemetry dashboardTelemetry;
protected CenterStageCommon.PropLocation propLocation;
@Override
public void runOpMode() {
this.robot = new Robot(hardwareMap, telemetry);
this.dashboard = FtcDashboard.getInstance();
this.dashboardTelemetry = dashboard.getTelemetry();
this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Blue);
this.robot.getDrive().setPoseEstimate(new Pose2d(-36, 63, Math.toRadians(-90)));
while(!isStarted() && !isStopRequested()) {
this.robot.update();
this.sleep(20);
}
setPropLocationIfVisible(Center, null);
TrajectorySequenceBuilder builder = this.robot.getDrive()
.trajectorySequenceBuilder(new Pose2d(-36, 63, Math.toRadians(-90)));
if (this.propLocation != CenterStageCommon.PropLocation.Center) {
builder.forward(5);
builder.turn(Math.toRadians(-33));
this.robot.getDrive().followTrajectorySequence(builder.build());
setPropLocationIfVisible(Right, Left);
return;
} else {
// Center
builder.lineToConstantHeading(new Vector2d(-36, 11));
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
this.robot.getClaw().openSync();
this.sleep(100);
this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX);
}
builder = this.robot.getDrive().trajectorySequenceBuilder(new Pose2d(-36, 11));
builder.lineToLinearHeading(new Pose2d(36, 11, 0));
builder.lineToLinearHeading(new Pose2d(36, 38, 0));
this.robot.getDrive().followTrajectorySequence(builder.build());
double distance = getDistanceToAprilTag();
builder = this.robot.getDrive().trajectorySequenceBuilder(new Pose2d(36, 38, 0));
builder.forward(distance - SCORING_DISTANCE_FROM_APRIL_TAG);
this.robot.getDrive().followTrajectorySequence(builder.build());
this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT);
this.robot.getGantry().armOut();
while(this.robot.getGantry().isIn()) {
this.robot.getGantry().update();
sleep(20);
}
this.robot.getGantry().deposit();
while(opModeIsActive()) {
this.robot.update();
AprilTagDetection aprilTagDetection = this.robot.getCamera().getAprilTag(2);
if (aprilTagDetection != null) {
Point center = aprilTagDetection.center;
this.dashboardTelemetry.addData("center", center);
this.dashboardTelemetry.addData("x", aprilTagDetection.ftcPose.x);
this.dashboardTelemetry.addData("y", aprilTagDetection.ftcPose.y);
this.dashboardTelemetry.addData("z", aprilTagDetection.ftcPose.z);
this.dashboardTelemetry.update();
}
sleep(20);
}
}
private double getDistanceToAprilTag() {
double minDistance = Double.MAX_VALUE;
for (int i = 0; i < 10; i++) {
AprilTagDetection aprilTagDetection = this.robot.getCamera().getAprilTag(2);
if (aprilTagDetection != null) {
if (aprilTagDetection.ftcPose.y < minDistance) {
minDistance = aprilTagDetection.ftcPose.y;
}
}
}
return minDistance;
}
protected static int getExpectedAprilTagId(CenterStageCommon.PropLocation propLocation) {
switch (propLocation) {
case Left:
return 1;
case Unknown:
case Center:
return 2;
case Right:
return 3;
}
return 2;
}
protected void setPropLocationIfVisible(CenterStageCommon.PropLocation ifVisible, CenterStageCommon.PropLocation ifNotVisible) {
float seenCount = 0;
float samples = 10;
for (int i = 0; i < samples; i++) {
Detection detection = this.robot.getCamera().getProp();
if (detection.isValid()) {
seenCount++;
}
}
if (seenCount / samples > 0.5) {
this.propLocation = ifVisible;
} else {
this.propLocation = ifNotVisible;
}
}
}

11
TeamCode/src/main/java/opmodes/LeftAuto.java Normal file
View File

@ -0,0 +1,11 @@
package opmodes;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@Autonomous(name = "Left Auto", preselectTeleOp = "MainTeleOp")
public class LeftAuto extends AutoBase {
@Override
public void runOpMode() {
super.runOpMode();
}
}

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java
View File

@ -21,9 +21,6 @@ import lombok.Getter;
import lombok.Setter;
public class Camera {
@Getter
@Setter
private CenterStageCommon.Alliance alliance;
private PropDetectionPipeline prop;
private AprilTagProcessor aprilTag;
private VisionPortal visionPortal;
@ -50,16 +47,17 @@ public class Camera {
}
public Detection getProp() {
if (!initialized || alliance == null) {
if (!initialized || getAlliance() == null) {
return INVALID_DETECTION;
}
switch (alliance) {
switch (getAlliance()) {
case Blue:
return this.prop.getBlue();
Detection blue = this.prop.getBlue();
return blue != null && blue.isValid() ? blue : INVALID_DETECTION;
case Red:
return this.prop.getRed();
Detection red = this.prop.getRed();
return red != null && red.isValid() ? red : INVALID_DETECTION;
}
return INVALID_DETECTION;
@ -67,7 +65,7 @@ public class Camera {
public CenterStageCommon.PropLocation getPropLocation() {
Detection prop = this.getProp();
if (!prop.isValid()) {
if (prop == null || !prop.isValid()) {
return CenterStageCommon.PropLocation.Unknown;
}
@ -93,4 +91,12 @@ public class Camera {
.findFirst()
.orElse(null);
}
public void setAlliance(CenterStageCommon.Alliance alliance) {
this.prop.setAlliance(alliance);
}
public CenterStageCommon.Alliance getAlliance() {
return this.prop != null ? this.prop.getAlliance() : null;
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java
View File

@ -41,6 +41,13 @@ public class Claw implements Updatable {
this.clawControllerTarget = CLAW_MAX;
}
public void openSync() {
this.clawControllerTarget = CLAW_MAX;
while (Math.abs(this.claw.getPosition() - CLAW_MAX) > 0.001) {
this.update();
}
}
public void close() {
this.clawControllerTarget = CLAW_MIN;
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java
View File

@ -53,7 +53,7 @@ public class Gantry {
this.liftRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.liftRight.setDirection(DcMotorSimple.Direction.REVERSE);
this.xControllerTarget = X_MIN;
this.xControllerTarget = X_CENTER;
}
public Gantry(HardwareMap hardwareMap, Telemetry telemetry) {
@ -87,13 +87,11 @@ public class Gantry {
}
public void intake() {
this.screwServo.setPower(1);
this.screwServo.setDirection(DcMotorSimple.Direction.FORWARD);
this.screwServo.setPower(-1);
}
public void deposit() {
this.screwServo.setPower(1);
this.screwServo.setDirection(DcMotorSimple.Direction.REVERSE);
this.screwServo.setPower(-1);
}
public void stop() {

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java
View File

@ -19,14 +19,15 @@ public class RobotConfig {
public static final String ROBOT_LIFT_ROTATION_NAME = "liftRotation";
public static final String ROBOT_LIFT_LIFT_NAME = "liftLift";
public static final String WEBCAM_NAME = "webcam";
public static final String PARALLEL_DRIVE_ODOMETRY = BACK_RIGHT_NAME;
public static final String PERPENDICULAR_DRIVE_ODOMETRY = FRONT_RIGHT_NAME;
public static final String PARALLEL_DRIVE_ODOMETRY = FRONT_LEFT_NAME;
public static final String PERPENDICULAR_DRIVE_ODOMETRY = BACK_LEFT_NAME;
// Drive
public static double SLOW_MODE_SPEED_PCT = 0.25;
public static double SLOW_MODE_TURN_PCT = 0.25;
public static double SPEED = 1f;
public static double TURN = 1f;
public static double AUTO_STRAFE_SLOWDOWN = 4;
// Arm
public static double PICKUP_ARM_MIN = 0.185;
@ -60,9 +61,10 @@ public class RobotConfig {
// Vision
public static double CAMERA_OFFSET_X = 0f;
public static double DETECTION_AREA_MIN = 0f;
public static double DETECTION_AREA_MAX = 1f;
public static double DETECTION_AREA_MIN = 0.02f;
public static double DETECTION_AREA_MAX = 0.3f;
public static double DETECTION_LEFT_X = 0;
public static double DETECTION_CENTER_X = .5;
public static double DETECTION_RIGHT_X = 1;
public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6.5;
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java
View File

@ -21,8 +21,8 @@ public class DriveConstants {
/*
* These are motor constants that should be listed online for your motors.
*/
public static final double TICKS_PER_REV = 1;
public static final double MAX_RPM = 1;
public static final double TICKS_PER_REV = 384.5;
public static final double MAX_RPM = 435;
/*
* Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
@ -44,9 +44,9 @@ public class DriveConstants {
* angular distances although most angular parameters are wrapped in Math.toRadians() for
* convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
*/
public static double WHEEL_RADIUS = 2; // in
public static double WHEEL_RADIUS = 3.779528 / 2; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 1; // in
public static double TRACK_WIDTH = 12.04724; // in
/*
* These are the feedforward parameters used to model the drive motor behavior. If you are using
@ -65,8 +65,8 @@ public class DriveConstants {
* small and gradually increase them later after everything is working. All distance units are
* inches.
*/
public static double MAX_VEL = 30;
public static double MAX_ACCEL = 30;
public static double MAX_VEL = 45;
public static double MAX_ACCEL = 45;
public static double MAX_ANG_VEL = Math.toRadians(60);
public static double MAX_ANG_ACCEL = Math.toRadians(60);

13
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java
View File

@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.AUTO_STRAFE_SLOWDOWN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.BACK_LEFT_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.BACK_RIGHT_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FRONT_LEFT_NAME;
@ -64,8 +65,8 @@ import java.util.List;
*/
@Config
public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(10, 0, 1.5);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(6, 0, 0);
public static double LATERAL_MULTIPLIER = 1;
@ -75,8 +76,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive
private TrajectorySequenceRunner trajectorySequenceRunner;
private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
public static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
public static final TrajectoryVelocityConstraint SLOW_VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL / AUTO_STRAFE_SLOWDOWN, MAX_ANG_VEL / AUTO_STRAFE_SLOWDOWN, TRACK_WIDTH);
public static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
private TrajectoryFollower follower;
@ -206,6 +208,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive
waitForIdle();
}
public void breakFollowing() {
trajectorySequenceRunner.breakFollowing();
}
public Pose2d getLastError() {
return trajectorySequenceRunner.getLastPoseError();
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java
View File

@ -38,14 +38,14 @@ import java.util.List;
*/
public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
public static double TICKS_PER_REV = 2000;
public static double WHEEL_RADIUS = 0.944882; // in
public static double WHEEL_RADIUS = 1.89 / 2; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
public static double PARALLEL_X = -0.269685; // X is the up and down direction
public static double PARALLEL_Y = -4.409449; // Y is the strafe direction
public static double PARALLEL_Y = 4.409449; // Y is the strafe direction
public static double PERPENDICULAR_X = 0.6299213;
public static double PERPENDICULAR_Y = -0.1122047;
public static double PERPENDICULAR_Y = 0.1122047;
public static double X_MULTIPLIER = 1; // Multiplier in the X direction
public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/TrajectorySequenceRunner.java
View File

@ -304,4 +304,9 @@ public class TrajectorySequenceRunner {
public boolean isBusy() {
return currentTrajectorySequence != null;
}
public void breakFollowing() {
currentTrajectorySequence = null;
remainingMarkers.clear();
}
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java
View File

@ -6,11 +6,11 @@ public class Colors {
// CV Color Threshold Constants
public static Scalar FTC_RED_LOWER = new Scalar(165, 80, 80);
public static Scalar FTC_RED_UPPER = new Scalar(15, 255, 255);
public static ScalarRange FTC_RED_RANGE_1 = new ScalarRange(FTC_RED_UPPER, FTC_RED_LOWER);
public static ScalarRange FTC_RED_RANGE_2 = new ScalarRange(FTC_RED_UPPER, FTC_RED_LOWER);
public static ScalarRange FTC_RED_RANGE_1 = new ScalarRange(new Scalar(180, FTC_RED_UPPER.val[1], FTC_RED_UPPER.val[2]), FTC_RED_LOWER);
public static ScalarRange FTC_RED_RANGE_2 = new ScalarRange(FTC_RED_UPPER, new Scalar(0, FTC_RED_LOWER.val[1], FTC_RED_LOWER.val[2]));
public static Scalar FTC_BLUE_LOWER = new Scalar(75, 40, 80);
public static Scalar FTC_BLUE_UPPER = new Scalar(120, 255, 255);
public static ScalarRange FTC_BLUE_RANGE = new ScalarRange(FTC_BLUE_UPPER, FTC_RED_LOWER);
public static ScalarRange FTC_BLUE_RANGE = new ScalarRange(FTC_BLUE_UPPER, FTC_BLUE_LOWER);
public static Scalar FTC_WHITE_LOWER = new Scalar(0, 0, 40);
public static Scalar FTC_WHITE_UPPER = new Scalar(180, 30, 255);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java
View File

@ -67,7 +67,7 @@ public class Detection {
// Check if the current Detection is valid
public boolean isValid() {
return (this.contour != null) && (this.centerPx != INVALID_POINT) && (this.areaPx != INVALID_AREA);
return (this.contour != null) && (this.areaPx != INVALID_AREA);
}
// Get the current contour

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java
View File

@ -79,11 +79,12 @@ public class PropDetectionPipeline implements VisionProcessor {
}
private MatOfPoint detect(ScalarRange... colorRanges) {
if (!mask.empty()) {
mask.setTo(this.zeros(mask.size(), mask.type()));
}
mask.release();
for (ScalarRange colorRange : colorRanges) {
Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), tmpMask);
if (mask.empty() || mask.rows() <= 0) {
Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), mask);
}
Core.add(mask, tmpMask, mask);
}
@ -105,5 +106,13 @@ public class PropDetectionPipeline implements VisionProcessor {
canvas.drawLine(detectionLeftXPx, 0, detectionLeftXPx, canvas.getHeight(), WHITE);
canvas.drawLine(detectionCenterXPx, 0, detectionCenterXPx, canvas.getHeight(), WHITE);
canvas.drawLine(detectionRightXPx, 0, detectionRightXPx, canvas.getHeight(), WHITE);
if (red.isValid()) {
canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, WHITE);
}
if (blue.isValid()) {
canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, WHITE);
}
}
}