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

Center auto mostly working

This commit is contained in:
Scott Barnes 2023-11-16 20:23:33 -06:00
parent 385c19ff9f
commit 94c9deae5b
9 changed files with 157 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
TeamCode/src/main/java/opmodes/AutoBase.java
View File

@ -1,14 +1,30 @@
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;
@ -20,14 +36,113 @@ public abstract class AutoBase extends LinearOpMode {
this.dashboard = FtcDashboard.getInstance();
this.dashboardTelemetry = dashboard.getTelemetry();
this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Red);
this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Blue);
this.robot.getDrive().setPoseEstimate(new Pose2d(-36, 63, Math.toRadians(-90)));
while(!isStarted() && !isStopRequested()) {
this.propLocation = this.robot.getCamera().getPropLocation();
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();
this.dashboardTelemetry.addData("Prop", detection.getCenterPx());
this.dashboardTelemetry.addData("Prop Location", this.propLocation);
this.dashboardTelemetry.update();
if (detection.isValid()) {
seenCount++;
}
}
if (seenCount / samples > 0.5) {
this.propLocation = ifVisible;
} else {
this.propLocation = ifNotVisible;
}
}
}

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() {

6
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;
@ -65,4 +66,5 @@ public class RobotConfig {
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();
}
}

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

@ -8,7 +8,6 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_RIGH
import static org.firstinspires.ftc.teamcode.util.Colors.FTC_BLUE_RANGE;
import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_1;
import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_2;
import static org.firstinspires.ftc.teamcode.util.Colors.RED;
import static org.firstinspires.ftc.teamcode.util.Colors.WHITE;
import static org.firstinspires.ftc.teamcode.util.Constants.ANCHOR;
import static org.firstinspires.ftc.teamcode.util.Constants.BLUR_SIZE;