bunch of minor changes plus major change in running auton with customizable settings
This commit is contained in:
@@ -1,62 +1,69 @@
|
||||
package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
|
||||
@@ -64,6 +71,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
|
||||
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
|
||||
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
|
||||
|
||||
import androidx.annotation.NonNull;
|
||||
@@ -82,7 +90,6 @@ import com.acmerobotics.roadrunner.ftc.Actions;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.constants.Poses_V2;
|
||||
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.utils.Flywheel;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
@@ -95,7 +102,7 @@ import java.util.Objects;
|
||||
|
||||
@Config
|
||||
@Autonomous(preselectTeleOp = "TeleopV3")
|
||||
public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
public class Auto_LT_Close extends LinearOpMode {
|
||||
public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset;
|
||||
public static double autoSpinStartPos = 0.2;
|
||||
public static double shoot0SpinSpeedIncrease = 0.015;
|
||||
@@ -110,7 +117,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
public static double blueObeliskTurrPos1 = -0.12;
|
||||
public static double blueObeliskTurrPos2 = -0.13;
|
||||
public static double blueObeliskTurrPos3 = -0.14;
|
||||
public static double redTurretShootPos = 0.12;
|
||||
public static double redTurretShootPos = 0.1;
|
||||
public static double blueTurretShootPos = -0.14;
|
||||
|
||||
double obeliskTurrPos1 = 0.0;
|
||||
@@ -163,21 +170,22 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
Targeting.Settings targetingSettings;
|
||||
private double firstSpindexShootPos = autoSpinStartPos;
|
||||
private boolean shootForward = true;
|
||||
private double x1, y1, h1;
|
||||
double x1, y1, h1;
|
||||
|
||||
private double x2a, y2a, h2a, t2a;
|
||||
double x2a, y2a, h2a, t2a;
|
||||
|
||||
private double x2b, y2b, h2b, t2b;
|
||||
private double x2c, y2c, h2c, t2c;
|
||||
double x2b, y2b, h2b, t2b;
|
||||
double x2c, y2c, h2c, t2c;
|
||||
|
||||
private double x3a, y3a, h3a;
|
||||
private double x3b, y3b, h3b;
|
||||
private double x4a, y4a, h4a;
|
||||
private double x4b, y4b, h4b;
|
||||
double x3a, y3a, h3a;
|
||||
double x3b, y3b, h3b;
|
||||
double x4a, y4a, h4a;
|
||||
double x4b, y4b, h4b;
|
||||
|
||||
private double xShoot, yShoot, hShoot;
|
||||
private double xGate, yGate, hGate;
|
||||
private double xPrep, yPrep, hPrep;
|
||||
double xShoot, yShoot, hShoot;
|
||||
double xGate, yGate, hGate;
|
||||
double xPrep, yPrep, hPrep;
|
||||
double xLeave, yLeave, hLeave;
|
||||
|
||||
private double shoot1Tangent;
|
||||
|
||||
@@ -186,6 +194,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
|
||||
private int rearSlotGreen = 0;
|
||||
private int mostGreenSlot = 0;
|
||||
int ballCycles = 3;
|
||||
int prevMotif = 0;
|
||||
|
||||
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
|
||||
return new Action() {
|
||||
@@ -340,7 +350,25 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
if (getRuntime() - stamp < shootTime) {
|
||||
double robX = drive.localizer.getPose().position.x;
|
||||
double robY = drive.localizer.getPose().position.y;
|
||||
double robotHeading = drive.localizer.getPose().heading.toDouble();
|
||||
|
||||
double goalX = -15;
|
||||
double goalY = 0;
|
||||
|
||||
double dx = robX - goalX; // delta x from robot to goal
|
||||
double dy = robY - goalY; // delta y from robot to goal
|
||||
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
|
||||
|
||||
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
|
||||
|
||||
targetingSettings = targeting.calculateSettings
|
||||
(robX, robY, robotHeading, 0.0, turretInterpolate);
|
||||
|
||||
turret.trackGoal(deltaPose);
|
||||
|
||||
if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
|
||||
|
||||
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
|
||||
robot.spin1.setPosition(autoSpinStartPos);
|
||||
@@ -403,6 +431,24 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
double robX = drive.localizer.getPose().position.x;
|
||||
double robY = drive.localizer.getPose().position.y;
|
||||
double robotHeading = drive.localizer.getPose().heading.toDouble();
|
||||
|
||||
double goalX = -15;
|
||||
double goalY = 0;
|
||||
|
||||
double dx = robX - goalX; // delta x from robot to goal
|
||||
double dy = robY - goalY; // delta y from robot to goal
|
||||
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
|
||||
|
||||
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
|
||||
|
||||
targetingSettings = targeting.calculateSettings
|
||||
(robX, robY, robotHeading, 0.0, turretInterpolate);
|
||||
|
||||
turret.trackGoal(deltaPose);
|
||||
|
||||
if (getRuntime() - stamp < shootTime) {
|
||||
|
||||
if (getRuntime() - stamp < firstShootTime) {
|
||||
@@ -457,11 +503,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
TELE.addData("Velocity", flywheel.getVelo());
|
||||
TELE.addData("Hood", robot.hood.getPosition());
|
||||
TELE.update();
|
||||
|
||||
return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
|
||||
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
|
||||
}
|
||||
};
|
||||
}
|
||||
@@ -781,6 +824,13 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
turrDefault += 0.01;
|
||||
}
|
||||
|
||||
if (gamepad2.rightBumperWasPressed()){
|
||||
ballCycles++;
|
||||
}
|
||||
if (gamepad2.leftBumperWasPressed()){
|
||||
ballCycles--;
|
||||
}
|
||||
|
||||
if (redAlliance) {
|
||||
robot.light.setPosition(0.28);
|
||||
|
||||
@@ -814,6 +864,9 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
xShoot = rShootX;
|
||||
yShoot = rShootY;
|
||||
hShoot = rShootH;
|
||||
xLeave = rLeaveX;
|
||||
yLeave = rLeaveY;
|
||||
hLeave = rLeaveH;
|
||||
|
||||
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
|
||||
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
|
||||
@@ -854,6 +907,9 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
xShoot = bShootX;
|
||||
yShoot = bShootY;
|
||||
hShoot = bShootH;
|
||||
xLeave = bLeaveX;
|
||||
yLeave = bLeaveY;
|
||||
hLeave = bLeaveH;
|
||||
|
||||
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
|
||||
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
|
||||
@@ -863,33 +919,45 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
}
|
||||
|
||||
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
|
||||
|
||||
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
.strafeToLinearHeading(new Vector2d(x2a, y2a), h2a)
|
||||
.strafeToLinearHeading(new Vector2d(x2b, y2b), h2b,
|
||||
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
|
||||
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
|
||||
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
|
||||
new TranslationalVelConstraint(pickup1Speed));
|
||||
|
||||
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
|
||||
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
|
||||
if (ballCycles < 2){
|
||||
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
|
||||
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
|
||||
} else {
|
||||
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
|
||||
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
|
||||
}
|
||||
|
||||
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
|
||||
.strafeToLinearHeading(new Vector2d(x3a, y3a), h3a)
|
||||
.strafeToLinearHeading(new Vector2d(x3b, y3b), h3b,
|
||||
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
|
||||
.strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
|
||||
.strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
|
||||
new TranslationalVelConstraint(pickup1Speed));
|
||||
|
||||
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b))
|
||||
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
|
||||
if (ballCycles < 3){
|
||||
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
|
||||
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
|
||||
} else {
|
||||
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
|
||||
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hLeave));
|
||||
}
|
||||
|
||||
pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
.strafeToLinearHeading(new Vector2d(x4a, y4a), h4a)
|
||||
.strafeToLinearHeading(new Vector2d(x4b, y4b), h4b,
|
||||
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
|
||||
.strafeToLinearHeading(new Vector2d(x4a, y4a), Math.toRadians(h4a))
|
||||
.strafeToLinearHeading(new Vector2d(x4b, y4b), Math.toRadians(h4b),
|
||||
new TranslationalVelConstraint(pickup1Speed));
|
||||
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b))
|
||||
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
|
||||
|
||||
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, Math.toRadians(h4b)))
|
||||
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
|
||||
|
||||
TELE.addData("Red?", redAlliance);
|
||||
TELE.addData("Turret Default", turrDefault);
|
||||
TELE.addData("Ball Cycles", ballCycles);
|
||||
|
||||
TELE.update();
|
||||
}
|
||||
@@ -924,6 +992,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
|
||||
);
|
||||
|
||||
if (ballCycles > 0){
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
pickup1.build(),
|
||||
@@ -952,7 +1021,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
motif = turret.getObeliskID();
|
||||
|
||||
if (motif == 0) motif = 22;
|
||||
int prevMotif = motif;
|
||||
prevMotif = motif;
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
@@ -984,7 +1053,9 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
)
|
||||
|
||||
);
|
||||
}
|
||||
|
||||
if (ballCycles > 1){
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
pickup2.build(),
|
||||
@@ -1040,7 +1111,9 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
)
|
||||
|
||||
);
|
||||
}
|
||||
|
||||
if (ballCycles > 2){
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
pickup3.build(),
|
||||
@@ -1096,6 +1169,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
||||
)
|
||||
|
||||
);
|
||||
}
|
||||
|
||||
while (opModeIsActive()) {
|
||||
|
||||
@@ -1,61 +1,61 @@
|
||||
package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
|
||||
|
||||
@@ -76,7 +76,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.constants.Poses_V2;
|
||||
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.utils.Flywheel;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
@@ -1,61 +1,61 @@
|
||||
package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3aG;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3aG;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by3aG;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3aG;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3aG;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3aG;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3aG;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3aG;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3aG;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3aG;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3aG;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3aG;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
|
||||
@@ -78,6 +78,7 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint;
|
||||
import com.acmerobotics.roadrunner.Vector2d;
|
||||
import com.acmerobotics.roadrunner.ftc.Actions;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.constants.Poses_V2;
|
||||
@@ -91,6 +92,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
@Disabled
|
||||
@Config
|
||||
@Autonomous(preselectTeleOp = "TeleopV3")
|
||||
public class Auto_LT_Close_GateOpen extends LinearOpMode {
|
||||
|
||||
@@ -1,9 +1,16 @@
|
||||
package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleEnd;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
|
||||
@@ -11,6 +18,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
|
||||
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
|
||||
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
|
||||
|
||||
import androidx.annotation.NonNull;
|
||||
@@ -22,6 +30,8 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||
import com.acmerobotics.roadrunner.Action;
|
||||
import com.acmerobotics.roadrunner.ParallelAction;
|
||||
import com.acmerobotics.roadrunner.Pose2d;
|
||||
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
|
||||
import com.acmerobotics.roadrunner.Vector2d;
|
||||
import com.acmerobotics.roadrunner.ftc.Actions;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
@@ -38,7 +48,7 @@ import java.util.Objects;
|
||||
|
||||
@Config
|
||||
@Autonomous(preselectTeleOp = "TeleopV3")
|
||||
public class Auto_LT_Far_3Ball extends LinearOpMode {
|
||||
public class Auto_LT_Far extends LinearOpMode {
|
||||
public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset;
|
||||
public static double autoSpinStartPos = 0.2;
|
||||
public static double shoot0SpinSpeedIncrease = 0.015;
|
||||
@@ -46,12 +56,7 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
|
||||
public static double redTurretShootPos = 0.05;
|
||||
public static double blueTurretShootPos = -0.05;
|
||||
public static int fwdTime = 200, strafeTime = 2300;
|
||||
public static double rPickupGateX = 1, rPickupGateY = 1, rPickupGateH = 1;
|
||||
public static double rPickupZoneX = 1, rPickupZoneY = 1, rPickupZoneH = 1;
|
||||
public static double rShootX = 1, rShootY = 1, rShootH = 1;
|
||||
public static double bPickupZoneX = 1, bPickupZoneY = 1, bPickupZoneH = 1;
|
||||
public static double bPickupGateX = 1, bPickupGateY = 1, bPickupGateH = 1;
|
||||
public static double bShootX = 1, bShootY = 1, bShootH = 1;
|
||||
double xLeave, yLeave, hLeave;
|
||||
public static int sleepTime = 1300;
|
||||
public int motif = 0;
|
||||
double turretShootPos = 0.0;
|
||||
@@ -228,7 +233,25 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
if (getRuntime() - stamp < shootTime) {
|
||||
double robX = drive.localizer.getPose().position.x;
|
||||
double robY = drive.localizer.getPose().position.y;
|
||||
double robotHeading = drive.localizer.getPose().heading.toDouble();
|
||||
|
||||
double goalX = -15;
|
||||
double goalY = 0;
|
||||
|
||||
double dx = robX - goalX; // delta x from robot to goal
|
||||
double dy = robY - goalY; // delta y from robot to goal
|
||||
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
|
||||
|
||||
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
|
||||
|
||||
targetingSettings = targeting.calculateSettings
|
||||
(robX, robY, robotHeading, 0.0, turretInterpolate);
|
||||
|
||||
turret.trackGoal(deltaPose);
|
||||
|
||||
if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
|
||||
|
||||
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
|
||||
robot.spin1.setPosition(autoSpinStartPos);
|
||||
@@ -345,11 +368,8 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
TELE.addData("Velocity", flywheel.getVelo());
|
||||
TELE.addData("Hood", robot.hood.getPosition());
|
||||
TELE.update();
|
||||
|
||||
return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
|
||||
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
|
||||
}
|
||||
};
|
||||
}
|
||||
@@ -642,6 +662,8 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
|
||||
|
||||
robot.light.setPosition(1);
|
||||
|
||||
TrajectoryActionBuilder leave = null;
|
||||
|
||||
while (opModeInInit()) {
|
||||
|
||||
// Recalibration in initialization
|
||||
@@ -669,37 +691,25 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
|
||||
if (redAlliance) {
|
||||
robot.light.setPosition(0.28);
|
||||
|
||||
pickupGateX = rPickupGateX;
|
||||
pickupGateY = rPickupGateY;
|
||||
pickupGateH = rPickupGateH;
|
||||
|
||||
pickupZoneX = rPickupZoneX;
|
||||
pickupZoneY = rPickupZoneY;
|
||||
pickupZoneH = rPickupZoneH;
|
||||
|
||||
xShoot = rShootX;
|
||||
yShoot = rShootY;
|
||||
hShoot = rShootH;
|
||||
xLeave = rLeaveX;
|
||||
yLeave = rLeaveY;
|
||||
hLeave = rLeaveH;
|
||||
|
||||
turretShootPos = turrDefault + redTurretShootPos;
|
||||
} else {
|
||||
robot.light.setPosition(0.6);
|
||||
|
||||
pickupGateX = bPickupGateX;
|
||||
pickupGateY = bPickupGateY;
|
||||
pickupGateH = bPickupGateH;
|
||||
|
||||
pickupZoneX = bPickupZoneX;
|
||||
pickupZoneY = bPickupZoneY;
|
||||
pickupZoneH = bPickupZoneH;
|
||||
|
||||
xShoot = bShootX;
|
||||
yShoot = bShootY;
|
||||
hShoot = bShootH;
|
||||
xLeave = bLeaveX;
|
||||
yLeave = bLeaveY;
|
||||
hLeave = bLeaveH;
|
||||
|
||||
turretShootPos = turrDefault + blueTurretShootPos;
|
||||
}
|
||||
|
||||
leave = drive.actionBuilder(teleEnd)
|
||||
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
|
||||
|
||||
|
||||
TELE.addData("Red?", redAlliance);
|
||||
TELE.addData("Turret Default", turrDefault);
|
||||
TELE.addData("Start Position", teleEnd);
|
||||
@@ -721,7 +731,7 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
|
||||
manageFlywheel(
|
||||
shoot0Vel,
|
||||
shoot0Hood,
|
||||
9,
|
||||
5,
|
||||
0.501,
|
||||
0.501,
|
||||
shoot0XTolerance,
|
||||
@@ -736,9 +746,16 @@ public class Auto_LT_Far_3Ball extends LinearOpMode {
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
Actions.runBlocking(
|
||||
shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease)
|
||||
shootAll((int) shoot0Vel, 4, shoot0SpinSpeedIncrease)
|
||||
);
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
assert leave != null;
|
||||
Actions.runBlocking(leave.build());
|
||||
|
||||
// Actual way to end autonomous in to find final position
|
||||
while (opModeIsActive()) {
|
||||
|
||||
@@ -1,49 +1,49 @@
|
||||
package org.firstinspires.ftc.teamcode.autonomous.disabled;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
|
||||
|
||||
@@ -0,0 +1,10 @@
|
||||
package org.firstinspires.ftc.teamcode.constants;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
|
||||
@Config
|
||||
public class Back_Poses {
|
||||
public static double rLeaveX = 80, rLeaveY = 70, rLeaveH = 50;
|
||||
public static double bLeaveX = 80, bLeaveY = -70, bLeaveH = -50;
|
||||
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
package org.firstinspires.ftc.teamcode.constants;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.Pose2d;
|
||||
|
||||
@Config
|
||||
public class Front_Poses {
|
||||
|
||||
|
||||
public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
|
||||
public static double rx2a = 41, ry2a = 18, rh2a = 140;
|
||||
public static double rx2b = 19, ry2b = 40, rh2b = 140;
|
||||
|
||||
|
||||
public static double rx3a = 55, ry3a = 39, rh3a = 140;
|
||||
|
||||
public static double rx3aG = 60, ry3aG = 34, rh3aG = 140;
|
||||
|
||||
public static double rx3b = 36, ry3b = 58, rh3b = 140.1;
|
||||
|
||||
public static double rx4a = 80, ry4a = 48, rh4a = 140;
|
||||
public static double rx4b = 45, ry4b = 83, rh4b = 140.1;
|
||||
|
||||
public static double bx1 = 20, by1 = 0.5, bh1 = 0.1;
|
||||
public static double bx2a = 41, by2a = -18, bh2a = -140;
|
||||
public static double bx2b = 19, by2b = -40, bh2b = -140.1;
|
||||
|
||||
public static double bx3a = 55, by3a = -39, bh3a = -140;
|
||||
public static double bx3b = 41, by3b = -59, bh3b = -140.1;
|
||||
public static double bx3aG = 55, by3aG = -43, bh3aG = -140;
|
||||
|
||||
public static double bx4a = 75, by4a = -53, bh4a = -140;
|
||||
public static double bx4b = 47, by4b = -85, bh4b = -140.1;
|
||||
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0;
|
||||
|
||||
public static double rShootX = 40, rShootY = 10, rShootH = 50;
|
||||
public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
|
||||
|
||||
public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50;
|
||||
|
||||
public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50;
|
||||
|
||||
public static double bShootX = 40, bShootY = 0, bShootH = -50;
|
||||
public static double bxPrep = 45, byPrep = -10, bhPrep = -50;
|
||||
|
||||
|
||||
public static Pose2d teleStart = new Pose2d(0, 0, 0);
|
||||
|
||||
public static Pose2d teleEnd = new Pose2d(0, 0, 0);
|
||||
|
||||
|
||||
}
|
||||
@@ -1,49 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.constants;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.Pose2d;
|
||||
|
||||
@Config
|
||||
public class Poses {
|
||||
|
||||
|
||||
public static double rx1 = 20, ry1 = 0.5, rh1 = Math.toRadians(0.1);
|
||||
public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140);
|
||||
public static double rx2b = 19, ry2b = 40, rh2b = Math.toRadians(140.1);
|
||||
|
||||
|
||||
public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
|
||||
|
||||
public static double rx3aG = 60, ry3aG = 34, rh3aG = Math.toRadians(140);
|
||||
|
||||
public static double rx3b = 36, ry3b = 58, rh3b = Math.toRadians(140.1);
|
||||
|
||||
public static double rx4a = 75, ry4a = 53, rh4a = Math.toRadians(140);
|
||||
public static double rx4b = 45, ry4b = 83, rh4b = Math.toRadians(140.1);
|
||||
|
||||
public static double bx1 = 20, by1 = 0.5, bh1 = Math.toRadians(0.1);
|
||||
public static double bx2a = 41, by2a = -18, bh2a = Math.toRadians(-140);
|
||||
public static double bx2b = 19, by2b = -40, bh2b = Math.toRadians(-140.1);
|
||||
|
||||
public static double bx3a = 55, by3a = -39, bh3a = Math.toRadians(-140);
|
||||
public static double bx3b = 41, by3b = -59, bh3b = Math.toRadians(-140.1);
|
||||
public static double bx3aG = 55, by3aG = -43, bh3aG = Math.toRadians(-140);
|
||||
|
||||
|
||||
public static double bx4a = 75, by4a = -53, bh4a = Math.toRadians(-140);
|
||||
public static double bx4b = 47, by4b = -85, bh4b = Math.toRadians(-140.1);
|
||||
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
|
||||
|
||||
public static double rShootX = 40, rShootY = -7, rShootH = Math.toRadians(50);
|
||||
public static double rxPrep = 45, ryPrep = 10, rhPrep = Math.toRadians(50);
|
||||
|
||||
public static double bShootX = 40, bShootY = 7, bShootH = Math.toRadians(-50);
|
||||
public static double bxPrep = 45, byPrep = -10, bhPrep = Math.toRadians(-50);
|
||||
|
||||
|
||||
public static Pose2d teleStart = new Pose2d(0, 0, 0);
|
||||
|
||||
public static Pose2d teleEnd = new Pose2d(0, 0, 0);
|
||||
|
||||
|
||||
}
|
||||
@@ -1,10 +1,10 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
|
||||
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
|
||||
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
@@ -70,8 +70,6 @@ public class TeleopV3 extends LinearOpMode {
|
||||
boolean overrideTurr = false;
|
||||
|
||||
int intakeTicker = 0;
|
||||
|
||||
boolean turretInterpolate = false;
|
||||
private boolean shootAll = false;
|
||||
|
||||
@Override
|
||||
@@ -141,7 +139,7 @@ public class TeleopV3 extends LinearOpMode {
|
||||
|
||||
robot.transfer.setPower(1);
|
||||
|
||||
double offset;
|
||||
//TURRET TRACKING
|
||||
|
||||
double robX = drive.localizer.getPose().position.x;
|
||||
double robY = drive.localizer.getPose().position.y;
|
||||
@@ -318,18 +316,19 @@ public class TeleopV3 extends LinearOpMode {
|
||||
// TELE.addData("spinTestCounter", spindexer.counter);
|
||||
// TELE.addData("autoSpintake", autoSpintake);
|
||||
//
|
||||
TELE.addData("shootall commanded", shootAll);
|
||||
// TELE.addData("shootall commanded", shootAll);
|
||||
// Targeting Debug
|
||||
TELE.addData("robotX", robotX);
|
||||
TELE.addData("robotY", robotY);
|
||||
TELE.addData("robotInchesX", targeting.robotInchesX);
|
||||
TELE.addData( "robotInchesY", targeting.robotInchesY);
|
||||
TELE.addData("Targeting Interpolate", turretInterpolate);
|
||||
TELE.addData("Targeting GridX", targeting.robotGridX);
|
||||
TELE.addData("Targeting GridY", targeting.robotGridY);
|
||||
TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
|
||||
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
|
||||
TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
|
||||
// TELE.addData("robotX", robotX);
|
||||
// TELE.addData("robotY", robotY);
|
||||
// TELE.addData("robotInchesX", targeting.robotInchesX);
|
||||
// TELE.addData( "robotInchesY", targeting.robotInchesY);
|
||||
// TELE.addData("Targeting Interpolate", turretInterpolate);
|
||||
// TELE.addData("Targeting GridX", targeting.robotGridX);
|
||||
// TELE.addData("Targeting GridY", targeting.robotGridY);
|
||||
// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
|
||||
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
|
||||
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
|
||||
TELE.addData("Voltage", robot.voltage.getVoltage());
|
||||
|
||||
TELE.update();
|
||||
|
||||
|
||||
@@ -12,6 +12,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
import com.qualcomm.robotcore.hardware.VoltageSensor;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
||||
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
|
||||
@@ -56,11 +57,11 @@ public class Robot {
|
||||
public RevColorSensorV3 color3;
|
||||
public Limelight3A limelight;
|
||||
public Servo light;
|
||||
public VoltageSensor voltage;
|
||||
|
||||
public Robot(HardwareMap hardwareMap) {
|
||||
|
||||
//Define components w/ hardware map
|
||||
//TODO: fix the configuration of these - I trust you to figure it out yourself @KeshavAnandCode
|
||||
frontLeft = hardwareMap.get(DcMotorEx.class, "fl");
|
||||
frontRight = hardwareMap.get(DcMotorEx.class, "fr");
|
||||
backLeft = hardwareMap.get(DcMotorEx.class, "bl");
|
||||
@@ -78,7 +79,7 @@ public class Robot {
|
||||
shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
|
||||
|
||||
shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
|
||||
//TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode
|
||||
|
||||
shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F);
|
||||
shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
@@ -96,7 +97,6 @@ public class Robot {
|
||||
|
||||
turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
|
||||
|
||||
//TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer
|
||||
spin1 = hardwareMap.get(Servo.class, "spin2");
|
||||
|
||||
spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
|
||||
@@ -128,5 +128,6 @@ public class Robot {
|
||||
}
|
||||
|
||||
light = hardwareMap.get(Servo.class, "light");
|
||||
voltage = hardwareMap.voltageSensor.iterator().next();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -77,6 +77,7 @@ public class Targeting {
|
||||
double cancelOffsetY = 0.0; // was 7.0
|
||||
double unitConversionFactor = 0.95;
|
||||
int tileSize = 24; //inches
|
||||
public static boolean turretInterpolate = false;
|
||||
|
||||
public Targeting() {
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user