Its the most wonderful time of the year
This commit is contained in:
@@ -208,7 +208,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
|
|||||||
|
|
||||||
drive.updatePoseEstimate();
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
Poses_V2.teleStart = drive.localizer.getPose();
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
robot.intake.setPower(-0.3);
|
robot.intake.setPower(-0.3);
|
||||||
|
|
||||||
@@ -272,7 +272,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
|
|||||||
|
|
||||||
drive.updatePoseEstimate();
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
Poses_V2.teleStart = drive.localizer.getPose();
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
robot.intake.setPower(-0.3);
|
robot.intake.setPower(-0.3);
|
||||||
|
|
||||||
|
|||||||
@@ -326,7 +326,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
|||||||
|
|
||||||
drive.updatePoseEstimate();
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
Poses_V2.teleStart = drive.localizer.getPose();
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
robot.intake.setPower(-0.3);
|
robot.intake.setPower(-0.3);
|
||||||
|
|
||||||
@@ -390,7 +390,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
|
|||||||
|
|
||||||
drive.updatePoseEstimate();
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
Poses_V2.teleStart = drive.localizer.getPose();
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
robot.intake.setPower(-0.3);
|
robot.intake.setPower(-0.3);
|
||||||
|
|
||||||
|
|||||||
@@ -1,804 +0,0 @@
|
|||||||
package org.firstinspires.ftc.teamcode.autonomous;
|
|
||||||
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bHGate;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShoot1Tangent;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootH;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootX;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootY;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bXGate;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bYGate;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh1;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2a;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2b;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2c;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bt2a;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bt2b;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bt2c;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx1;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx2a;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx2b;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx2c;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by1;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2a;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2b;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2c;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rHGate;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShoot1Tangent;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootH;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootX;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootY;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rXGate;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rYGate;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh1;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2a;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2b;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2c;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rt2a;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rt2b;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rt2c;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx1;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx2a;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx2b;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx2c;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry1;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry2a;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry2b;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry2c;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.teleStart;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
|
|
||||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
|
|
||||||
|
|
||||||
import androidx.annotation.NonNull;
|
|
||||||
|
|
||||||
import com.acmerobotics.dashboard.FtcDashboard;
|
|
||||||
import com.acmerobotics.dashboard.config.Config;
|
|
||||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
|
||||||
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.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.LinearOpMode;
|
|
||||||
import com.qualcomm.robotcore.hardware.Servo;
|
|
||||||
|
|
||||||
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
|
|
||||||
import org.firstinspires.ftc.teamcode.utils.Flywheel;
|
|
||||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
|
||||||
import org.firstinspires.ftc.teamcode.utils.Servos;
|
|
||||||
import org.firstinspires.ftc.teamcode.utils.Spindexer;
|
|
||||||
import org.firstinspires.ftc.teamcode.utils.Targeting;
|
|
||||||
import org.firstinspires.ftc.teamcode.utils.Turret;
|
|
||||||
|
|
||||||
@Autonomous(preselectTeleOp = "TeleopV3")
|
|
||||||
@Config
|
|
||||||
public class Auto_LT_Close_GateCycle extends LinearOpMode {
|
|
||||||
|
|
||||||
public static double shoot0Vel = 2300, shoot0Hood = 0.93;
|
|
||||||
public static double autoSpinStartPos = 0.2;
|
|
||||||
public static double shoot0SpinSpeedIncrease = 0.014;
|
|
||||||
|
|
||||||
public static double spindexerSpeedIncrease = 0.02;
|
|
||||||
|
|
||||||
public static double obeliskTurrPos = 0.53;
|
|
||||||
public static double gatePickupTime = 3.0;
|
|
||||||
public static double shoot1Turr = 0.57;
|
|
||||||
public static double shoot0XTolerance = 1.0;
|
|
||||||
public static double turretShootPos = 0.72;
|
|
||||||
public static double shootAllTime = 1.8;
|
|
||||||
public static double shoot0Time = 1.6;
|
|
||||||
public static double intake1Time = 3.0;
|
|
||||||
public static double flywheel0Time = 3.5;
|
|
||||||
public static double pickup1Speed = 80.0;
|
|
||||||
// ---- SECOND SHOT / PICKUP ----
|
|
||||||
public static double shoot1Vel = 2300;
|
|
||||||
public static double shoot1Hood = 0.93;
|
|
||||||
|
|
||||||
public static double shootAllVelocity = 2500;
|
|
||||||
public static double shootAllHood = 0.78;
|
|
||||||
// ---- PICKUP TIMING ----
|
|
||||||
public static double pickup1Time = 3.0;
|
|
||||||
// ---- PICKUP POSITION TOLERANCES ----
|
|
||||||
public static double pickup1XTolerance = 2.0;
|
|
||||||
public static double pickup1YTolerance = 2.0;
|
|
||||||
// ---- OBELISK DETECTION ----
|
|
||||||
public static double obelisk1Time = 1.5;
|
|
||||||
public static double obelisk1XTolerance = 2.0;
|
|
||||||
public static double obelisk1YTolerance = 2.0;
|
|
||||||
public static double shoot1ToleranceX = 2.0;
|
|
||||||
public static double shoot1ToleranceY = 2.0;
|
|
||||||
public static double shoot1Time = 2.0;
|
|
||||||
public static double shootCycleTime = 2.5;
|
|
||||||
public int motif = 0;
|
|
||||||
Robot robot;
|
|
||||||
MultipleTelemetry TELE;
|
|
||||||
MecanumDrive drive;
|
|
||||||
Servos servos;
|
|
||||||
Spindexer spindexer;
|
|
||||||
Flywheel flywheel;
|
|
||||||
Turret turret;
|
|
||||||
Targeting targeting;
|
|
||||||
Targeting.Settings targetingSettings;
|
|
||||||
private double x1, y1, h1;
|
|
||||||
|
|
||||||
private double x2a, y2a, h2a, t2a;
|
|
||||||
|
|
||||||
private double x2b, y2b, h2b, t2b;
|
|
||||||
private double x2c, y2c, h2c, t2c;
|
|
||||||
|
|
||||||
private double xShoot, yShoot, hShoot;
|
|
||||||
private double xGate, yGate, hGate;
|
|
||||||
|
|
||||||
private double shoot1Tangent;
|
|
||||||
|
|
||||||
public Action prepareShootAll(double time) {
|
|
||||||
return new Action() {
|
|
||||||
double stamp = 0.0;
|
|
||||||
int ticker = 0;
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
|
||||||
if (ticker == 0) {
|
|
||||||
stamp = System.currentTimeMillis();
|
|
||||||
}
|
|
||||||
ticker++;
|
|
||||||
|
|
||||||
robot.spin1.setPosition(autoSpinStartPos);
|
|
||||||
robot.spin2.setPosition(1 - autoSpinStartPos);
|
|
||||||
|
|
||||||
robot.transferServo.setPosition(transferServo_out);
|
|
||||||
|
|
||||||
turret.manualSetTurret(turretShootPos);
|
|
||||||
|
|
||||||
robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000);
|
|
||||||
|
|
||||||
return (System.currentTimeMillis() - stamp) < (time * 1000);
|
|
||||||
|
|
||||||
}
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
public Action shootAll(int vel, double shootTime, double spindexSpeed) {
|
|
||||||
return new Action() {
|
|
||||||
int ticker = 1;
|
|
||||||
|
|
||||||
double stamp = 0.0;
|
|
||||||
|
|
||||||
double velo = vel;
|
|
||||||
|
|
||||||
int shooterTicker = 0;
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
|
||||||
TELE.addData("Velocity", velo);
|
|
||||||
TELE.addLine("shooting");
|
|
||||||
TELE.update();
|
|
||||||
|
|
||||||
flywheel.manageFlywheel(vel);
|
|
||||||
velo = flywheel.getVelo();
|
|
||||||
|
|
||||||
drive.updatePoseEstimate();
|
|
||||||
|
|
||||||
teleStart = drive.localizer.getPose();
|
|
||||||
|
|
||||||
robot.intake.setPower(-0.3);
|
|
||||||
|
|
||||||
if (ticker == 1) {
|
|
||||||
stamp = getRuntime();
|
|
||||||
}
|
|
||||||
ticker++;
|
|
||||||
|
|
||||||
robot.intake.setPower(0);
|
|
||||||
|
|
||||||
if (getRuntime() - stamp < shootTime) {
|
|
||||||
|
|
||||||
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
|
|
||||||
robot.spin1.setPosition(autoSpinStartPos);
|
|
||||||
robot.spin2.setPosition(1 - autoSpinStartPos);
|
|
||||||
} else {
|
|
||||||
robot.transferServo.setPosition(transferServo_in);
|
|
||||||
shooterTicker++;
|
|
||||||
double prevSpinPos = robot.spin1.getPosition();
|
|
||||||
robot.spin1.setPosition(prevSpinPos + spindexSpeed);
|
|
||||||
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
|
|
||||||
}
|
|
||||||
|
|
||||||
return true;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
robot.transferServo.setPosition(transferServo_out);
|
|
||||||
//spindexPos = spindexer_intakePos1;
|
|
||||||
|
|
||||||
spindexer.resetSpindexer();
|
|
||||||
spindexer.processIntake();
|
|
||||||
|
|
||||||
return false;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
public Action shootAllAuto(double shootTime, double spindexSpeed) {
|
|
||||||
return new Action() {
|
|
||||||
int ticker = 1;
|
|
||||||
|
|
||||||
double stamp = 0.0;
|
|
||||||
|
|
||||||
double velo = 0.0;
|
|
||||||
|
|
||||||
int shooterTicker = 0;
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
|
||||||
TELE.addData("Velocity", velo);
|
|
||||||
TELE.addLine("shooting");
|
|
||||||
TELE.update();
|
|
||||||
|
|
||||||
velo = flywheel.getVelo();
|
|
||||||
|
|
||||||
drive.updatePoseEstimate();
|
|
||||||
|
|
||||||
teleStart = drive.localizer.getPose();
|
|
||||||
|
|
||||||
robot.intake.setPower(-0.3);
|
|
||||||
|
|
||||||
if (ticker == 1) {
|
|
||||||
stamp = getRuntime();
|
|
||||||
}
|
|
||||||
ticker++;
|
|
||||||
|
|
||||||
robot.intake.setPower(0);
|
|
||||||
|
|
||||||
if (getRuntime() - stamp < shootTime) {
|
|
||||||
|
|
||||||
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
|
|
||||||
robot.spin1.setPosition(autoSpinStartPos);
|
|
||||||
robot.spin2.setPosition(1 - autoSpinStartPos);
|
|
||||||
} else {
|
|
||||||
robot.transferServo.setPosition(transferServo_in);
|
|
||||||
shooterTicker++;
|
|
||||||
double prevSpinPos = robot.spin1.getPosition();
|
|
||||||
robot.spin1.setPosition(prevSpinPos + spindexSpeed);
|
|
||||||
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
|
|
||||||
}
|
|
||||||
|
|
||||||
return true;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
robot.transferServo.setPosition(transferServo_out);
|
|
||||||
//spindexPos = spindexer_intakePos1;
|
|
||||||
|
|
||||||
spindexer.resetSpindexer();
|
|
||||||
spindexer.processIntake();
|
|
||||||
|
|
||||||
return false;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
public Action intake(double intakeTime) {
|
|
||||||
return new Action() {
|
|
||||||
double stamp = 0.0;
|
|
||||||
int ticker = 0;
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
|
||||||
if (ticker == 0) {
|
|
||||||
stamp = System.currentTimeMillis();
|
|
||||||
}
|
|
||||||
ticker++;
|
|
||||||
|
|
||||||
spindexer.processIntake();
|
|
||||||
robot.intake.setPower(1);
|
|
||||||
|
|
||||||
motif = turret.detectObelisk();
|
|
||||||
|
|
||||||
spindexer.ballCounterLight();
|
|
||||||
|
|
||||||
return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
|
|
||||||
|
|
||||||
}
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
public Action detectObelisk(
|
|
||||||
double time,
|
|
||||||
double posX,
|
|
||||||
double posY,
|
|
||||||
double posXTolerance,
|
|
||||||
double posYTolerance,
|
|
||||||
double turrPos
|
|
||||||
) {
|
|
||||||
|
|
||||||
boolean timeFallback = (time != 0.501);
|
|
||||||
boolean posXFallback = (posX != 0.501);
|
|
||||||
boolean posYFallback = (posY != 0.501);
|
|
||||||
|
|
||||||
return new Action() {
|
|
||||||
|
|
||||||
double stamp = 0.0;
|
|
||||||
int ticker = 0;
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
|
||||||
|
|
||||||
drive.updatePoseEstimate();
|
|
||||||
Pose2d currentPose = drive.localizer.getPose();
|
|
||||||
|
|
||||||
if (ticker == 0) {
|
|
||||||
stamp = System.currentTimeMillis();
|
|
||||||
}
|
|
||||||
|
|
||||||
ticker++;
|
|
||||||
|
|
||||||
motif = turret.detectObelisk();
|
|
||||||
|
|
||||||
robot.turr1.setPosition(turrPos);
|
|
||||||
robot.turr2.setPosition(1 - turrPos);
|
|
||||||
|
|
||||||
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
|
|
||||||
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
|
|
||||||
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
|
|
||||||
|
|
||||||
boolean shouldFinish = timeDone || xDone || yDone;
|
|
||||||
|
|
||||||
return !shouldFinish;
|
|
||||||
|
|
||||||
}
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
public Action manageFlywheel(
|
|
||||||
double vel,
|
|
||||||
double hoodPos,
|
|
||||||
double time,
|
|
||||||
double posX,
|
|
||||||
double posY,
|
|
||||||
double posXTolerance,
|
|
||||||
double posYTolerance
|
|
||||||
) {
|
|
||||||
|
|
||||||
boolean timeFallback = (time != 0.501);
|
|
||||||
boolean posXFallback = (posX != 0.501);
|
|
||||||
boolean posYFallback = (posY != 0.501);
|
|
||||||
|
|
||||||
return new Action() {
|
|
||||||
|
|
||||||
double stamp = 0.0;
|
|
||||||
int ticker = 0;
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
|
||||||
|
|
||||||
drive.updatePoseEstimate();
|
|
||||||
Pose2d currentPose = drive.localizer.getPose();
|
|
||||||
|
|
||||||
if (ticker == 0) {
|
|
||||||
stamp = System.currentTimeMillis();
|
|
||||||
}
|
|
||||||
|
|
||||||
ticker++;
|
|
||||||
|
|
||||||
flywheel.manageFlywheel(vel);
|
|
||||||
robot.hood.setPosition(hoodPos);
|
|
||||||
|
|
||||||
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
|
|
||||||
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
|
|
||||||
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
|
|
||||||
|
|
||||||
boolean shouldFinish = timeDone || xDone || yDone;
|
|
||||||
|
|
||||||
return !shouldFinish;
|
|
||||||
|
|
||||||
}
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
public Action manageShooterAuto(
|
|
||||||
double time,
|
|
||||||
double posX,
|
|
||||||
double posY,
|
|
||||||
double posXTolerance,
|
|
||||||
double posYTolerance
|
|
||||||
) {
|
|
||||||
|
|
||||||
boolean timeFallback = (time != 0.501);
|
|
||||||
boolean posXFallback = (posX != 0.501);
|
|
||||||
boolean posYFallback = (posY != 0.501);
|
|
||||||
|
|
||||||
return new Action() {
|
|
||||||
|
|
||||||
double stamp = 0.0;
|
|
||||||
int ticker = 0;
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
|
||||||
|
|
||||||
drive.updatePoseEstimate();
|
|
||||||
Pose2d currentPose = drive.localizer.getPose();
|
|
||||||
|
|
||||||
if (ticker == 0) {
|
|
||||||
stamp = System.currentTimeMillis();
|
|
||||||
}
|
|
||||||
|
|
||||||
ticker++;
|
|
||||||
|
|
||||||
double robotX = drive.localizer.getPose().position.x;
|
|
||||||
double robotY = drive.localizer.getPose().position.y;
|
|
||||||
|
|
||||||
double robotHeading = drive.localizer.getPose().heading.toDouble();
|
|
||||||
|
|
||||||
double goalX = -15;
|
|
||||||
double goalY = 0;
|
|
||||||
|
|
||||||
double dx = robotX - goalX; // delta x from robot to goal
|
|
||||||
double dy = robotY - 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
|
|
||||||
(robotX, robotY, robotHeading, 0.0, false);
|
|
||||||
|
|
||||||
turret.trackGoal(deltaPose);
|
|
||||||
|
|
||||||
robot.hood.setPosition(targetingSettings.hoodAngle);
|
|
||||||
|
|
||||||
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
|
|
||||||
|
|
||||||
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
|
|
||||||
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
|
|
||||||
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
|
|
||||||
|
|
||||||
boolean shouldFinish = timeDone || xDone || yDone;
|
|
||||||
|
|
||||||
return !shouldFinish;
|
|
||||||
|
|
||||||
}
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
public Action manageFlywheelAuto(
|
|
||||||
double time,
|
|
||||||
double posX,
|
|
||||||
double posY,
|
|
||||||
double posXTolerance,
|
|
||||||
double posYTolerance
|
|
||||||
) {
|
|
||||||
|
|
||||||
boolean timeFallback = (time != 0.501);
|
|
||||||
boolean posXFallback = (posX != 0.501);
|
|
||||||
boolean posYFallback = (posY != 0.501);
|
|
||||||
|
|
||||||
return new Action() {
|
|
||||||
|
|
||||||
double stamp = 0.0;
|
|
||||||
int ticker = 0;
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
|
||||||
|
|
||||||
drive.updatePoseEstimate();
|
|
||||||
Pose2d currentPose = drive.localizer.getPose();
|
|
||||||
|
|
||||||
if (ticker == 0) {
|
|
||||||
stamp = System.currentTimeMillis();
|
|
||||||
}
|
|
||||||
|
|
||||||
ticker++;
|
|
||||||
|
|
||||||
double robotX = drive.localizer.getPose().position.x;
|
|
||||||
double robotY = drive.localizer.getPose().position.y;
|
|
||||||
|
|
||||||
double robotHeading = drive.localizer.getPose().heading.toDouble();
|
|
||||||
|
|
||||||
double goalX = -15;
|
|
||||||
double goalY = 0;
|
|
||||||
|
|
||||||
double dx = robotX - goalX; // delta x from robot to goal
|
|
||||||
double dy = robotY - 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
|
|
||||||
(robotX, robotY, robotHeading, 0.0, false);
|
|
||||||
|
|
||||||
|
|
||||||
robot.hood.setPosition(targetingSettings.hoodAngle);
|
|
||||||
|
|
||||||
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
|
|
||||||
|
|
||||||
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
|
|
||||||
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
|
|
||||||
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
|
|
||||||
|
|
||||||
boolean shouldFinish = timeDone || xDone || yDone;
|
|
||||||
|
|
||||||
return !shouldFinish;
|
|
||||||
|
|
||||||
}
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void runOpMode() throws InterruptedException {
|
|
||||||
|
|
||||||
hardwareMap.get(Servo.class, "light").setPosition(0);
|
|
||||||
|
|
||||||
robot = new Robot(hardwareMap);
|
|
||||||
|
|
||||||
TELE = new MultipleTelemetry(
|
|
||||||
telemetry, FtcDashboard.getInstance().getTelemetry()
|
|
||||||
);
|
|
||||||
|
|
||||||
flywheel = new Flywheel(hardwareMap);
|
|
||||||
|
|
||||||
targeting = new Targeting();
|
|
||||||
targetingSettings = new Targeting.Settings(0.0, 0.0);
|
|
||||||
|
|
||||||
spindexer = new Spindexer(hardwareMap);
|
|
||||||
|
|
||||||
servos = new Servos(hardwareMap);
|
|
||||||
|
|
||||||
robot.limelight.pipelineSwitch(1);
|
|
||||||
|
|
||||||
turret = new Turret(robot, TELE, robot.limelight);
|
|
||||||
|
|
||||||
turret.manualSetTurret(0.4);
|
|
||||||
|
|
||||||
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
|
|
||||||
|
|
||||||
TrajectoryActionBuilder shoot0 = null;
|
|
||||||
TrajectoryActionBuilder pickup1 = null;
|
|
||||||
TrajectoryActionBuilder shoot1 = null;
|
|
||||||
TrajectoryActionBuilder gatePickup = null;
|
|
||||||
TrajectoryActionBuilder shootCycle = null;
|
|
||||||
|
|
||||||
robot.spin1.setPosition(autoSpinStartPos);
|
|
||||||
robot.spin2.setPosition(1 - autoSpinStartPos);
|
|
||||||
|
|
||||||
robot.transferServo.setPosition(transferServo_out);
|
|
||||||
|
|
||||||
robot.light.setPosition(1);
|
|
||||||
|
|
||||||
while (opModeInInit()) {
|
|
||||||
|
|
||||||
robot.hood.setPosition(shoot0Hood);
|
|
||||||
|
|
||||||
if (gamepad2.crossWasPressed()) {
|
|
||||||
redAlliance = !redAlliance;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (redAlliance) {
|
|
||||||
robot.light.setPosition(0.28);
|
|
||||||
|
|
||||||
// ---- FIRST SHOT ----
|
|
||||||
x1 = rx1;
|
|
||||||
y1 = ry1;
|
|
||||||
h1 = rh1;
|
|
||||||
|
|
||||||
// ---- PICKUP PATH ----
|
|
||||||
x2a = rx2a;
|
|
||||||
y2a = ry2a;
|
|
||||||
h2a = rh2a;
|
|
||||||
t2a = rt2a;
|
|
||||||
x2b = rx2b;
|
|
||||||
y2b = ry2b;
|
|
||||||
h2b = rh2b;
|
|
||||||
t2b = rt2b;
|
|
||||||
x2c = rx2c;
|
|
||||||
y2c = ry2c;
|
|
||||||
h2c = rh2c;
|
|
||||||
t2c = rt2c;
|
|
||||||
|
|
||||||
xShoot = rShootX;
|
|
||||||
yShoot = rShootY;
|
|
||||||
hShoot = rShootH;
|
|
||||||
shoot1Tangent = rShoot1Tangent;
|
|
||||||
|
|
||||||
xGate = rXGate;
|
|
||||||
yGate = rYGate;
|
|
||||||
hGate = rHGate;
|
|
||||||
|
|
||||||
|
|
||||||
} else {
|
|
||||||
robot.light.setPosition(0.6);
|
|
||||||
|
|
||||||
// ---- FIRST SHOT ----
|
|
||||||
x1 = bx1;
|
|
||||||
y1 = by1;
|
|
||||||
h1 = bh1;
|
|
||||||
|
|
||||||
// ---- PICKUP PATH ----
|
|
||||||
x2a = bx2a;
|
|
||||||
y2a = by2a;
|
|
||||||
h2a = bh2a;
|
|
||||||
t2a = bt2a;
|
|
||||||
x2b = bx2b;
|
|
||||||
y2b = by2b;
|
|
||||||
h2b = bh2b;
|
|
||||||
t2b = bt2b;
|
|
||||||
x2c = bx2c;
|
|
||||||
y2c = by2c;
|
|
||||||
h2c = bh2c;
|
|
||||||
t2c = bt2c;
|
|
||||||
|
|
||||||
xShoot = bShootX;
|
|
||||||
yShoot = bShootY;
|
|
||||||
hShoot = bShootH;
|
|
||||||
|
|
||||||
shoot1Tangent = bShoot1Tangent;
|
|
||||||
|
|
||||||
xGate = bXGate;
|
|
||||||
yGate = bYGate;
|
|
||||||
hGate = bHGate;
|
|
||||||
}
|
|
||||||
|
|
||||||
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
|
|
||||||
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
|
|
||||||
|
|
||||||
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
|
||||||
.strafeToLinearHeading(new Vector2d(x2a, y2a), h2a)
|
|
||||||
.strafeToLinearHeading(new Vector2d(x2b, y2b), h2b,
|
|
||||||
new TranslationalVelConstraint(pickup1Speed));
|
|
||||||
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
|
|
||||||
.setReversed(true)
|
|
||||||
.splineTo(new Vector2d(x2a, y2a), shoot1Tangent)
|
|
||||||
.splineToSplineHeading(new Pose2d(xShoot, yShoot, hShoot), shoot1Tangent);
|
|
||||||
|
|
||||||
gatePickup = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
|
|
||||||
.strafeToLinearHeading(new Vector2d(xGate, yGate), hGate);
|
|
||||||
|
|
||||||
shootCycle = drive.actionBuilder(new Pose2d(xGate, yGate, hGate))
|
|
||||||
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
waitForStart();
|
|
||||||
|
|
||||||
if (isStopRequested()) return;
|
|
||||||
|
|
||||||
if (opModeIsActive()) {
|
|
||||||
|
|
||||||
robot.transfer.setPower(1);
|
|
||||||
|
|
||||||
assert shoot0 != null;
|
|
||||||
|
|
||||||
Actions.runBlocking(
|
|
||||||
new ParallelAction(
|
|
||||||
shoot0.build(),
|
|
||||||
manageFlywheel(
|
|
||||||
shoot0Vel,
|
|
||||||
shoot0Hood,
|
|
||||||
flywheel0Time,
|
|
||||||
x1,
|
|
||||||
0.501,
|
|
||||||
shoot0XTolerance,
|
|
||||||
0.501
|
|
||||||
)
|
|
||||||
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
Actions.runBlocking(
|
|
||||||
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
|
|
||||||
);
|
|
||||||
|
|
||||||
Actions.runBlocking(
|
|
||||||
new ParallelAction(
|
|
||||||
pickup1.build(),
|
|
||||||
manageFlywheel(
|
|
||||||
shootAllVelocity,
|
|
||||||
shootAllHood,
|
|
||||||
pickup1Time,
|
|
||||||
x2b,
|
|
||||||
y2b,
|
|
||||||
pickup1XTolerance,
|
|
||||||
pickup1YTolerance
|
|
||||||
),
|
|
||||||
intake(intake1Time),
|
|
||||||
detectObelisk(
|
|
||||||
obelisk1Time,
|
|
||||||
x2b,
|
|
||||||
y2c,
|
|
||||||
obelisk1XTolerance,
|
|
||||||
obelisk1YTolerance,
|
|
||||||
obeliskTurrPos
|
|
||||||
)
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
motif = turret.detectObelisk();
|
|
||||||
|
|
||||||
Actions.runBlocking(
|
|
||||||
new ParallelAction(
|
|
||||||
manageFlywheel(
|
|
||||||
shootAllVelocity,
|
|
||||||
shootAllHood,
|
|
||||||
shoot1Time,
|
|
||||||
0.501,
|
|
||||||
0.501,
|
|
||||||
0.501,
|
|
||||||
0.501
|
|
||||||
),
|
|
||||||
shoot1.build(),
|
|
||||||
prepareShootAll(shoot1Time)
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
Actions.runBlocking(
|
|
||||||
new ParallelAction(
|
|
||||||
manageShooterAuto(
|
|
||||||
shootAllTime,
|
|
||||||
0.501,
|
|
||||||
0.501,
|
|
||||||
0.501,
|
|
||||||
0.501
|
|
||||||
),
|
|
||||||
shootAllAuto(shootAllTime, spindexerSpeedIncrease)
|
|
||||||
)
|
|
||||||
|
|
||||||
);
|
|
||||||
|
|
||||||
while (opModeIsActive()) {
|
|
||||||
|
|
||||||
Actions.runBlocking(
|
|
||||||
new ParallelAction(
|
|
||||||
gatePickup.build(),
|
|
||||||
manageShooterAuto(
|
|
||||||
gatePickupTime,
|
|
||||||
x2b,
|
|
||||||
y2b,
|
|
||||||
pickup1XTolerance,
|
|
||||||
pickup1YTolerance
|
|
||||||
),
|
|
||||||
intake(gatePickupTime)
|
|
||||||
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
Actions.runBlocking(
|
|
||||||
new ParallelAction(
|
|
||||||
manageFlywheelAuto(
|
|
||||||
shootCycleTime,
|
|
||||||
0.501,
|
|
||||||
0.501,
|
|
||||||
0.501,
|
|
||||||
0.501
|
|
||||||
),
|
|
||||||
shootCycle.build(),
|
|
||||||
prepareShootAll(shootCycleTime)
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
Actions.runBlocking(
|
|
||||||
new ParallelAction(
|
|
||||||
manageShooterAuto(
|
|
||||||
shootAllTime,
|
|
||||||
0.501,
|
|
||||||
0.501,
|
|
||||||
0.501,
|
|
||||||
0.501
|
|
||||||
),
|
|
||||||
shootAllAuto(shootAllTime, spindexerSpeedIncrease)
|
|
||||||
)
|
|
||||||
|
|
||||||
);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -98,7 +98,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
|
|||||||
public static double autoSpinStartPos = 0.2;
|
public static double autoSpinStartPos = 0.2;
|
||||||
public static double shoot0SpinSpeedIncrease = 0.015;
|
public static double shoot0SpinSpeedIncrease = 0.015;
|
||||||
|
|
||||||
public static double gateIntakeTime = 5.0;
|
public static double gateIntakeTime = 5.0; //TODO: Increase ig
|
||||||
|
|
||||||
public static double spindexerSpeedIncrease = 0.03;
|
public static double spindexerSpeedIncrease = 0.03;
|
||||||
|
|
||||||
@@ -113,7 +113,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
|
|||||||
public static double shoot0Time = 1.6;
|
public static double shoot0Time = 1.6;
|
||||||
public static double intake1Time = 3.0;
|
public static double intake1Time = 3.0;
|
||||||
public static double flywheel0Time = 3.5;
|
public static double flywheel0Time = 3.5;
|
||||||
public static double pickup1Speed = 19;
|
public static double pickup1Speed = 25;
|
||||||
// ---- SECOND SHOT / PICKUP ----
|
// ---- SECOND SHOT / PICKUP ----
|
||||||
public static double shoot1Vel = 2300;
|
public static double shoot1Vel = 2300;
|
||||||
public static double shoot1Hood = 0.93;
|
public static double shoot1Hood = 0.93;
|
||||||
@@ -137,7 +137,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
|
|||||||
|
|
||||||
public static double firstShootTime = 0.3;
|
public static double firstShootTime = 0.3;
|
||||||
public static double shootGateTime = 3.0;
|
public static double shootGateTime = 3.0;
|
||||||
public static double gaitWait = 0.8;
|
public static double gaitWait = 0.45;
|
||||||
public int motif = 0;
|
public int motif = 0;
|
||||||
|
|
||||||
Robot robot;
|
Robot robot;
|
||||||
@@ -321,7 +321,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
|
|||||||
|
|
||||||
drive.updatePoseEstimate();
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
Poses_V2.teleStart = drive.localizer.getPose();
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
robot.intake.setPower(-0.3);
|
robot.intake.setPower(-0.3);
|
||||||
|
|
||||||
@@ -385,7 +385,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
|
|||||||
|
|
||||||
drive.updatePoseEstimate();
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
Poses_V2.teleStart = drive.localizer.getPose();
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
robot.intake.setPower(-0.3);
|
robot.intake.setPower(-0.3);
|
||||||
|
|
||||||
|
|||||||
@@ -24,7 +24,7 @@ public class Poses {
|
|||||||
|
|
||||||
public static double rx3aG = 60, ry3aG = 34, rh3aG = Math.toRadians(140);
|
public static double rx3aG = 60, ry3aG = 34, rh3aG = Math.toRadians(140);
|
||||||
|
|
||||||
public static double rx3b = 38, ry3b = 56, rh3b = Math.toRadians(140.1);
|
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 rx4a = 75, ry4a = 53, rh4a = Math.toRadians(140);
|
||||||
public static double rx4b = 45, ry4b = 83, rh4b = Math.toRadians(140.1);
|
public static double rx4b = 45, ry4b = 83, rh4b = Math.toRadians(140.1);
|
||||||
|
|||||||
@@ -10,55 +10,16 @@ public class Poses_V2 {
|
|||||||
|
|
||||||
public static double turretHeight = 12;
|
public static double turretHeight = 12;
|
||||||
|
|
||||||
public static double relativeGoalHeight = goalHeight - turretHeight;
|
public static double rXGateA = 35.5, rYGateA = 46.5, rHGateA = 2.7;
|
||||||
|
|
||||||
public static Pose2d goalPose = new Pose2d(-10, 0, 0);
|
public static double rXGateB = 56, rYGateB = 37, rHGateB = 2.7750735106709836;
|
||||||
|
|
||||||
public static double rx1 = 20, ry1 = 0, rh1 = 0;
|
|
||||||
public static double rx2a = 55, ry2a = 39, rh2a = Math.toRadians(140), rt2a = Math.toRadians(Math.PI/2);
|
|
||||||
public static double rx2b = 33, ry2b = 61, rh2b = Math.toRadians(140), rt2b = Math.toRadians(Math.PI/2);
|
|
||||||
|
|
||||||
public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140), rt2c = Math.toRadians(Math.PI/2);
|
|
||||||
|
|
||||||
public static double rXGateA = 27, rYGateA = 56, rHGateA = Math.toRadians(160);
|
|
||||||
|
|
||||||
public static double rXGateB = 40, rYGateB = 43, rHGateB = Math.toRadians(159);
|
|
||||||
|
|
||||||
public static double rXGate = 30, rYGate = 63, rHGate = Math.toRadians(179);
|
public static double rXGate = 30, rYGate = 63, rHGate = Math.toRadians(179);
|
||||||
|
|
||||||
public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
|
|
||||||
public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140);
|
|
||||||
|
|
||||||
public static double rx4a = 72, ry4a = 55, rh4a = Math.toRadians(140);
|
|
||||||
public static double rx4b = 48, ry4b = 79, rh4b = Math.toRadians(140);
|
|
||||||
|
|
||||||
public static double bx1 = 20, by1 = 0, bh1 = 0;
|
|
||||||
public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140), bt2a = Math.toRadians(140);
|
|
||||||
public static double bx2b = 25, by2b = -38, bh2b = Math.toRadians(-140), bt2b = Math.toRadians(140);
|
|
||||||
public static double bx2c = 34, by2c = -50, bh2c = Math.toRadians(-140), bt2c = Math.toRadians(140);
|
|
||||||
|
|
||||||
public static double rShootX = 40, rShootY = 7, rShootH = Math.toRadians(140);
|
|
||||||
|
|
||||||
public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140);
|
|
||||||
|
|
||||||
public static double bXGateA = 33, bYGateA = 61, bHGateA = Math.toRadians(165);
|
public static double bXGateA = 33, bYGateA = 61, bHGateA = Math.toRadians(165);
|
||||||
public static double bXGateB = 33, bYGateB = 61, bHGateB = Math.toRadians(165);
|
public static double bXGateB = 33, bYGateB = 61, bHGateB = Math.toRadians(165);
|
||||||
|
|
||||||
public static double bXGate = 25, bYGate = 69, bHGate = Math.toRadians(165);
|
public static double bXGate = 25, bYGate = 69, bHGate = Math.toRadians(165);
|
||||||
|
|
||||||
|
|
||||||
public static double bx3a = 55, by3a = -43, bh3a = Math.toRadians(-140);
|
|
||||||
public static double bx3b = 37, by3b = -61, bh3b = Math.toRadians(-140);
|
|
||||||
|
|
||||||
public static double bx4a = 72, by4a = -55, bh4a = Math.toRadians(-140);
|
|
||||||
public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140);
|
|
||||||
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
|
|
||||||
|
|
||||||
public static double rShoot1Tangent = Math.toRadians(0);
|
|
||||||
public static double bShoot1Tangent = Math.toRadians(0);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
public static Pose2d teleStart = new Pose2d(0, 0, 0);
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -329,6 +329,8 @@ public class TeleopV3 extends LinearOpMode {
|
|||||||
// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
|
// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
|
||||||
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
|
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
|
||||||
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
|
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
|
||||||
|
//
|
||||||
|
// TELE.update();
|
||||||
|
|
||||||
ticker++;
|
ticker++;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,41 +1,15 @@
|
|||||||
package org.firstinspires.ftc.teamcode.utils;
|
package org.firstinspires.ftc.teamcode.utils;
|
||||||
|
|
||||||
import android.provider.Settings;
|
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||||
|
|
||||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
|
||||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
|
||||||
|
|
||||||
public class Targeting {
|
public class Targeting {
|
||||||
MultipleTelemetry TELE;
|
|
||||||
|
|
||||||
double cancelOffsetX = 0.0; // was -40.0
|
|
||||||
double cancelOffsetY = 0.0; // was 7.0
|
|
||||||
double unitConversionFactor = 0.95;
|
|
||||||
|
|
||||||
int tileSize = 24; //inches
|
|
||||||
public final int TILE_UPPER_QUARTILE = 18;
|
|
||||||
public final int TILE_LOWER_QUARTILE = 6;
|
|
||||||
|
|
||||||
public double robotInchesX, robotInchesY = 0.0;
|
|
||||||
|
|
||||||
public int robotGridX, robotGridY = 0;
|
|
||||||
|
|
||||||
|
|
||||||
public static class Settings {
|
|
||||||
public double flywheelRPM = 0.0;
|
|
||||||
public double hoodAngle = 0.0;
|
|
||||||
|
|
||||||
public Settings (double flywheelRPM, double hoodAngle) {
|
|
||||||
this.flywheelRPM = flywheelRPM;
|
|
||||||
this.hoodAngle = hoodAngle;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Known settings discovered using shooter test.
|
// Known settings discovered using shooter test.
|
||||||
// Keep the fidelity at 1 floor tile for now but we could also half it if more
|
// Keep the fidelity at 1 floor tile for now but we could also half it if more
|
||||||
// accuracy is needed.
|
// accuracy is needed.
|
||||||
public static final Settings[][] KNOWNTARGETING;
|
public static final Settings[][] KNOWNTARGETING;
|
||||||
|
|
||||||
static {
|
static {
|
||||||
KNOWNTARGETING = new Settings[6][6];
|
KNOWNTARGETING = new Settings[6][6];
|
||||||
// ROW 0 - Closet to the goals
|
// ROW 0 - Closet to the goals
|
||||||
@@ -89,10 +63,20 @@ public class Targeting {
|
|||||||
KNOWNTARGETING[5][3] = new Settings(3350, 0.48);
|
KNOWNTARGETING[5][3] = new Settings(3350, 0.48);
|
||||||
KNOWNTARGETING[5][4] = new Settings(3350, 0.48);
|
KNOWNTARGETING[5][4] = new Settings(3350, 0.48);
|
||||||
KNOWNTARGETING[5][5] = new Settings(3350, 0.48);
|
KNOWNTARGETING[5][5] = new Settings(3350, 0.48);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public Targeting()
|
public final int TILE_UPPER_QUARTILE = 18;
|
||||||
{
|
public final int TILE_LOWER_QUARTILE = 6;
|
||||||
|
public double robotInchesX, robotInchesY = 0.0;
|
||||||
|
public int robotGridX, robotGridY = 0;
|
||||||
|
MultipleTelemetry TELE;
|
||||||
|
double cancelOffsetX = 0.0; // was -40.0
|
||||||
|
double cancelOffsetY = 0.0; // was 7.0
|
||||||
|
double unitConversionFactor = 0.95;
|
||||||
|
int tileSize = 24; //inches
|
||||||
|
|
||||||
|
public Targeting() {
|
||||||
}
|
}
|
||||||
|
|
||||||
public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) {
|
public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) {
|
||||||
@@ -186,8 +170,14 @@ public class Targeting {
|
|||||||
}
|
}
|
||||||
|
|
||||||
//clamp
|
//clamp
|
||||||
|
|
||||||
|
if (redAlliance) {
|
||||||
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
|
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
|
||||||
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
|
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
|
||||||
|
} else {
|
||||||
|
robotGridY = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
|
||||||
|
robotGridX = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
|
||||||
|
}
|
||||||
|
|
||||||
// basic search
|
// basic search
|
||||||
if (true) { //!interpolate) {
|
if (true) { //!interpolate) {
|
||||||
@@ -225,4 +215,14 @@ public class Targeting {
|
|||||||
return recommendedSettings;
|
return recommendedSettings;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public static class Settings {
|
||||||
|
public double flywheelRPM = 0.0;
|
||||||
|
public double hoodAngle = 0.0;
|
||||||
|
|
||||||
|
public Settings(double flywheelRPM, double hoodAngle) {
|
||||||
|
this.flywheelRPM = flywheelRPM;
|
||||||
|
this.hoodAngle = hoodAngle;
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user