KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Merge branch 'auto9Ball'

Browse Source 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_12Ball.java
This commit is contained in:
Keshav Anand
2026-01-29 15:21:33 -06:00
parent 6c6ea03cac f8369b51ed
commit 6b71bb17f4
12 changed files with 2347 additions and 1207 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1688
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_12Ball.java
View File

File diff suppressed because it is too large Load Diff

921
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java Normal file
View File

@@ -0,0 +1,921 @@
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.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 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;
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;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class ProtoAutoClose_V4 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 normalIntakeTime = 3.0;
public static double shoot1Turr = 0.57;
public static double shoot0XTolerance = 1.0;
public static double turretShootPos = 0.53;
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 = 25;
// ---- 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 x3a, y3a, h3a;
private double x3b, y3b, h3b;
private double x4a, y4a, h4a;
private double x4b, y4b, h4b;
private double xShoot, yShoot, hShoot;
private double xGate, yGate, hGate;
private double xPrep, yPrep, hPrep;
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);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
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", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
drive.updatePoseEstimate();
Poses_V2.teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
robot.intake.setPower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
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", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
velo = flywheel.getVelo();
drive.updatePoseEstimate();
Poses_V2.teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
robot.intake.setPower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
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();
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);
}
};
}
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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
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;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return !shouldFinish;
}
};
}
@Override
public void runOpMode() throws InterruptedException {
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));
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
robot.transferServo.setPosition(transferServo_out);
TrajectoryActionBuilder shoot0 = null;
TrajectoryActionBuilder pickup1 = null;
TrajectoryActionBuilder shoot1 = null;
TrajectoryActionBuilder pickup2 = null;
TrajectoryActionBuilder shoot2 = null;
TrajectoryActionBuilder pickup3 = null;
TrajectoryActionBuilder shoot3 = null;
robot.limelight.start();
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;
x2b = rx2b;
y2b = ry2b;
h2b = rh2b;
x3a = rx3a;
y3a = ry3a;
h3a = rh3a;
x3b = rx3b;
y3b = ry3b;
h3b = rh3b;
x4a = rx4a;
y4a = ry4a;
h4a = rh4a;
x4b = rx4b;
y4b = ry4b;
h4b = rh4b;
xPrep = rxPrep;
yPrep = ryPrep;
hPrep = rhPrep;
xShoot = rShootX;
yShoot = rShootY;
hShoot = rShootH;
} else {
robot.light.setPosition(0.6);
// ---- FIRST SHOT ----
x1 = bx1;
y1 = by1;
h1 = bh1;
// ---- PICKUP PATH ----
x2a = bx2a;
y2a = by2a;
h2a = bh2a;
x2b = bx2b;
y2b = by2b;
h2b = bh2b;
x3a = bx3a;
y3a = by3a;
h3a = bh3a;
x3b = bx3b;
y3b = by3b;
h3b = bh3b;
x4a = bx4a;
y4a = by4a;
h4a = bh4a;
x4b = bx4b;
y4b = by4b;
h4b = bh4b;
xPrep = bxPrep;
yPrep = byPrep;
hPrep = bhPrep;
xShoot = bShootX;
yShoot = bShootY;
hShoot = bShootH;
}
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))
.strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
.strafeToLinearHeading(new Vector2d(x3a, y3a), h3a)
.strafeToLinearHeading(new Vector2d(x3b, y3b), h3b,
new TranslationalVelConstraint(pickup1Speed));
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b))
.strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x4a, y4a), h4a)
.strafeToLinearHeading(new Vector2d(x4b, y4b), h4b,
new TranslationalVelConstraint(pickup1Speed));
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b))
.strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
TELE.addData("Red?", redAlliance);
TELE.update();
}
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)
)
);
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)
)
);
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
manageShooterAuto(
normalIntakeTime,
x2b,
y2b,
pickup1XTolerance,
pickup1YTolerance
),
intake(normalIntakeTime)
)
);
Actions.runBlocking(
new ParallelAction(
manageFlywheelAuto(
shootCycleTime,
0.501,
0.501,
0.501,
0.501
),
shoot2.build(),
prepareShootAll(shootCycleTime)
)
);
Actions.runBlocking(
new ParallelAction(
manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501
),
shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
);
Actions.runBlocking(
new ParallelAction(
pickup3.build(),
manageShooterAuto(
normalIntakeTime,
x2b,
y2b,
pickup1XTolerance,
pickup1YTolerance
),
intake(normalIntakeTime)
)
);
Actions.runBlocking(
new ParallelAction(
manageFlywheelAuto(
shootCycleTime,
0.501,
0.501,
0.501,
0.501
),
shoot3.build(),
prepareShootAll(shootCycleTime)
)
);
Actions.runBlocking(
new ParallelAction(
manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501
),
shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
);
TELE.addLine("finished");
TELE.update();
sleep(2000);
}
}
}

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java
View File

@@ -1,6 +1,11 @@
package org.firstinspires.ftc.teamcode.constants; package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
@Config
public class Color { public class Color {
public static boolean redAlliance = true; public static boolean redAlliance = true;
public static double Light0 = 0.28, Light1 = 0.67, Light2 = 0.36, Light3 = 0.5; public static double Light0 = 0.28, Light1 = 0.67, Light2 = 0.36, Light3 = 0.5;
public static double colorFilterAlpha = 0.15;
} }

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
View File

