Skip to content

Commit

Permalink
MTI 2023
Browse files Browse the repository at this point in the history
  • Loading branch information
@pserb
pserb committed Jun 28, 2023
1 parent 954ca9b commit a29c329
Show file tree
Hide file tree
Showing 6 changed files with 416 additions and 27 deletions.
6 changes: 3 additions & 3 deletions hardware/mechanisms/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,11 +13,11 @@ public class Arm extends Mechanism {
private Servo leftArm;
private Servo rightArm;

public static double INTAKE_POS = 0.865; // 0.18
public static double INTAKE_POS = 0.89; // 0.18
public static double AUTO_INTAKE_POS = 0.18;
public static double AUTO_SCORE_POS = 0.28;
public static double AUTO_SCORE_POS = 0.27;
public static double AUTO_CONE_STACK_POS = 0.57;
public static double SCORE_POS = 0.3; //0.76
public static double SCORE_POS = 0.27; //0.76
public static double GROUND_SCORE_POS = 0.82;

public Arm(LinearOpMode opMode) { this.opMode = opMode; }
Expand Down
2 changes: 1 addition & 1 deletion hardware/mechanisms/ScoringFSM.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public class ScoringFSM extends Mechanism {
public static double DELAY_MEDIUM = 0.3;
public static double DELAY_HIGH = 0.5;
public static double DELAY_RETRACTING = 0.3;
public static double DELAY_SCORING = 0.07; //0.3
public static double DELAY_SCORING = 0.15; //0.07

public static double DELAY_CLAMP_INTAKE = 1;

Expand Down
2 changes: 1 addition & 1 deletion hardware/mechanisms/SlidesMotors.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ public class SlidesMotors extends Mechanism {
public static double POS_GROUND = 0;
public static double POS_PREP_ARM = 15;
public static double POS_LOW = 20.2;
public static double POS_MEDIUM = 39;
public static double POS_MEDIUM = 39.7;
public static double POS_HIGH = 58.4;
public static double POS_HIGH_AUTO = 58.9;

Expand Down
28 changes: 14 additions & 14 deletions opmode/auton/AutoConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ public class AutoConstants {
public static final double DELAY_SCORING = 0.05; // 0.15

public static final double[] SLIDE_EXTEND_POSITIONS = {12, 8.8, 5.8, 3.3, 0.6, 0, 0, 0};
public static final double[] ARM_CONE_STACK_POSITIONS = {0.82, 0.83, 0.83, 0.83, 0.83, 0.9, 0.9};
public static final double[] ARM_CONE_STACK_POSITIONS = {0.83, 0.83, 0.84, 0.85, 0.86, 0.89, 0.92};

/** ==== END CONSTANTS FOR CONE AUTOS ==== **/

Expand Down Expand Up @@ -76,28 +76,28 @@ public class AutoConstants {
public static final double RR_ODO_SIDE_PARK_HEADING = Math.toRadians(90);

public static final double RR_ODO_CONE_STACK_X_SIX = 60.75; //61
public static final double RR_ODO_CONE_STACK_Y_SIX = -12.8; // -12.1 or smth
public static final double RR_ODO_CONE_STACK_Y_SIX = -11.8; // -12.1 or smth
public static final Vector2d RR_ODO_CONE_STACK_VECTOR_SIX = new Vector2d(RR_ODO_CONE_STACK_X_SIX, RR_ODO_CONE_STACK_Y_SIX);
public static final Pose2d RR_ODO_PRELOAD_CONE_STACK_POSE_SIX = new Pose2d(RR_CENTER_X, RR_ODO_CONE_STACK_Y_SIX, RR_HEADING);

public static final double RR_ODO_CONE_STACK_X = 61; //61.5
public static final double RR_ODO_CONE_STACK_Y = -12.6;
public static final double RR_ODO_CONE_STACK_X = 61.6; //61.5
public static final double RR_ODO_CONE_STACK_Y = -11.5; //-12.6
public static final Vector2d RR_ODO_CONE_STACK_VECTOR = new Vector2d(RR_ODO_CONE_STACK_X, RR_ODO_CONE_STACK_Y);
public static final Pose2d RR_ODO_PRELOAD_CONE_STACK_POSE = new Pose2d(RR_CENTER_X, RR_ODO_CONE_STACK_Y, RR_HEADING);


public static final double RR_ODO_HIGH_GOAL_X_SIX = 28.1; //26.75
public static final double RR_ODO_HIGH_GOAL_Y_SIX = -3.8; //-2.69
public static final double RR_ODO_HIGH_GOAL_X_SIX = 27.5; //28.1
public static final double RR_ODO_HIGH_GOAL_Y_SIX = -3.35; //-3.8

public static final double RR_ODO_PRELOAD_HIGH_GOAL_X_SIX = 28.6;
public static final double RR_ODO_PRELOAD_HIGH_GOAL_X_SIX = 29.2; //28.6
public static final double RR_ODO_PRELOAD_HIGH_GOAL_Y_SIX = -0.5;

public static final Vector2d RR_ODO_HIGH_GOAL_VECTOR_SIX = new Vector2d(RR_ODO_HIGH_GOAL_X_SIX, RR_ODO_HIGH_GOAL_Y_SIX);
public static final Vector2d RR_ODO_PRELOAD_HIGH_GOAL_VECTOR_SIX = new Vector2d(RR_ODO_PRELOAD_HIGH_GOAL_X_SIX, RR_ODO_PRELOAD_HIGH_GOAL_Y_SIX);
public static final Pose2d RR_ODO_PRELOAD_HIGH_GOAL_POSE_SIX = new Pose2d(RR_CENTER_X, RR_ODO_PRELOAD_HIGH_GOAL_Y_SIX, RR_HEADING);

public static final double RR_ODO_HIGH_GOAL_X = 26.75; //26.75
public static final double RR_ODO_HIGH_GOAL_Y = -2.69; //-2.69
public static final double RR_ODO_HIGH_GOAL_Y = -2.1; //-2.69

public static final double RR_ODO_PRELOAD_HIGH_GOAL_X = 28;
public static final double RR_ODO_PRELOAD_HIGH_GOAL_Y = 0.2;
Expand All @@ -120,18 +120,18 @@ public class AutoConstants {
public static final double RR_ODO_MID_MIDDLE_PARK_HEADING = Math.toRadians(70); //0
public static final double RR_ODO_MID_SIDE_PARK_HEADING = Math.toRadians(270);

public static final double RR_ODO_MID_GOAL_X = 29;
public static final double RR_ODO_MID_GOAL_Y = -19.24;
public static final double RR_ODO_PRELOAD_MID_GOAL_X = 28;
public static final double RR_ODO_PRELOAD_MID_GOAL_Y = -24;
public static final double RR_ODO_MID_GOAL_X = 28.4; //29
public static final double RR_ODO_MID_GOAL_Y = -19.7; //-19.24
public static final double RR_ODO_PRELOAD_MID_GOAL_X = 28; //28
public static final double RR_ODO_PRELOAD_MID_GOAL_Y = -24; //-24
public static final Vector2d RR_ODO_MID_GOAL_VECTOR = new Vector2d(RR_ODO_MID_GOAL_X, RR_ODO_MID_GOAL_Y);
public static final Vector2d RR_ODO_PRELOAD_MID_GOAL_VECTOR = new Vector2d(RR_ODO_PRELOAD_MID_GOAL_X, RR_ODO_PRELOAD_MID_GOAL_Y);
public static final Pose2d RR_ODO_PRELOAD_MID_GOAL_POSE = new Pose2d(RR_CENTER_X, RR_ODO_PRELOAD_MID_GOAL_Y, RR_HEADING);

public static final Vector2d RR_ODO_MID_MIDDLE_PARK_VECTOR = new Vector2d(RR_CENTER_X + 4, RR_ODO_CONE_STACK_Y + 3.25);

public static final double RR_ODO_MID_CONE_STACK_X = 61.5; //61.5
public static final double RR_ODO_MID_CONE_STACK_Y = -9.5;
public static final double RR_ODO_MID_CONE_STACK_X = 61.65; //61.5
public static final double RR_ODO_MID_CONE_STACK_Y = -8; //-9.5
public static final Vector2d RR_ODO_MID_CONE_STACK_VECTOR = new Vector2d(RR_ODO_MID_CONE_STACK_X, RR_ODO_MID_CONE_STACK_Y);

public static final Pose2d RR_ODO_MID_LEFT_PARK_POSE = new Pose2d(RR_ODO_LEFT_PARK_X + 0.5, RR_ODO_CONE_STACK_Y - 2.5, RR_ODO_MID_SIDE_PARK_HEADING);
Expand Down
19 changes: 11 additions & 8 deletions opmode/auton/highgoal/SixConeAutoAprilTag.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import android.annotation.SuppressLint;

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
Expand All @@ -25,13 +26,13 @@

import java.util.ArrayList;

@Autonomous (name = "HIGH 6 Cone Auto", group = "_ared", preselectTeleOp = "Drift Comp Main")
@Autonomous (name = "APRILTAG HIGH 6 Cone Auto", group = "_ared", preselectTeleOp = "Drift Comp Main")
public class SixConeAutoAprilTag extends LinearOpMode {

private Arm arm;
private Clamp clamp;
private SlidesMotors slides;
private final SignalSleeveWebcam signalSleeveWebcam = new SignalSleeveWebcam(this, "rightWebcam", SignalSleeveWebcam.ROBOT_SIDE.CONTROL_HUB);
// private final SignalSleeveWebcam signalSleeveWebcam = new SignalSleeveWebcam(this, "rightWebcam", SignalSleeveWebcam.ROBOT_SIDE.CONTROL_HUB);

ElapsedTime time = new ElapsedTime();

Expand All @@ -55,7 +56,7 @@ public class SixConeAutoAprilTag extends LinearOpMode {
private boolean canSlidesExtend = false;

private static final double DELAY_PRELOAD_PICKUP = 3.9;
public static final double DELAY_PICKUP = 2.1;
public static final double DELAY_PICKUP = 2.15;

public Runnable scoreReady = () -> {
try {
Expand Down Expand Up @@ -115,7 +116,7 @@ public void runOpMode() throws InterruptedException {
clamp = new Clamp(this);
arm = new Arm(this);
slides = new SlidesMotors(this);
signalSleeveWebcam.init(hardwareMap);
// signalSleeveWebcam.init(hardwareMap);
clamp.init(hardwareMap);
arm.init(hardwareMap);
slides.init(hardwareMap);
Expand Down Expand Up @@ -184,15 +185,16 @@ public void runOpMode() throws InterruptedException {
arm.intakePos();

int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "camera"), cameraMonitorViewId);
camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "rightWebcam"), cameraMonitorViewId);
aprilTagDetectionPipeline = new AprilTagDetectionPipeline(tagsize, fx, fy, cx, cy);

camera.setPipeline(aprilTagDetectionPipeline);

camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
@Override
public void onOpened() {
camera.startStreaming(800, 600, OpenCvCameraRotation.UPRIGHT);
camera.startStreaming(800, 600, OpenCvCameraRotation.SIDEWAYS_RIGHT);
FtcDashboard.getInstance().startCameraStream(camera, 0);
}

@Override
Expand Down Expand Up @@ -280,7 +282,7 @@ public void onError(int errorCode) {
}
if (!drive.isBusy()) {
Pose2d currPose = drive.getPoseEstimate();
drive.setPoseEstimate(new Pose2d(currPose.getX(), currPose.getY() + 0.15, currPose.getHeading()));
drive.setPoseEstimate(new Pose2d(currPose.getX(), currPose.getY() + 0.1, currPose.getHeading()));
clamp.close();
if (canSlidesExtend) {
slides.extendHighAuto();
Expand Down Expand Up @@ -332,7 +334,7 @@ public void onError(int errorCode) {
}
if (!drive.isBusy()) {
Pose2d currPose = drive.getPoseEstimate();
drive.setPoseEstimate(new Pose2d(currPose.getX(), currPose.getY() + 0.197, currPose.getHeading()));
drive.setPoseEstimate(new Pose2d(currPose.getX(), currPose.getY() + 0.19, currPose.getHeading()));
clamp.close();
if (canSlidesExtend) {
slides.extendHighAuto();
Expand Down Expand Up @@ -370,6 +372,7 @@ public void onError(int errorCode) {
time.reset();
}
}
break;
case IDLE:
arm.autoConeStackPos();
clamp.close();
Expand Down
Loading

0 comments on commit a29c329

Please sign in to comment.