@@ -14,17 +14,17 @@ public class Poses {
public static Pose2d goalPose = new Pose2d(-10, 0, 0); public static Pose2d goalPose = new Pose2d(-10, 0, 0);
public static double rx1 = 40, ry1 = -7, rh1 = 0; 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 rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140);
public static double rx2b = 23, ry2b = 36, rh2b = Math.toRadians(140); public static double rx2b = 23, ry2b = 36, rh2b = Math.toRadians(140.1);
public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140); public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140);
public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); 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 rx3b = 38, ry3b = 56, rh3b = Math.toRadians(140.1);
public static double rx4a = 72, ry4a = 55, rh4a = Math.toRadians(140); public static double rx4a = 72, ry4a = 65, rh4a = Math.toRadians(145);
public static double rx4b = 48, ry4b = 79, rh4b = Math.toRadians(140); public static double rx4b = 37, ry4b = 85, rh4b = Math.toRadians(145.1);
public static double bx1 = 40, by1 = 7, bh1 = 0; public static double bx1 = 40, by1 = 7, bh1 = 0;
public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140); public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140);
@@ -38,6 +38,13 @@ public class Poses {
public static double bx4b = 48, by4b = -79, bh4b = 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 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 = 20, bShootY = 30, bShootH = Math.toRadians(140);
public static double bxPrep = 50, byPrep = -10, bhPrep = Math.toRadians(140);
public static Pose2d teleStart = new Pose2d(0, 0, 0); public static Pose2d teleStart = new Pose2d(0, 0, 0);
} }

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -5,18 +5,21 @@ import com.acmerobotics.dashboard.config.Config;
@Config @Config
public class ServoPositions { public class ServoPositions {
public static double spindexer_intakePos1 = 0; public static double spindexer_intakePos1 = 0.05; //0.13;
public static double spindexer_intakePos2 = 0.19;//0.5; public static double spindexer_intakePos2 = 0.24; //0.33;//0.5;
public static double spindexer_intakePos3 = 0.38;//0.66; public static double spindexer_intakePos3 = 0.43; //0.53;//0.66;
public static double spindexer_outtakeBall3 = 0.65; public static double spindexer_outtakeBall3 = 0.71; //0.65; //0.24;
public static double spindexer_outtakeBall2 = 0.46; public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.27; public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4;
public static double spinStartPos = spindexer_outtakeBall1 - 0.08; public static double spinStartPos = spindexer_outtakeBall3 - 0.1;
public static double shootAllAutoSpinStartPos = 0.2;
public static double shootAllSpindexerSpeedIncrease = 0.02;
public static double shootAllTime = 1.8;
public static double transferServo_out = 0.15; public static double transferServo_out = 0.15;

279
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java
View File

@@ -5,9 +5,7 @@ import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.canvas.Canvas; import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.AccelConstraint; import com.acmerobotics.roadrunner.*;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.Actions;
import com.acmerobotics.roadrunner.AngularVelConstraint; import com.acmerobotics.roadrunner.AngularVelConstraint;
import com.acmerobotics.roadrunner.DualNum; import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.HolonomicController; import com.acmerobotics.roadrunner.HolonomicController;
@@ -16,20 +14,12 @@ import com.acmerobotics.roadrunner.MinVelConstraint;
import com.acmerobotics.roadrunner.MotorFeedforward; import com.acmerobotics.roadrunner.MotorFeedforward;
import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Pose2dDual; import com.acmerobotics.roadrunner.Pose2dDual;
import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.acmerobotics.roadrunner.PoseVelocity2dDual;
import com.acmerobotics.roadrunner.ProfileAccelConstraint; import com.acmerobotics.roadrunner.ProfileAccelConstraint;
import com.acmerobotics.roadrunner.ProfileParams;
import com.acmerobotics.roadrunner.Rotation2d;
import com.acmerobotics.roadrunner.Time; import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.TimeTrajectory; import com.acmerobotics.roadrunner.TimeTrajectory;
import com.acmerobotics.roadrunner.TimeTurn; import com.acmerobotics.roadrunner.TimeTurn;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TrajectoryBuilderParams;
import com.acmerobotics.roadrunner.TurnConstraints; import com.acmerobotics.roadrunner.TurnConstraints;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.VelConstraint; import com.acmerobotics.roadrunner.VelConstraint;
import com.acmerobotics.roadrunner.ftc.DownsampledWriter; import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
import com.acmerobotics.roadrunner.ftc.Encoder; import com.acmerobotics.roadrunner.ftc.Encoder;
@@ -56,131 +46,13 @@ import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumCommandMessage;
import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumLocalizerInputsMessage; import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumLocalizerInputsMessage;
import org.firstinspires.ftc.teamcode.libs.RR.messages.PoseMessage; import org.firstinspires.ftc.teamcode.libs.RR.messages.PoseMessage;
import java.lang.Math;
import java.util.Arrays; import java.util.Arrays;
import java.util.LinkedList; import java.util.LinkedList;
import java.util.List; import java.util.List;
@Config @Config
public final class MecanumDrive { public final class MecanumDrive {
public static Params PARAMS = new Params();
public final MecanumKinematics kinematics = new MecanumKinematics(
PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick);
public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
public final VelConstraint defaultVelConstraint =
new MinVelConstraint(Arrays.asList(
kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
new AngularVelConstraint(PARAMS.maxAngVel)
));
public final AccelConstraint defaultAccelConstraint =
new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);
public final DcMotorEx leftFront, leftBack, rightBack, rightFront;
public final VoltageSensor voltageSensor;
public final LazyImu lazyImu;
public final Localizer localizer;
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000);
public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
// TODO: make sure your config has motors with these names (or change them)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
leftFront = hardwareMap.get(DcMotorEx.class, "fl");
leftBack = hardwareMap.get(DcMotorEx.class, "bl");
rightBack = hardwareMap.get(DcMotorEx.class, "br");
rightFront = hardwareMap.get(DcMotorEx.class, "fr");
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// TODO: reverse motor directions if needed
//
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
// TODO: make sure your config has an IMU with this name (can be BNO or BHI)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
voltageSensor = hardwareMap.voltageSensor.iterator().next();
localizer = new PinpointLocalizer(hardwareMap, PARAMS.inPerTick, pose);
FlightRecorder.write("MECANUM_PARAMS", PARAMS);
}
public void setDrivePowers(PoseVelocity2d powers) {
MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(
PoseVelocity2dDual.constant(powers, 1));
double maxPowerMag = 1;
for (DualNum<Time> power : wheelVels.all()) {
maxPowerMag = Math.max(maxPowerMag, power.value());
}
leftFront.setPower(wheelVels.leftFront.get(0) / maxPowerMag);
leftBack.setPower(wheelVels.leftBack.get(0) / maxPowerMag);
rightBack.setPower(wheelVels.rightBack.get(0) / maxPowerMag);
rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag);
}
public PoseVelocity2d updatePoseEstimate() {
PoseVelocity2d vel = localizer.update();
poseHistory.add(localizer.getPose());
while (poseHistory.size() > 100) {
poseHistory.removeFirst();
}
estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
return vel;
}
private void drawPoseHistory(Canvas c) {
double[] xPoints = new double[poseHistory.size()];
double[] yPoints = new double[poseHistory.size()];
int i = 0;
for (Pose2d t : poseHistory) {
xPoints[i] = t.position.x;
yPoints[i] = t.position.y;
i++;
}
c.setStrokeWidth(1);
c.setStroke("#3F51B5");
c.strokePolyline(xPoints, yPoints);
}
public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
return new TrajectoryActionBuilder(
TurnAction::new,
FollowTrajectoryAction::new,
new TrajectoryBuilderParams(
1e-6,
new ProfileParams(
0.25, 0.1, 1e-2
)
),
beginPose, 0.0,
defaultTurnConstraints,
defaultVelConstraint, defaultAccelConstraint
);
}
public static class Params { public static class Params {
// IMU orientation // IMU orientation
// TODO: fill in these values based on // TODO: fill in these values based on
@@ -201,13 +73,13 @@ public final class MecanumDrive {
public double kA = 0.000046; public double kA = 0.000046;
// path profile parameters (in inches) // path profile parameters (in inches)
public double maxWheelVel = 250; public double maxWheelVel = 180;
public double minProfileAccel = -40; public double minProfileAccel = -40;
public double maxProfileAccel = 250; public double maxProfileAccel = 180;
// turn profile parameters (in radians) // turn profile parameters (in radians)
public double maxAngVel = 4 * Math.PI; // shared with path public double maxAngVel = 4* Math.PI; // shared with path
public double maxAngAccel = 4 * Math.PI; public double maxAngAccel = 4* Math.PI;
// path controller gains // path controller gains
public double axialGain = 4; public double axialGain = 4;
@@ -219,6 +91,35 @@ public final class MecanumDrive {
public double headingVelGain = 0.0; // shared with turn public double headingVelGain = 0.0; // shared with turn
} }
public static Params PARAMS = new Params();
public final MecanumKinematics kinematics = new MecanumKinematics(
PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick);
public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
public final VelConstraint defaultVelConstraint =
new MinVelConstraint(Arrays.asList(
kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
new AngularVelConstraint(PARAMS.maxAngVel)
));
public final AccelConstraint defaultAccelConstraint =
new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);
public final DcMotorEx leftFront, leftBack, rightBack, rightFront;
public final VoltageSensor voltageSensor;
public final LazyImu lazyImu;
public final Localizer localizer;
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000);
public class DriveLocalizer implements Localizer { public class DriveLocalizer implements Localizer {
public final Encoder leftFront, leftBack, rightBack, rightFront; public final Encoder leftFront, leftBack, rightBack, rightFront;
public final IMU imu; public final IMU imu;
@@ -243,13 +144,13 @@ public final class MecanumDrive {
} }
@Override @Override
public Pose2d getPose() { public void setPose(Pose2d pose) {
return pose; this.pose = pose;
} }
@Override @Override
public void setPose(Pose2d pose) { public Pose2d getPose() {
this.pose = pose; return pose;
} }
@Override @Override
@@ -315,11 +216,64 @@ public final class MecanumDrive {
} }
} }
public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
// TODO: make sure your config has motors with these names (or change them)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
leftFront = hardwareMap.get(DcMotorEx.class, "fl");
leftBack = hardwareMap.get(DcMotorEx.class, "bl");
rightBack = hardwareMap.get(DcMotorEx.class, "br");
rightFront = hardwareMap.get(DcMotorEx.class, "fr");
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// TODO: reverse motor directions if needed
//
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
// TODO: make sure your config has an IMU with this name (can be BNO or BHI)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
voltageSensor = hardwareMap.voltageSensor.iterator().next();
localizer = new PinpointLocalizer(hardwareMap, PARAMS.inPerTick, pose);
FlightRecorder.write("MECANUM_PARAMS", PARAMS);
}
public void setDrivePowers(PoseVelocity2d powers) {
MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(
PoseVelocity2dDual.constant(powers, 1));
double maxPowerMag = 1;
for (DualNum<Time> power : wheelVels.all()) {
maxPowerMag = Math.max(maxPowerMag, power.value());
}
leftFront.setPower(wheelVels.leftFront.get(0) / maxPowerMag);
leftBack.setPower(wheelVels.leftBack.get(0) / maxPowerMag);
rightBack.setPower(wheelVels.rightBack.get(0) / maxPowerMag);
rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag);
}
public final class FollowTrajectoryAction implements Action { public final class FollowTrajectoryAction implements Action {
public final TimeTrajectory timeTrajectory; public final TimeTrajectory timeTrajectory;
private final double[] xPoints, yPoints;
private double beginTs = -1; private double beginTs = -1;
private final double[] xPoints, yPoints;
public FollowTrajectoryAction(TimeTrajectory t) { public FollowTrajectoryAction(TimeTrajectory t) {
timeTrajectory = t; timeTrajectory = t;
@@ -496,4 +450,51 @@ public final class MecanumDrive {
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2); c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
} }
} }
public PoseVelocity2d updatePoseEstimate() {
PoseVelocity2d vel = localizer.update();
poseHistory.add(localizer.getPose());
while (poseHistory.size() > 100) {
poseHistory.removeFirst();
}
estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
return vel;
}
private void drawPoseHistory(Canvas c) {
double[] xPoints = new double[poseHistory.size()];
double[] yPoints = new double[poseHistory.size()];
int i = 0;
for (Pose2d t : poseHistory) {
xPoints[i] = t.position.x;
yPoints[i] = t.position.y;
i++;
}
c.setStrokeWidth(1);
c.setStroke("#3F51B5");
c.strokePolyline(xPoints, yPoints);
}
public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
return new TrajectoryActionBuilder(
TurnAction::new,
FollowTrajectoryAction::new,
new TrajectoryBuilderParams(
1e-6,
new ProfileParams(
0.25, 0.1, 1e-2
)
),
beginPose, 0.0,
defaultTurnConstraints,
defaultVelConstraint, defaultAccelConstraint
);
}
} }

255
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -1,8 +1,13 @@
package org.firstinspires.ftc.teamcode.teleop; package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; 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.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinD; import static org.firstinspires.ftc.teamcode.utils.Servos.spinD;
@@ -27,6 +32,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -122,7 +128,8 @@ public class TeleopV3 extends LinearOpMode {
private int tickerA = 1; private int tickerA = 1;
private boolean transferIn = false; private boolean transferIn = false;
boolean turretInterpolate = false; boolean turretInterpolate = false;
public static double spinSpeedIncrease = 0.04; public static double spinSpeedIncrease = 0.03;
public static int resetSpinTicks = 4;
public static double velPrediction(double distance) { public static double velPrediction(double distance) {
if (distance < 30) { if (distance < 30) {
@@ -172,12 +179,24 @@ public class TeleopV3 extends LinearOpMode {
Turret turret = new Turret(robot, TELE, robot.limelight); Turret turret = new Turret(robot, TELE, robot.limelight);
robot.light.setPosition(1); robot.light.setPosition(1);
while (opModeInInit()){
robot.limelight.start();
if (redAlliance) {
robot.limelight.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
}
}
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
while (opModeIsActive()) { while (opModeIsActive()) {
//LIMELIGHT START
TELE.addData("Is limelight on?", robot.limelight.getStatus());
// LIGHT COLORS // LIGHT COLORS
spindexer.ballCounterLight(); spindexer.ballCounterLight();
@@ -245,56 +264,56 @@ public class TeleopV3 extends LinearOpMode {
} }
if (autoSpintake) { // if (autoSpintake) {
if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
robot.spin1.setPosition(spindexPos);
robot.spin2.setPosition(1-spindexPos);
}
if (gamepad1.right_bumper) {
shootAll = false;
intakeTicker++;
// if (intakeTicker % 20 < 2) {
// //
// robot.spin1.setPower(-1); // if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
// robot.spin2.setPower(1); //
//
// robot.spin1.setPosition(spindexPos);
// robot.spin2.setPosition(1-spindexPos);
// //
// } else if (intakeTicker % 20 < 10) {
// robot.spin1.setPower(-0.5);
// robot.spin2.setPower(0.5);
// } else if (intakeTicker % 20 < 12) {
// robot.spin1.setPower(1);
// robot.spin2.setPower(-1);
// } else {
// robot.spin1.setPower(0.5);
// robot.spin2.setPower(-0.5);
// } // }
//
robot.intake.setPower(1); // if (gamepad1.right_bumper) {
intakeStamp = getRuntime(); //
TELE.addData("Reverse?", reverse); // shootAll = false;
TELE.update(); //
} else { // intakeTicker++;
if (!servo.spinEqual(spindexPos)) { //
robot.spin1.setPosition(spindexPos); //// if (intakeTicker % 20 < 2) {
robot.spin2.setPosition(1-spindexPos); ////
//// robot.spin1.setPower(-1);
} //// robot.spin2.setPower(1);
////
spindexPos = spindexer_intakePos1; //// } else if (intakeTicker % 20 < 10) {
//// robot.spin1.setPower(-0.5);
robot.intake.setPower(0); //// robot.spin2.setPower(0.5);
//// } else if (intakeTicker % 20 < 12) {
intakeTicker = 0; //// robot.spin1.setPower(1);
} //// robot.spin2.setPower(-1);
} //// } else {
//// robot.spin1.setPower(0.5);
//// robot.spin2.setPower(-0.5);
//// }
//
// robot.intake.setPower(1);
// intakeStamp = getRuntime();
// TELE.addData("Reverse?", reverse);
// TELE.update();
// } else {
// if (!servo.spinEqual(spindexPos)) {
// robot.spin1.setPosition(spindexPos);
// robot.spin2.setPosition(1-spindexPos);
//
// }
//
// spindexPos = spindexer_intakePos1;
//
// robot.intake.setPower(0);
//
// intakeTicker = 0;
// }
// }
//COLOR: //COLOR:
@@ -582,58 +601,96 @@ public class TeleopV3 extends LinearOpMode {
// } // }
if (enableSpindexerManager) { if (enableSpindexerManager) {
if (!shootAll) { //if (!shootAll) {
spindexer.processIntake(); spindexer.processIntake();
} //}
// RIGHT_BUMPER // RIGHT_BUMPER
if (gamepad1.right_bumper) { if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
robot.intake.setPower(1); robot.intake.setPower(1);
} else { } else {
robot.intake.setPower(0); robot.intake.setPower(0);
} }
// LEFT_BUMPER // LEFT_BUMPER
if (!shootAll && if (!shootAll && gamepad1.leftBumperWasReleased()) {
(gamepad1.leftBumperWasReleased() ||
gamepad1.leftBumperWasPressed() ||
gamepad1.left_bumper)) {
shootStamp = getRuntime(); shootStamp = getRuntime();
shootAll = true; shootAll = true;
shooterTicker = 0; shooterTicker = 0;
spindexPos = spinStartPos;// TODO: Change starting position based on desired order to shoot green ball
} }
intakeTicker++;
if (shootAll) { if (shootAll) {
intakeTicker = 0;
intake = false; intake = false;
reject = false; reject = false;
if (getRuntime() - shootStamp < 3.5) { // if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) {
//
// if (shooterTicker == 0){
// robot.transferServo.setPosition(transferServo_out);
// robot.spin1.setPosition(spinStartPos);
// robot.spin2.setPosition(1-spinStartPos);
// if (servo.spinEqual(spinStartPos)){
// shooterTicker++;
// }
// TELE.addLine("starting to shoot");
// } else {
// robot.transferServo.setPosition(transferServo_in);
// shooterTicker++;
// double prevSpinPos = servo.getSpinPos();
// robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease);
// robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
// TELE.addLine("shooting");
// }
if (shooterTicker == 0 && !servo.spinEqual(spindexPos)){ // //robot.intake.setPower(-0.3);
robot.spin1.setPosition(spindexPos); // if (getRuntime() - shootStamp < 3.0) {
robot.spin2.setPosition(1-spindexPos); //
} else { // if (shooterTicker == 0 && !servo.spinEqual(ServoPositions.shootAllAutoSpinStartPos)) {
// robot.spin1.setPosition(ServoPositions.shootAllAutoSpinStartPos);
// robot.spin2.setPosition(1 - ServoPositions.shootAllAutoSpinStartPos);
// } else {
// shooterTicker++;
// //robot.intake.setPower(0.0);
// robot.transferServo.setPosition(transferServo_in);
// double prevSpinPos = robot.spin1.getPosition();
// robot.spin1.setPosition(prevSpinPos + ServoPositions.shootAllSpindexerSpeedIncrease);
// robot.spin2.setPosition(1 - prevSpinPos - ServoPositions.shootAllAutoSpinStartPos);
// }
//
// } else {
// robot.transferServo.setPosition(transferServo_out);
// //spindexPos = spindexer_intakePos1;
// shootAll = false;
// spindexer.resetSpindexer();
// spindexer.processIntake();
//
// }
if (shooterTicker == 0) {
spindexer.prepareShootAll();
TELE.addLine("preparing to shoot");
} else if (shooterTicker == 2) {
//robot.transferServo.setPosition(transferServo_in);
spindexer.shootAll();
TELE.addLine("starting to shoot");
} else if (!spindexer.shootAllComplete()) {
robot.transferServo.setPosition(transferServo_in); robot.transferServo.setPosition(transferServo_in);
shooterTicker++; TELE.addLine("shoot");
double prevSpinPos = robot.spin1.getPosition();
robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease);
robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
}
} else { } else {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1; //spindexPos = spindexer_intakePos1;
shootAll = false; shootAll = false;
spindexer.resetSpindexer(); spindexer.resetSpindexer();
spindexer.processIntake(); //spindexer.processIntake();
TELE.addLine("stop shooting");
} }
shooterTicker++;
//spindexer.processIntake();
} }
if (gamepad1.left_stick_button){ if (gamepad1.left_stick_button){
@@ -818,42 +875,40 @@ public class TeleopV3 extends LinearOpMode {
hub.clearBulkCache(); hub.clearBulkCache();
} }
// //
TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); // TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); // TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
TELE.addData("Spin3Green", green3 + ": " + ballIn(3)); // TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
//
TELE.addData("pose", drive.localizer.getPose()); // TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); // TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
TELE.addData("distanceToGoal", distanceToGoal); // TELE.addData("distanceToGoal", distanceToGoal);
TELE.addData("hood", robot.hood.getPosition()); // TELE.addData("hood", robot.hood.getPosition());
TELE.addData("targetVel", vel); // TELE.addData("targetVel", vel);
TELE.addData("Velocity", flywheel.getVelo()); // TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Velo1", flywheel.velo1); // TELE.addData("Velo1", flywheel.velo1);
TELE.addData("Velo2", flywheel.velo2); // TELE.addData("Velo2", flywheel.velo2);
TELE.addData("shootOrder", shootOrder); // TELE.addData("shootOrder", shootOrder);
TELE.addData("oddColor", oddBallColor); // TELE.addData("oddColor", oddBallColor);
//
// Spindexer Debug // // Spindexer Debug
TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1)); TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1));
TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition); TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition);
TELE.addData("spinIntakeState", spindexer.currentIntakeState); TELE.addData("spinIntakeState", spindexer.currentIntakeState);
TELE.addData("spinTestCounter", spindexer.counter); TELE.addData("spinTestCounter", spindexer.counter);
TELE.addData("autoSpintake", autoSpintake); TELE.addData("autoSpintake", autoSpintake);
//TELE.addData("distanceRearCenter", spindexer.distanceRearCenter); //
//TELE.addData("distanceFrontDriver", spindexer.distanceFrontDriver); // TELE.addData("shootall commanded", shootAll);
//TELE.addData("distanceFrontPassenger", spindexer.distanceFrontPassenger); // // Targeting Debug
TELE.addData("shootall commanded", shootAll); // TELE.addData("robotX", robotX);
// Targeting Debug // TELE.addData("robotY", robotY);
TELE.addData("robotX", robotX); // TELE.addData("robotInchesX", targeting.robotInchesX);
TELE.addData("robotY", robotY); // TELE.addData( "robotInchesY", targeting.robotInchesY);
TELE.addData("robotInchesX", targeting.robotInchesX); // TELE.addData("Targeting Interpolate", turretInterpolate);
TELE.addData( "robotInchesY", targeting.robotInchesY);
TELE.addData("Targeting Interpolate", turretInterpolate);
TELE.addData("Targeting GridX", targeting.robotGridX); TELE.addData("Targeting GridX", targeting.robotGridX);
TELE.addData("Targeting GridY", targeting.robotGridY); TELE.addData("Targeting GridY", targeting.robotGridY);
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(); TELE.update();

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java
View File

@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.tests; package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.Color.colorFilterAlpha;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -18,6 +20,9 @@ public class ColorTest extends LinearOpMode {
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
double color1Distance = 0;
double color2Distance = 0;
double color3Distance = 0;
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
@@ -26,28 +31,34 @@ public class ColorTest extends LinearOpMode {
double green1 = robot.color1.getNormalizedColors().green; double green1 = robot.color1.getNormalizedColors().green;
double blue1 = robot.color1.getNormalizedColors().blue; double blue1 = robot.color1.getNormalizedColors().blue;
double red1 = robot.color1.getNormalizedColors().red; double red1 = robot.color1.getNormalizedColors().red;
double dist1 = robot.color1.getDistance(DistanceUnit.MM);
color1Distance = (colorFilterAlpha * dist1) + ((1-colorFilterAlpha) * color1Distance);
TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor()); TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
TELE.addData("Color1 green", green1 / (green1 + blue1 + red1)); TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM)); TELE.addData("Color1 distance (mm)", color1Distance);
// ----- COLOR 2 ----- // ----- COLOR 2 -----
double green2 = robot.color2.getNormalizedColors().green; double green2 = robot.color2.getNormalizedColors().green;
double blue2 = robot.color2.getNormalizedColors().blue; double blue2 = robot.color2.getNormalizedColors().blue;
double red2 = robot.color2.getNormalizedColors().red; double red2 = robot.color2.getNormalizedColors().red;
double dist2 = robot.color2.getDistance(DistanceUnit.MM);
color2Distance = (colorFilterAlpha * dist2) + ((1-colorFilterAlpha) * color2Distance);
TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor()); TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor());
TELE.addData("Color2 green", green2 / (green2 + blue2 + red2)); TELE.addData("Color2 green", green2 / (green2 + blue2 + red2));
TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM)); TELE.addData("Color2 distance (mm)", color2Distance);
// ----- COLOR 3 ----- // ----- COLOR 3 -----
double green3 = robot.color3.getNormalizedColors().green; double green3 = robot.color3.getNormalizedColors().green;
double blue3 = robot.color3.getNormalizedColors().blue; double blue3 = robot.color3.getNormalizedColors().blue;
double red3 = robot.color3.getNormalizedColors().red; double red3 = robot.color3.getNormalizedColors().red;
double dist3 = robot.color3.getDistance(DistanceUnit.MM);
color3Distance = (colorFilterAlpha * dist3) + ((1-colorFilterAlpha) * color3Distance);
TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor()); TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor());
TELE.addData("Color3 green", green3 / (green3 + blue3 + red3)); TELE.addData("Color3 green", green3 / (green3 + blue3 + red3));
TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM)); TELE.addData("Color3 distance (mm)", color3Distance);
TELE.update(); TELE.update();
} }

166
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
View File

@@ -39,6 +39,11 @@ public class Spindexer {
public double distanceFrontDriver = 0.0; public double distanceFrontDriver = 0.0;
public double distanceFrontPassenger = 0.0; public double distanceFrontPassenger = 0.0;
public double spindexerWiggle = 0.01;
public double spindexerOuttakeWiggle = 0.01;
private double prevPos = 0.0;
public double spindexerPosOffset = 0.00;
public Types.Motif desiredMotif = Types.Motif.NONE; public Types.Motif desiredMotif = Types.Motif.NONE;
// For Use // For Use
enum RotatedBallPositionNames { enum RotatedBallPositionNames {
@@ -70,9 +75,12 @@ public class Spindexer {
SHOOT_ALL_READY SHOOT_ALL_READY
} }
int shootWaitCount = 0;
public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
public int unknownColorDetect = 0; public int unknownColorDetect = 0;
enum BallColor { public enum BallColor {
UNKNOWN, UNKNOWN,
GREEN, GREEN,
PURPLE PURPLE
@@ -107,7 +115,9 @@ public class Spindexer {
double[] outakePositions = double[] outakePositions =
{spindexer_outtakeBall1, spindexer_outtakeBall2, spindexer_outtakeBall3}; {spindexer_outtakeBall1+spindexerPosOffset,
spindexer_outtakeBall2+spindexerPosOffset,
spindexer_outtakeBall3+spindexerPosOffset};
double[] intakePositions = double[] intakePositions =
{spindexer_intakePos1, spindexer_intakePos2, spindexer_intakePos3}; {spindexer_intakePos1, spindexer_intakePos2, spindexer_intakePos3};
@@ -151,12 +161,15 @@ public class Spindexer {
int spindexerBallPos = 0; int spindexerBallPos = 0;
// Read Distances // Read Distances
distanceRearCenter = robot.color1.getDistance(DistanceUnit.MM); double dRearCenter = robot.color1.getDistance(DistanceUnit.MM);
distanceFrontDriver = robot.color2.getDistance(DistanceUnit.MM); distanceRearCenter = (colorFilterAlpha * dRearCenter) + ((1-colorFilterAlpha) * distanceRearCenter);
distanceFrontPassenger = robot.color3.getDistance(DistanceUnit.MM); double dFrontDriver = robot.color2.getDistance(DistanceUnit.MM);
distanceFrontDriver = (colorFilterAlpha * dFrontDriver) + ((1-colorFilterAlpha) * distanceFrontDriver);
double dFrontPassenger = robot.color3.getDistance(DistanceUnit.MM);
distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
// Position 1 // Position 1
if (distanceRearCenter < 43) { if (distanceRearCenter < 60) {
// Mark Ball Found // Mark Ball Found
newPos1Detection = true; newPos1Detection = true;
@@ -170,17 +183,17 @@ public class Spindexer {
double gP = green / (green + red + blue); double gP = green / (green + red + blue);
// FIXIT - Add filtering to improve accuracy. // FIXIT - Add filtering to improve accuracy.
if (gP >= 0.4) { if (gP >= 0.38) {
ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // green
} else { } else {
ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // purple ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple
} }
} }
} }
// Position 2 // Position 2
// Find which ball position this is in the spindexer // Find which ball position this is in the spindexer
spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]; spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()];
if (distanceFrontDriver < 60) { if (distanceFrontDriver < 50) {
// reset FoundEmpty because looking for 3 in a row before reset // reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0; ballPositions[spindexerBallPos].foundEmpty = 0;
if (detectFrontColor) { if (detectFrontColor) {
@@ -191,9 +204,9 @@ public class Spindexer {
double gP = green / (green + red + blue); double gP = green / (green + red + blue);
if (gP >= 0.4) { if (gP >= 0.4) {
ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
} else { } else {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // purple ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
} }
} }
} else { } else {
@@ -208,7 +221,7 @@ public class Spindexer {
// Position 3 // Position 3
spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]; spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()];
if (distanceFrontPassenger < 33) { if (distanceFrontPassenger < 29) {
// reset FoundEmpty because looking for 3 in a row before reset // reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0; ballPositions[spindexerBallPos].foundEmpty = 0;
@@ -219,10 +232,10 @@ public class Spindexer {
double gP = green / (green + red + blue); double gP = green / (green + red + blue);
if (gP >= 0.4) { if (gP >= 0.42) {
ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
} else { } else {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // purple ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
} }
} }
} else { } else {
@@ -246,10 +259,21 @@ public class Spindexer {
return newPos1Detection; return newPos1Detection;
} }
// Has code to unjam spindexer
public void moveSpindexerToPos(double pos) { private void moveSpindexerToPos(double pos) {
robot.spin1.setPosition(pos); robot.spin1.setPosition(pos);
robot.spin2.setPosition(1-pos); robot.spin2.setPosition(1-pos);
double currentPos = servos.getSpinPos();
if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
// if (currentPos > pos){
// robot.spin1.setPosition(servos.getSpinPos() + 0.05);
// robot.spin2.setPosition(1 - servos.getSpinPos() - 0.05);
// } else {
// robot.spin1.setPosition(servos.getSpinPos() - 0.05);
// robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05);
// }
}
prevPos = currentPos;
} }
public void stopSpindexer() { public void stopSpindexer() {
@@ -278,6 +302,10 @@ public class Spindexer {
} }
} }
public boolean slotIsEmpty(int slot){
return !ballPositions[slot].isEmpty;
}
public boolean isFull () { public boolean isFull () {
return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty); return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty);
} }
@@ -287,7 +315,7 @@ public class Spindexer {
case UNKNOWN_START: case UNKNOWN_START:
// For now just set position ONE if UNKNOWN // For now just set position ONE if UNKNOWN
commandedIntakePosition = 0; commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[0]); moveSpindexerToPos(intakePositions[0]);
currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE; currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE;
break; break;
case UNKNOWN_MOVE: case UNKNOWN_MOVE:
@@ -316,34 +344,32 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.FINDNEXT; currentIntakeState = Spindexer.IntakeState.FINDNEXT;
} else { } else {
// Maintain Position // Maintain Position
moveSpindexerToPos(intakePositions[commandedIntakePosition]); spindexerWiggle *= -1.0;
moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
} }
break; break;
case FINDNEXT: case FINDNEXT:
// Find Next Open Position and start movement // Find Next Open Position and start movement
double currentSpindexerPos = servos.getSpinPos(); double currentSpindexerPos = servos.getSpinPos();
double commandedtravelDistance = 2.0; double commandedtravelDistance = 2.0;
//double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos); double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos);
//if (ballPositions[0].isEmpty && (proposedTravelDistance < commandedtravelDistance)) { //if (ballPositions[0].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
if (ballPositions[0].isEmpty) { if (ballPositions[0].isEmpty) {
// Position 1 // Position 1
commandedIntakePosition = 0; commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING; currentIntakeState = Spindexer.IntakeState.MOVING;
} }
//proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos); proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
//if (ballPositions[1].isEmpty && (proposedTravelDistance < commandedtravelDistance)) { //if (ballPositions[1].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
if (ballPositions[1].isEmpty) { if (ballPositions[1].isEmpty) {
// Position 2 // Position 2
commandedIntakePosition = 1; commandedIntakePosition = 1;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING; currentIntakeState = Spindexer.IntakeState.MOVING;
} }
//proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos); proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
if (ballPositions[2].isEmpty) { if (ballPositions[2].isEmpty) {
// Position 3 // Position 3
commandedIntakePosition = 2; commandedIntakePosition = 2;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING; currentIntakeState = Spindexer.IntakeState.MOVING;
} }
if (currentIntakeState != Spindexer.IntakeState.MOVING) { if (currentIntakeState != Spindexer.IntakeState.MOVING) {
@@ -374,29 +400,29 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.FINDNEXT; currentIntakeState = Spindexer.IntakeState.FINDNEXT;
} }
// Maintain Position // Maintain Position
moveSpindexerToPos(intakePositions[commandedIntakePosition]); spindexerWiggle *= -1.0;
moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
break; break;
case SHOOT_ALL_PREP: case SHOOT_ALL_PREP:
// We get here with function call to prepareToShootMotif // We get here with function call to prepareToShootMotif
// Stopping when we get to the new position // Stopping when we get to the new position
if (servos.spinEqual(intakePositions[commandedIntakePosition])) { commandedIntakePosition = 0;
currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_READY; if (!servos.spinEqual(outakePositions[commandedIntakePosition])) {
} else {
// Keep moving the spindexer // Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
} }
break; break;
case SHOOT_ALL_READY: case SHOOT_ALL_READY: // Not used
// Double Check Colors // Double Check Colors
//detectBalls(false, false); // Minimize hardware calls //detectBalls(false, false); // Minimize hardware calls
if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) { if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) {
// All ball shot move to intake state // All ball shot move to intake state
currentIntakeState = Spindexer.IntakeState.FINDNEXT; currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
} }
// Maintain Position // Maintain Position
moveSpindexerToPos(intakePositions[commandedIntakePosition]); moveSpindexerToPos(outakePositions[commandedIntakePosition]);
break; break;
case SHOOTNEXT: case SHOOTNEXT:
@@ -404,23 +430,20 @@ public class Spindexer {
if (!ballPositions[0].isEmpty) { if (!ballPositions[0].isEmpty) {
// Position 1 // Position 1
commandedIntakePosition = 0; commandedIntakePosition = 0;
servos.setSpinPos(outakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else if (ballPositions[1].isEmpty) { // Possible error: should it be !ballPosition[1].isEmpty? } else if (!ballPositions[1].isEmpty) {
// Position 2 // Position 2
commandedIntakePosition = 1; commandedIntakePosition = 1;
servos.setSpinPos(outakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else if (ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty? } else if (!ballPositions[2].isEmpty) {
// Position 3 // Position 3
commandedIntakePosition = 2; commandedIntakePosition = 2;
servos.setSpinPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else { } else {
// Empty return to intake state // Empty return to intake state
currentIntakeState = IntakeState.FINDNEXT; currentIntakeState = IntakeState.FINDNEXT;
} }
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" moveSpindexerToPos(outakePositions[commandedIntakePosition]);
break; break;
case SHOOTMOVING: case SHOOTMOVING:
@@ -429,25 +452,39 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
} else { } else {
// Keep moving the spindexer // Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" moveSpindexerToPos(outakePositions[commandedIntakePosition]);
} }
break; break;
case SHOOTWAIT: case SHOOTWAIT:
double shootWaitMax = 4;
// Stopping when we get to the new position // Stopping when we get to the new position
if (servos.spinEqual(intakePositions[commandedIntakePosition])) { if (prevIntakeState != currentIntakeState) {
currentIntakeState = Spindexer.IntakeState.INTAKE; if (commandedIntakePosition==2) {
stopSpindexer(); shootWaitMax = 5;
//detectBalls(true, false);
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
} }
shootWaitCount = 0;
} else {
shootWaitCount++;
}
// wait 10 cycles
if (shootWaitCount > shootWaitMax) {
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
ballPositions[commandedIntakePosition].isEmpty = true;
shootWaitCount = 0;
//stopSpindexer();
//detectBalls(true, false);
}
// Keep moving the spindexer
spindexerOuttakeWiggle *= -1.01;
moveSpindexerToPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle);
break; break;
default: default:
// Statements to execute if no case matches // Statements to execute if no case matches
} }
prevIntakeState = currentIntakeState;
//TELE.addData("commandedIntakePosition", commandedIntakePosition); //TELE.addData("commandedIntakePosition", commandedIntakePosition);
//TELE.update(); //TELE.update();
// Signal a successful intake // Signal a successful intake
@@ -476,7 +513,7 @@ public class Spindexer {
} else if (ballPositions[1].ballColor == BallColor.GREEN) { } else if (ballPositions[1].ballColor == BallColor.GREEN) {
return 1; return 1;
} else { } else {
return 3; return 2;
} }
//break; //break;
case PPG: case PPG:
@@ -499,6 +536,25 @@ public class Spindexer {
commandedIntakePosition = bestFitMotif(); commandedIntakePosition = bestFitMotif();
} }
public void prepareShootAll(){
currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP;
}
public void shootAll () {
ballPositions[0].isEmpty = false;
ballPositions[1].isEmpty = false;
ballPositions[2].isEmpty = false;
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
}
public boolean shootAllComplete ()
{
return ((currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_PREP) &&
(currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) &&
(currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) &&
(currentIntakeState != Spindexer.IntakeState.SHOOTNEXT) &&
(currentIntakeState != Spindexer.IntakeState.SHOOTWAIT));
}
void shootAllToIntake () { void shootAllToIntake () {
currentIntakeState = Spindexer.IntakeState.FINDNEXT; currentIntakeState = Spindexer.IntakeState.FINDNEXT;
} }
@@ -506,4 +562,16 @@ public class Spindexer {
public void update() public void update()
{ {
} }
public BallColor GetFrontDriverColor () {
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]].ballColor;
}
public BallColor GetFrontPassengerColor () {
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]].ballColor;
}
public BallColor GetRearCenterColor () {
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor;
}
} }

174
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java
View File

@@ -67,19 +67,19 @@ public class Targeting {
KNOWNTARGETING[3][4] = new Settings (3100.0, 0.47); KNOWNTARGETING[3][4] = new Settings (3100.0, 0.47);
KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47); KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47);
// ROW 4 // ROW 4
KNOWNTARGETING[4][0] = new Settings (4540.0, 0.1); KNOWNTARGETING[4][0] = new Settings (3100, 0.49);
KNOWNTARGETING[4][1] = new Settings (4541.0, 0.1); KNOWNTARGETING[4][1] = new Settings (3100, 0.49);
KNOWNTARGETING[4][2] = new Settings (4542.0, 0.1); KNOWNTARGETING[4][2] = new Settings (3100, 0.5);
KNOWNTARGETING[4][3] = new Settings (4543.0, 0.1); KNOWNTARGETING[4][3] = new Settings (3200, 0.5);
KNOWNTARGETING[4][4] = new Settings (4544.0, 0.1); KNOWNTARGETING[4][4] = new Settings (3250, 0.49);
KNOWNTARGETING[4][5] = new Settings (4545.0, 0.1); KNOWNTARGETING[4][5] = new Settings (3300, 0.49);
// ROW 1 // ROW 5
KNOWNTARGETING[5][0] = new Settings (4550.0, 0.1); KNOWNTARGETING[5][0] = new Settings (3200, 0.48);
KNOWNTARGETING[5][1] = new Settings (4551.0, 0.1); KNOWNTARGETING[5][1] = new Settings (3200, 0.48);
KNOWNTARGETING[5][2] = new Settings (4552.0, 0.1); KNOWNTARGETING[5][2] = new Settings (3300, 0.48);
KNOWNTARGETING[5][3] = new Settings (4553.0, 0.1); KNOWNTARGETING[5][3] = new Settings (3350, 0.48);
KNOWNTARGETING[5][4] = new Settings (4554.0, 0.1); KNOWNTARGETING[5][4] = new Settings (3350, 0.48);
KNOWNTARGETING[5][5] = new Settings (4555.0, 0.1); KNOWNTARGETING[5][5] = new Settings (3350, 0.48);
} }
public Targeting() public Targeting()
@@ -107,61 +107,81 @@ public class Targeting {
// Determine if we need to interpolate based on tile position. // Determine if we need to interpolate based on tile position.
// if near upper or lower quarter or tile interpolate with next tile. // if near upper or lower quarter or tile interpolate with next tile.
int x0 = 0;
int y0 = 0;
int x1 = 0; int x1 = 0;
int y1 = 0; int y1 = 0;
// interpolate = false; interpolate = false;
// if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
// (robotGridX < 5) && (robotGridY <5)) { (robotGridX < 5) && (robotGridY <5)) {
// // +X, +Y // +X, +Y
// interpolate = true; interpolate = true;
// x1 = robotGridX + 1; x0 = robotGridX;
// y1 = robotGridY + 1; x1 = robotGridX + 1;
// } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && y0 = robotGridY;
// (robotGridX > 0) && (robotGridY > 0)) { y1 = robotGridY + 1;
// // -X, -Y } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
// interpolate = true; (robotGridX > 0) && (robotGridY > 0)) {
// x1 = robotGridX - 1; // -X, -Y
// y1 = robotGridY - 1; interpolate = true;
// } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && x0 = robotGridX - 1;
// (robotGridX < 5) && (robotGridY > 0)) { x1 = robotGridX;
// // +X, -Y y0 = robotGridY - 1;
// interpolate = true; y1 = robotGridY;
// x1 = robotGridX + 1; } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
// y1 = robotGridY - 1; (robotGridX < 5) && (robotGridY > 0)) {
// } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && // +X, -Y
// (robotGridX > 0) && (robotGridY < 5)) { interpolate = true;
// // -X, +Y x0 = robotGridX;
// interpolate = true; x1 = robotGridX + 1;
// x1 = robotGridX - 1; y0 = robotGridY - 1;
// y1 = robotGridY + 1; y1 = robotGridY;
// } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) { } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
// // -X, Y (robotGridX > 0) && (robotGridY < 5)) {
// interpolate = true; // -X, +Y
// x1 = robotGridX - 1; interpolate = true;
// y1 = robotGridY; x0 = robotGridX - 1;
// } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) { x1 = robotGridX;
// // X, -Y y0 = robotGridY;
// interpolate = true; y1 = robotGridY + 1;
// x1 = robotGridX; } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) {
// y1 = robotGridY - 1; // -X, Y
// } else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) { interpolate = true;
// // +X, Y x0 = robotGridX - 1;
// interpolate = true; x1 = robotGridX;
// x1 = robotGridX + 1; y0 = robotGridY;
// y1 = robotGridY; y1 = robotGridY;
// } else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) { } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) {
// // X, +Y // X, -Y
// interpolate = true; interpolate = true;
// x1 = robotGridX; x0 = robotGridX;
// y1 = robotGridY + 1; x1 = robotGridX;
// } y0 = robotGridY - 1;
y1 = robotGridY;
} else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) {
// +X, Y
interpolate = true;
x0 = robotGridX;
x1 = robotGridX + 1;
y0 = robotGridY;
y1 = robotGridY;
} else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) {
// X, +Y
interpolate = true;
x0 = robotGridX;
x1 = robotGridX;
y0 = robotGridY;
y1 = robotGridY + 1;
} else {
interpolate = false;
}
//clamp //clamp
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));
// basic search // basic search
if(!interpolate) { if(true) { //!interpolate) {
if ((robotGridY < 6) && (robotGridX <6)) { if ((robotGridY < 6) && (robotGridX <6)) {
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM; recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle; recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
@@ -170,27 +190,29 @@ public class Targeting {
} else { } else {
// bilinear interpolation // bilinear interpolation
int x0 = robotGridX; //int x0 = robotGridX;
//int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1); //int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1);
int y0 = robotGridY; //int y0 = robotGridY;
//int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1); //int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1);
double x = (robotInchesX - (x0 * tileSize)) / tileSize; // double x = (robotInchesX - (x0 * tileSize)) / tileSize;
double y = (robotInchesY - (y0 * tileSize)) / tileSize; // double y = (robotInchesY - (y0 * tileSize)) / tileSize;
double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM; // double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM;
double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM; // double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM;
double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM; // double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM;
double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM; // double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM;
//
double angle00 = KNOWNTARGETING[y0][x0].hoodAngle; // double angle00 = KNOWNTARGETING[y0][x0].hoodAngle;
double angle10 = KNOWNTARGETING[y0][x1].hoodAngle; // double angle10 = KNOWNTARGETING[y0][x1].hoodAngle;
double angle01 = KNOWNTARGETING[y1][x0].hoodAngle; // double angle01 = KNOWNTARGETING[y1][x0].hoodAngle;
double angle11 = KNOWNTARGETING[y1][x1].hoodAngle; // double angle11 = KNOWNTARGETING[y1][x1].hoodAngle;
recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11;
recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11;
// recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11;
// recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11;
// Average target tiles
recommendedSettings.flywheelRPM = (KNOWNTARGETING[x0][y0].flywheelRPM + KNOWNTARGETING[x1][y1].flywheelRPM)/2.0;
recommendedSettings.hoodAngle = (KNOWNTARGETING[x0][y0].hoodAngle + KNOWNTARGETING[x1][y1].hoodAngle)/2.0;
return recommendedSettings; return recommendedSettings;
} }
} }

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -61,12 +61,6 @@ public class Turret {
this.TELE = tele; this.TELE = tele;
this.robot = rob; this.robot = rob;
this.webcam = cam; this.webcam = cam;
webcam.start();
if (redAlliance) {
webcam.pipelineSwitch(4);
} else {
webcam.pipelineSwitch(2);
}
bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D); bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D);
} }
@@ -119,17 +113,14 @@ public class Turret {
} }
public double getTy() { public double getTy() {
limelightRead();
return ty; return ty;
} }
public double getLimelightX() { public double getLimelightX() {
limelightRead();
return limelightPosX; return limelightPosX;
} }
public double getLimelightY() { public double getLimelightY() {
limelightRead();
return limelightPosY; return limelightPosY;
} }

8
build.dependencies.gradle
View File

@@ -26,10 +26,10 @@ dependencies {
implementation 'com.bylazar:fullpanels:1.0.2' //Panels implementation 'com.bylazar:fullpanels:1.0.2' //Panels
implementation "com.acmerobotics.roadrunner:ftc:0.1.25" implementation "com.acmerobotics.roadrunner:ftc:0.1.25" //RR
implementation "com.acmerobotics.roadrunner:core:1.0.1" implementation "com.acmerobotics.roadrunner:core:1.0.1" //RR
implementation "com.acmerobotics.roadrunner:actions:1.0.1" implementation "com.acmerobotics.roadrunner:actions:1.0.1" //RR
implementation "com.acmerobotics.dashboard:dashboard:0.5.1" implementation "com.acmerobotics.dashboard:dashboard:0.5.1" //FTC Dash
implementation 'org.ftclib.ftclib:core:2.1.1' // FTC LIB implementation 'org.ftclib.ftclib:core:2.1.1' // FTC LIB