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

Merge branch 'danielv5'

Browse Source 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
This commit is contained in:
Keshav Anand
2026-02-12 20:03:07 -06:00
parent 645267d05f 6edc2075c0
commit 754cb6c622
24 changed files with 2574 additions and 1093 deletions
Download Patch File Download Diff File Expand all files Collapse all files

461
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
View File

@@ -1,62 +1,9 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
@@ -64,6 +11,8 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
import androidx.annotation.NonNull;
@@ -82,7 +31,6 @@ import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Poses_V2;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -95,30 +43,30 @@ import java.util.Objects;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
public class Auto_LT_Close extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015;
public static double shoot0SpinSpeedIncrease = 0.02;
public static double spindexerSpeedIncrease = 0.03;
public static double finalSpindexerSpeedIncrease = 0.025;
public static double finalSpindexerSpeedIncrease = 0.03;
// These values are ADDED to turrDefault
public static double redObeliskTurrPos1 = 0.12;
public static double redObeliskTurrPos2 = 0.13;
public static double redObeliskTurrPos3 = 0.14;
public static double blueObeliskTurrPos1 = -0.12;
public static double blueObeliskTurrPos2 = -0.13;
public static double blueObeliskTurrPos3 = -0.14;
public static double redTurretShootPos = 0.1;
public static double blueTurretShootPos = -0.14;
public static double redObeliskTurrPos1 = turrDefault + 0.12;
public static double redObeliskTurrPos2 = turrDefault + 0.13;
public static double redObeliskTurrPos3 = turrDefault + 0.14;
public static double blueObeliskTurrPos1 = turrDefault - 0.12;
public static double blueObeliskTurrPos2 = turrDefault - 0.13;
public static double blueObeliskTurrPos3 = turrDefault - 0.14;
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0;
public static double normalIntakeTime = 3.3;
public static double shoot1Turr = 0.57;
public static double shoot0XTolerance = 1.0;
public static double redTurretShootPos = turrDefault + 0.12;
public static double blueTurretShootPos = turrDefault - 0.14;
double turretShootPos = 0.0;
public static double finalShootAllTime = 3.0;
@@ -130,7 +78,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
public static double intake3Time = 4.2;
public static double flywheel0Time = 3.5;
public static double pickup1Speed = 23;
public static double pickup1Speed = 15;
// ---- SECOND SHOT / PICKUP ----
public static double shoot1Vel = 2300;
public static double shootAllVelocity = 2500;
@@ -163,21 +111,22 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
Targeting.Settings targetingSettings;
private double firstSpindexShootPos = autoSpinStartPos;
private boolean shootForward = true;
private double x1, y1, h1;
double x1, y1, h1;
private double x2a, y2a, h2a, t2a;
double x2a, y2a, h2a, t2a;
private double x2b, y2b, h2b, t2b;
private double x2c, y2c, h2c, t2c;
double x2b, y2b, h2b, t2b;
double x2c, y2c, h2c, t2c;
private double x3a, y3a, h3a;
private double x3b, y3b, h3b;
private double x4a, y4a, h4a;
private double x4b, y4b, h4b;
double x3a, y3a, h3a;
double x3b, y3b, h3b;
double x4a, y4a, h4a;
double x4b, y4b, h4b;
private double xShoot, yShoot, hShoot;
private double xGate, yGate, hGate;
private double xPrep, yPrep, hPrep;
double xShoot, yShoot, hShoot;
double xGate, yGate, hGate;
double xPrep, yPrep, hPrep;
double xLeave, yLeave, hLeave;
private double shoot1Tangent;
@@ -186,6 +135,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
private int rearSlotGreen = 0;
private int mostGreenSlot = 0;
int ballCycles = 3;
int prevMotif = 0;
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() {
@@ -202,14 +153,29 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
stamp = System.currentTimeMillis();
}
ticker++;
robot.transferServo.setPosition(transferServo_out);
turret.manualSetTurret(turretShootPos);
servos.setTransferPos(transferServo_out);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.addData("motif", motif_id);
@@ -219,8 +185,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
spindexerWiggle *= -1.0;
robot.spin1.setPosition(spindexer_intakePos1 + spindexerWiggle);
robot.spin2.setPosition(1 - spindexer_intakePos1 - spindexerWiggle);
servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
spindexer.detectBalls(true, true);
@@ -231,11 +196,12 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) {
passengerSlotGreen++;
}
if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) {
rearSlotGreen++;
}
robot.intake.setPower(1);
spindexer.setIntakePower(1);
decideGreenSlot = true;
@@ -250,8 +216,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
mostGreenSlot = 1;
}
decideGreenSlot = false;
if (motif_id == 21) {
@@ -293,14 +257,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
return true;
} else if ((System.currentTimeMillis() - stamp) < (time * 1000)) {
// TELE.addData("MostGreenSlot", mostGreenSlot);
// TELE.update();
spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
TELE.addData("MostGreenSlot", mostGreenSlot);
robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
robot.spin1.setPosition(firstSpindexShootPos);
robot.spin2.setPosition(1 - firstSpindexShootPos);
servos.setSpinPos(firstSpindexShootPos);
return true;
} else {
@@ -314,11 +275,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
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
@@ -327,6 +285,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
@@ -334,36 +294,51 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
robot.intake.setPower(0);
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (getRuntime() - stamp < shootTime) {
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
servos.setSpinPos(autoSpinStartPos);
} else {
robot.transferServo.setPosition(transferServo_in);
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = robot.spin1.getPosition();
robot.spin1.setPosition(prevSpinPos + spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
double prevSpinPos = servos.getSpinCmdPos();
servos.setSpinPos(prevSpinPos + spindexSpeed);
}
return true;
} else {
robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1;
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
@@ -398,43 +373,57 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
robot.intake.setPower(0);
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if (getRuntime() - stamp < shootTime) {
if (getRuntime() - stamp < firstShootTime) {
robot.transferServo.setPosition(transferServo_in);
robot.spin1.setPosition(firstSpindexShootPos);
robot.spin2.setPosition(1 - firstSpindexShootPos);
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {
robot.transferServo.setPosition(transferServo_in);
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = robot.spin1.getPosition();
double prevSpinPos = servos.getSpinCmdPos();
if (shootForward) {
robot.spin1.setPosition(prevSpinPos + spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else {
robot.spin1.setPosition(prevSpinPos - spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos + spindexSpeed);
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
}
return true;
} else {
robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1;
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
@@ -459,18 +448,17 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
ticker++;
spindexer.processIntake();
robot.intake.setPower(1);
spindexer.setIntakePower(1);
spindexer.ballCounterLight();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.addData("Full?", spindexer.isFull());
TELE.update();
return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
}
};
}
@@ -507,8 +495,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
ticker++;
motif = turret.detectObelisk();
robot.turr1.setPosition(turrPos);
robot.turr2.setPosition(1 - turrPos);
turret.setTurret(turrPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
@@ -568,8 +555,10 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
ticker++;
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
robot.hood.setPosition(hoodPos);
servos.setHoodPos(hoodPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
@@ -637,8 +626,10 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
turret.trackGoal(deltaPose);
robot.hood.setPosition(targetingSettings.hoodAngle);
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
@@ -706,8 +697,10 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, false);
robot.hood.setPosition(targetingSettings.hoodAngle);
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
@@ -728,6 +721,15 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
};
}
// initialize path variables here
TrajectoryActionBuilder shoot0 = null;
TrajectoryActionBuilder pickup1 = null;
TrajectoryActionBuilder shoot1 = null;
TrajectoryActionBuilder pickup2 = null;
TrajectoryActionBuilder shoot2 = null;
TrajectoryActionBuilder pickup3 = null;
TrajectoryActionBuilder shoot3 = null;
@Override
public void runOpMode() throws InterruptedException {
@@ -746,62 +748,49 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
servos = new Servos(hardwareMap);
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(turrDefault);
turret.setTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
servos.setSpinPos(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;
servos.setTransferPos(transferServo_out);
limelightUsed = false;
robot.light.setPosition(1);
while (opModeInInit()) {
robot.hood.setPosition(shoot0Hood);
turret.manualSetTurret(turrDefault);
servos.setHoodPos(shoot0Hood);
turret.setTurret(turrDefault);
if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance;
}
if (gamepad2.dpadLeftWasPressed()) {
turrDefault -=0.01;
turrDefault -= 0.01;
}
if (gamepad2.dpadRightWasPressed()) {
turrDefault +=0.01;
turrDefault += 0.01;
}
redObeliskTurrPos1 = turrDefault + 0.12;
redObeliskTurrPos2 = turrDefault + 0.13;
redObeliskTurrPos3 = turrDefault + 0.14;
blueObeliskTurrPos1 = turrDefault - 0.12;
blueObeliskTurrPos2 = turrDefault - 0.13;
blueObeliskTurrPos3 = turrDefault - 0.14;
redTurretShootPos = turrDefault + 0.12;
blueTurretShootPos = turrDefault - 0.14;
if (gamepad2.rightBumperWasPressed()){
ballCycles++;
}
if (gamepad2.leftBumperWasPressed()){
ballCycles--;
}
if (gamepad2.squareWasPressed()){
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
gamepad2.rumble(500);
}
if (redAlliance) {
robot.light.setPosition(0.28);
@@ -836,11 +825,14 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
xShoot = rShootX;
yShoot = rShootY;
hShoot = rShootH;
xLeave = rLeaveX;
yLeave = rLeaveY;
hLeave = rLeaveH;
obeliskTurrPos1 = redObeliskTurrPos1;
obeliskTurrPos2 = redObeliskTurrPos2;
obeliskTurrPos3 = redObeliskTurrPos3;
turretShootPos = redTurretShootPos;
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
turretShootPos = turrDefault + redTurretShootPos;
} else {
robot.light.setPosition(0.6);
@@ -876,44 +868,57 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
xShoot = bShootX;
yShoot = bShootY;
hShoot = bShootH;
xLeave = bLeaveX;
yLeave = bLeaveY;
hLeave = bLeaveH;
obeliskTurrPos1 = blueObeliskTurrPos1;
obeliskTurrPos2 = blueObeliskTurrPos2;
obeliskTurrPos3 = blueObeliskTurrPos3;
turretShootPos = blueTurretShootPos;
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
turretShootPos = turrDefault + blueTurretShootPos;
}
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
.strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x2a, y2a), h2a)
.strafeToLinearHeading(new Vector2d(x2b, y2b), h2b,
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
new TranslationalVelConstraint(pickup1Speed));
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
if (ballCycles < 2){
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
} else {
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
}
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
.strafeToLinearHeading(new Vector2d(x3a, y3a), h3a)
.strafeToLinearHeading(new Vector2d(x3b, y3b), h3b,
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
.strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
new TranslationalVelConstraint(pickup1Speed));
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
if (ballCycles < 3){
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
} else {
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hLeave));
}
pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x4a, y4a), h4a)
.strafeToLinearHeading(new Vector2d(x4b, y4b), h4b,
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(x4a, y4a), Math.toRadians(h4a))
.strafeToLinearHeading(new Vector2d(x4b, y4b), Math.toRadians(h4b),
new TranslationalVelConstraint(pickup1Speed));
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, Math.toRadians(h4b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.addData("Ball Cycles", ballCycles);
TELE.update();
}
@@ -926,6 +931,37 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
robot.transfer.setPower(1);
startAuto();
if (ballCycles > 0){
cycleStackClose();
}
if (ballCycles > 1){
cycleStackMiddle();
}
if (ballCycles > 2){
cycleStackFar();
}
while (opModeIsActive()) {
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
flywheel.manageFlywheel(0);
TELE.addLine("finished");
TELE.update();
}
}
}
void startAuto() {
assert shoot0 != null;
Actions.runBlocking(
@@ -947,7 +983,9 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
Actions.runBlocking(
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
);
}
void cycleStackClose(){
Actions.runBlocking(
new ParallelAction(
pickup1.build(),
@@ -955,8 +993,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
shootAllVelocity,
shootAllHood,
intake1Time,
x2b,
y2b,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
),
@@ -975,9 +1013,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
motif = turret.getObeliskID();
if (motif == 0) motif = 22;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
@@ -1009,14 +1046,16 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
)
);
}
void cycleStackMiddle(){
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
manageShooterAuto(
intake2Time,
x2b,
y2b,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
),
@@ -1033,6 +1072,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
)
);
motif = turret.getObeliskID();
if (motif == 0) motif = prevMotif;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
manageFlywheelAuto(
@@ -1060,14 +1104,16 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
)
);
}
void cycleStackFar(){
Actions.runBlocking(
new ParallelAction(
pickup3.build(),
manageShooterAuto(
intake3Time,
x2b,
y2b,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
),
@@ -1084,6 +1130,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
)
);
motif = turret.getObeliskID();
if (motif == 0) motif = prevMotif;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
manageFlywheelAuto(
@@ -1111,17 +1162,5 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addLine("finished");
TELE.update();
sleep(2000);
}
}
}

938
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java Normal file
View File

@@ -0,0 +1,938 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
import androidx.annotation.NonNull;
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.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;
import java.util.Objects;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Far extends LinearOpMode {
public static double shoot0Vel = 3300, shoot0Hood = 0.48 + hoodOffset;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015;
public static double shoot0XTolerance = 1.0;
public static double redTurretShootPos = 0.05;
public static double blueTurretShootPos = -0.05;
public static int fwdTime = 200, strafeTime = 2300;
double xLeave, yLeave, hLeave;
public static int sleepTime = 1300;
public int motif = 0;
double turretShootPos = 0.0;
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
Servos servos;
Spindexer spindexer;
Flywheel flywheel;
Turret turret;
Targeting targeting;
Targeting.Settings targetingSettings;
double firstSpindexShootPos = autoSpinStartPos;
boolean shootForward = true;
double xShoot, yShoot, hShoot;
private int driverSlotGreen = 0;
private int passengerSlotGreen = 0;
int rearSlotGreen = 0;
int mostGreenSlot = 0;
double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
public static double firstShootTime = 0.3;
public static double flywheel0Time = 3.5;
public static double shoot0Time = 2;
boolean gatePickup = false;
boolean stack3 = true;
double xStackPickupA, yStackPickupA, hStackPickupA;
double xStackPickupB, yStackPickupB, hStackPickupB;
public static int pickupStackSpeed = 15;
int prevMotif = 0;
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
double spindexerWiggle = 0.01;
boolean decideGreenSlot = false;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
servos.setTransferPos(transferServo_out);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.addData("motif", motif_id);
TELE.update();
if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
spindexerWiggle *= -1.0;
servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
spindexer.detectBalls(true, true);
if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) {
driverSlotGreen++;
}
if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) {
passengerSlotGreen++;
}
if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) {
rearSlotGreen++;
}
spindexer.setIntakePower(1);
decideGreenSlot = true;
return true;
} else if (decideGreenSlot) {
if (driverSlotGreen >= passengerSlotGreen && driverSlotGreen >= rearSlotGreen) {
mostGreenSlot = 3;
} else if (passengerSlotGreen >= driverSlotGreen && passengerSlotGreen >= rearSlotGreen) {
mostGreenSlot = 2;
} else {
mostGreenSlot = 1;
}
decideGreenSlot = false;
if (motif_id == 21) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
} else {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
}
} else if (motif_id == 22) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
} else {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = true;
}
} else {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
} else {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
}
}
return true;
} else if ((System.currentTimeMillis() - stamp) < (time * 1000)) {
// TELE.addData("MostGreenSlot", mostGreenSlot);
// TELE.update();
spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
servos.setSpinPos(firstSpindexShootPos);
return true;
} else {
return false;
}
}
};
}
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();
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
servos.setSpinPos(autoSpinStartPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = servos.getSpinCmdPos();
servos.setSpinPos(prevSpinPos + spindexSpeed);
}
return true;
} else {
servos.setTransferPos(transferServo_out);
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();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if (getRuntime() - stamp < shootTime) {
if (getRuntime() - stamp < firstShootTime) {
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = servos.getSpinCmdPos();
if (shootForward) {
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else {
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
}
return true;
} else {
servos.setTransferPos(transferServo_out);
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();
spindexer.setIntakePower(1);
spindexer.ballCounterLight();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Full?", spindexer.isFull());
TELE.update();
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
}
};
}
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();
robot.limelight.pipelineSwitch(1);
}
ticker++;
motif = turret.detectObelisk();
turret.setTurret(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();
if (shouldFinish){
if (redAlliance){
robot.limelight.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
}
return false;
} else {
return true;
}
}
};
}
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++;
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
servos.setHoodPos(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);
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
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);
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
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;
}
};
}
// initialize path variables here
TrajectoryActionBuilder leave3Ball = null;
TrajectoryActionBuilder leaveFromShoot = null;
TrajectoryActionBuilder pickup3 = null;
TrajectoryActionBuilder shoot3 = null;
@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.start();
robot.limelight.pipelineSwitch(1);
turret = new Turret(robot, TELE, robot.limelight);
turret.setTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, autoStart);
servos.setSpinPos(autoSpinStartPos);
servos.setTransferPos(transferServo_out);
while (opModeInInit()) {
// Recalibration in initialization
drive.updatePoseEstimate();
if (gamepad2.triangle) {
autoStart = drive.localizer.getPose(); // use this position as starting position
gamepad2.rumble(1000);
}
if (gamepad2.squareWasPressed()){
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
gamepad2.rumble(500);
}
if (gamepad2.leftBumperWasPressed()){
gatePickup = !gatePickup;
}
if (gamepad2.rightBumperWasPressed()){
stack3 = !stack3;
}
turret.setTurret(turretShootPos);
robot.hood.setPosition(shoot0Hood);
if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance;
}
if (gamepad2.dpadLeftWasPressed()) {
turrDefault -= 0.01;
}
if (gamepad2.dpadRightWasPressed()) {
turrDefault += 0.01;
}
if (redAlliance) {
robot.light.setPosition(0.28);
xLeave = rLeaveX;
yLeave = rLeaveY;
hLeave = rLeaveH;
xShoot = rShootX;
yShoot = rShootY;
hShoot = rShootH;
xStackPickupA = rStackPickupAX;
yStackPickupA = rStackPickupAY;
hStackPickupA = rStackPickupAH;
xStackPickupB = rStackPickupBX;
yStackPickupB = rStackPickupBY;
hStackPickupB = rStackPickupBH;
turretShootPos = turrDefault + redTurretShootPos;
} else {
robot.light.setPosition(0.6);
xLeave = bLeaveX;
yLeave = bLeaveY;
hLeave = bLeaveH;
xShoot = bShootX;
yShoot = bShootY;
hShoot = bShootH;
xStackPickupA = bStackPickupAX;
yStackPickupA = bStackPickupAY;
hStackPickupA = bStackPickupAH;
xStackPickupB = bStackPickupBX;
yStackPickupB = bStackPickupBY;
hStackPickupB = bStackPickupBH;
turretShootPos = turrDefault + blueTurretShootPos;
}
leave3Ball = drive.actionBuilder(autoStart)
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
leaveFromShoot = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA))
.strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB),
new TranslationalVelConstraint(pickupStackSpeed));
shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.addData("Gate Cycle?", gatePickup);
TELE.addData("Pickup Stack?", stack3);
TELE.addData("Start Position", autoStart);
TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
// Currently only shoots; keep this start and modify times and then add extra paths
if (opModeIsActive()) {
double stamp = getRuntime();
robot.transfer.setPower(1);
startAuto();
if (stack3){
//cycleStackFar();
}
if (gatePickup || stack3){
leave();
} else {
leave3Ball();
}
// Actual way to end autonomous in to find final position
while (opModeIsActive()) {
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
flywheel.manageFlywheel(0);
TELE.addLine("finished");
TELE.update();
}
}
}
void startAuto(){
Actions.runBlocking(
new ParallelAction(
manageFlywheel(
shoot0Vel,
shoot0Hood,
flywheel0Time,
0.501,
0.501,
shoot0XTolerance,
0.501
)
)
);
Actions.runBlocking(
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
);
}
void leave3Ball(){
assert leave3Ball != null;
Actions.runBlocking(leave3Ball.build());
}
void leave(){
assert leaveFromShoot != null;
Actions.runBlocking(leaveFromShoot.build());
}
// void cycleStackFar(){
// Actions.runBlocking(
// new ParallelAction(
// pickup3.build(),
// manageShooterAuto(
// intake3Time,
// 0.501,
// 0.501,
// 0.501,
// 0.501
// ),
// intake(intake3Time),
// detectObelisk(
// intake3Time,
// 0.501,
// 0.501,
// 0.501,
// 0.501,
// obeliskTurrPos3
// )
//
// )
// );
//
// motif = turret.getObeliskID();
//
// if (motif == 0) motif = prevMotif;
// prevMotif = motif;
//
// Actions.runBlocking(
// new ParallelAction(
// manageFlywheelAuto(
// shoot3Time,
// 0.501,
// 0.501,
// 0.501,
// 0.501
// ),
// shoot3.build(),
// prepareShootAll(colorSenseTime, shoot3Time, motif)
// )
// );
//
// Actions.runBlocking(
// new ParallelAction(
// manageShooterAuto(
// finalShootAllTime,
// 0.501,
// 0.501,
// 0.501,
// 0.501
// ),
// shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
// )
//
// );
// }
}

403
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java
View File

@@ -1,403 +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.teleEnd;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
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.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
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 Auto_LT_Far_3Ball extends LinearOpMode {
public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015;
public static double redObeliskTurrPos1 = turrDefault + 0.12;
public static double redObeliskTurrPos2 = turrDefault + 0.13;
public static double redObeliskTurrPos3 = turrDefault + 0.14;
public static double blueObeliskTurrPos1 = turrDefault - 0.12;
public static double blueObeliskTurrPos2 = turrDefault - 0.13;
public static double blueObeliskTurrPos3 = turrDefault - 0.14;
public static double shoot0XTolerance = 1.0;
public static double redTurretShootPos = turrDefault;
public static double blueTurretShootPos = turrDefault;
public static int fwdTime = 200, strafeTime = 2300;
public static double rPickupGateX = 1, rPickupGateY = 1, rPickupGateH = 1;
public static double rPickupZoneX = 1, rPickupZoneY = 1, rPickupZoneH = 1;
public static double rShootX = 1, rShootY = 1, rShootH = 1;
public static double bPickupZoneX = 1, bPickupZoneY = 1, bPickupZoneH = 1;
public static double bPickupGateX = 1, bPickupGateY = 1, bPickupGateH = 1;
public static double bShootX = 1, bShootY = 1, bShootH = 1;
public static int sleepTime = 1300;
public int motif = 0;
double turretShootPos = 0.0;
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
Servos servos;
Spindexer spindexer;
Flywheel flywheel;
Turret turret;
Targeting targeting;
Targeting.Settings targetingSettings;
private double firstSpindexShootPos = autoSpinStartPos;
private boolean shootForward = true;
private double xShoot, yShoot, hShoot;
private int driverSlotGreen = 0;
private int passengerSlotGreen = 0;
private int rearSlotGreen = 0;
private int mostGreenSlot = 0;
private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
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();
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);
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 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;
}
};
}
@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.start();
robot.limelight.pipelineSwitch(1);
turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, teleEnd);
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
robot.transferServo.setPosition(transferServo_out);
robot.light.setPosition(1);
while (opModeInInit()) {
robot.hood.setPosition(shoot0Hood);
turret.manualSetTurret(turretShootPos);
if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance;
}
if (gamepad2.dpadLeftWasPressed()) {
turrDefault -= 0.01;
}
if (gamepad2.dpadRightWasPressed()) {
turrDefault += 0.01;
}
redObeliskTurrPos1 = turrDefault + 0.12;
redObeliskTurrPos2 = turrDefault + 0.13;
redObeliskTurrPos3 = turrDefault + 0.14;
blueObeliskTurrPos1 = turrDefault - 0.12;
blueObeliskTurrPos2 = turrDefault - 0.13;
blueObeliskTurrPos3 = turrDefault - 0.14;
redTurretShootPos = turrDefault + 0.05;
if (redAlliance) {
robot.light.setPosition(0.28);
pickupGateX = rPickupGateX;
pickupGateY = rPickupGateY;
pickupGateH = rPickupGateH;
pickupZoneX = rPickupZoneX;
pickupZoneY = rPickupZoneY;
pickupZoneH = rPickupZoneH;
xShoot = rShootX;
yShoot = rShootY;
hShoot = rShootH;
turretShootPos = redTurretShootPos;
} else {
robot.light.setPosition(0.6);
pickupGateX = bPickupGateX;
pickupGateY = bPickupGateY;
pickupGateH = bPickupGateH;
pickupZoneX = bPickupZoneX;
pickupZoneY = bPickupZoneY;
pickupZoneH = bPickupZoneH;
xShoot = bShootX;
yShoot = bShootY;
hShoot = bShootH;
turretShootPos = blueTurretShootPos;
}
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()) {
robot.transfer.setPower(1);
Actions.runBlocking(
new ParallelAction(
manageFlywheel(
shoot0Vel,
shoot0Hood,
9,
0.501,
0.501,
shoot0XTolerance,
0.501
)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease)
);
robot.frontLeft.setPower(-0.4);
robot.frontRight.setPower(-0.4);
robot.backLeft.setPower(-0.4);
robot.backRight.setPower(-0.4);
sleep(fwdTime);
robot.frontLeft.setPower(0);
robot.frontRight.setPower(0);
robot.backLeft.setPower(0);
robot.backRight.setPower(0);
sleep(sleepTime);
robot.frontLeft.setPower(-0.4);
robot.frontRight.setPower(0.4);
robot.backLeft.setPower(0.4);
robot.backRight.setPower(-0.4);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
sleep(strafeTime);
robot.frontLeft.setPower(0);
robot.frontRight.setPower(0);
robot.backLeft.setPower(0);
robot.backRight.setPower(0);
while (opModeIsActive()) {
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addLine("finished");
TELE.update();
}
}
}
}

648
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/Actions.java Normal file
View File

@@ -0,0 +1,648 @@
package org.firstinspires.ftc.teamcode.autonomous.actions;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
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_outtakeBall1;
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.spindexer_outtakeBall3b;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.Pose2d;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Drivetrain;
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;
import java.util.Objects;
public class Actions{
Robot robot;
MultipleTelemetry TELE;
Servos servos;
Flywheel flywheel;
MecanumDrive drive;
Spindexer spindexer;
Targeting targeting;
Targeting.Settings targetingSettings;
Turret turret;
private int driverSlotGreen = 0;
private int passengerSlotGreen = 0;
private int rearSlotGreen = 0;
private int mostGreenSlot = 0;
private double firstSpindexShootPos = spinStartPos;
private boolean shootForward = true;
public static double firstShootTime = 0.3;
public int motif = 0;
public Actions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur){
this.robot = rob;
this.drive = dri;
this.TELE = tel;
this.servos = ser;
this.flywheel = fly;
this.spindexer = spi;
this.targeting = tar;
this.targetingSettings = tS;
this.turret = tur;
}
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
double spindexerWiggle = 0.01;
boolean decideGreenSlot = false;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
servos.setTransferPos(transferServo_out);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.addData("motif", motif_id);
TELE.update();
if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
spindexerWiggle *= -1.0;
servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
spindexer.detectBalls(true, true);
if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) {
driverSlotGreen++;
}
if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) {
passengerSlotGreen++;
}
if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) {
rearSlotGreen++;
}
spindexer.setIntakePower(1);
decideGreenSlot = true;
return true;
} else if (decideGreenSlot) {
if (driverSlotGreen >= passengerSlotGreen && driverSlotGreen >= rearSlotGreen) {
mostGreenSlot = 3;
} else if (passengerSlotGreen >= driverSlotGreen && passengerSlotGreen >= rearSlotGreen) {
mostGreenSlot = 2;
} else {
mostGreenSlot = 1;
}
decideGreenSlot = false;
if (motif_id == 21) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
} else {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
}
} else if (motif_id == 22) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
} else {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = true;
}
} else {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
} else {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
}
}
return true;
} else if ((System.currentTimeMillis() - stamp) < (time * 1000)) {
// TELE.addData("MostGreenSlot", mostGreenSlot);
// TELE.update();
spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
servos.setSpinPos(firstSpindexShootPos);
return true;
} else {
return false;
}
}
};
}
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();
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = System.currentTimeMillis();
}
ticker++;
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if ((System.currentTimeMillis() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) {
servos.setSpinPos(spinStartPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = servos.getSpinCmdPos();
servos.setSpinPos(prevSpinPos + spindexSpeed);
}
return true;
} else {
servos.setTransferPos(transferServo_out);
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();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = System.currentTimeMillis();
}
ticker++;
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if (System.currentTimeMillis() - stamp < shootTime) {
if (System.currentTimeMillis() - stamp < firstShootTime) {
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = servos.getSpinCmdPos();
if (shootForward) {
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else {
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
}
return true;
} else {
servos.setTransferPos(transferServo_out);
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();
spindexer.setIntakePower(1);
spindexer.ballCounterLight();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Full?", spindexer.isFull());
TELE.update();
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
}
};
}
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();
robot.limelight.pipelineSwitch(1);
}
ticker++;
motif = turret.detectObelisk();
turret.setTurret(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();
if (shouldFinish){
if (redAlliance){
robot.limelight.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
}
return false;
} else {
return true;
}
}
};
}
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++;
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
servos.setHoodPos(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);
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
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);
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
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;
}
};
}
}

117
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java
View File

@@ -1,61 +1,61 @@
package org.firstinspires.ftc.teamcode.autonomous;
package org.firstinspires.ftc.teamcode.autonomous.disabled;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
@@ -76,7 +76,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Poses_V2;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -170,7 +169,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
robot.transferServo.setPosition(transferServo_out);
turret.manualSetTurret(turretShootPos);
turret.setTurret(turretShootPos);
robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000);
drive.updatePoseEstimate();
@@ -612,7 +611,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(0.4);
turret.setTurret(0.4);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));

118
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java
View File

@@ -1,61 +1,61 @@
package org.firstinspires.ftc.teamcode.autonomous;
package org.firstinspires.ftc.teamcode.autonomous.disabled;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3aG;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3aG;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3aG;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3aG;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3aG;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3aG;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3aG;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3aG;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3aG;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3aG;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3aG;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3aG;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
@@ -78,6 +78,7 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Poses_V2;
@@ -91,6 +92,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
import java.util.Objects;
@Disabled
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close_GateOpen extends LinearOpMode {
@@ -205,7 +207,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
ticker++;
robot.transferServo.setPosition(transferServo_out);
turret.manualSetTurret(turretShootPos);
turret.setTurret(turretShootPos);
drive.updatePoseEstimate();
@@ -750,7 +752,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(0.4);
turret.setTurret(0.4);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));

86
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java
View File

@@ -1,49 +1,49 @@
package org.firstinspires.ftc.teamcode.autonomous.disabled;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java Normal file
View File

@@ -0,0 +1,23 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Back_Poses {
public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50.1;
public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50;
public static double rShootX = 95, rShootY = 85, rShootH = 90;
public static double bShootX = 95, bShootY = -85, bShootH = -90;
public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140;
public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
public static double rStackPickupBX = 50, rStackPickupBY = 78, rStackPickupBH = 140.1;
public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1;
public static Pose2d autoStart = new Pose2d(0, 0, 0); // TODO: find this position
}

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java Normal file
View File

@@ -0,0 +1,46 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Front_Poses {
public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
public static double bx1 = 20, by1 = 0.5, bh1 = 0.1;
public static double rx2a = 41, ry2a = 18, rh2a = 140;
public static double bx2a = 41, by2a = -18, bh2a = -140;
public static double rx2b = 23, ry2b = 36, rh2b = 140.1;
public static double bx2b = 19, by2b = -40, bh2b = -140.1;
public static double rx3a = 55, ry3a = 39, rh3a = 140;
public static double bx3a = 55, by3a = -39, bh3a = -140;
public static double rx3aG = 60, ry3aG = 34, rh3aG = 140;
public static double bx3aG = 55, by3aG = -43, bh3aG = -140;
public static double rx3b = 36, ry3b = 58, rh3b = 140.1;
public static double bx3b = 41, by3b = -59, bh3b = -140.1;
public static double rx4a = 75, ry4a = 53, rh4a = 140;
public static double bx4a = 75, by4a = -53, bh4a = -140;
public static double rx4b = 50, ry4b = 78, rh4b = 140.1;
public static double bx4b = 47, by4b = -85, bh4b = -140.1;
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0;
public static double rShootX = 40, rShootY = 10, rShootH = 50;
public static double bShootX = 40, bShootY = 0, bShootH = -50;
public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
public static double bxPrep = 45, byPrep = -10, bhPrep = -50;
public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50;
public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50;
public static Pose2d teleStart = new Pose2d(0, 0, 0);
}

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

@@ -1,49 +0,0 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Poses {
public static double rx1 = 20, ry1 = 0.5, rh1 = Math.toRadians(0.1);
public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140);
public static double rx2b = 19, ry2b = 40, rh2b = Math.toRadians(140.1);
public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
public static double rx3aG = 60, ry3aG = 34, rh3aG = Math.toRadians(140);
public static double rx3b = 36, ry3b = 58, rh3b = Math.toRadians(140.1);
public static double rx4a = 75, ry4a = 53, rh4a = Math.toRadians(140);
public static double rx4b = 45, ry4b = 83, rh4b = Math.toRadians(140.1);
public static double bx1 = 20, by1 = 0.5, bh1 = Math.toRadians(0.1);
public static double bx2a = 41, by2a = -18, bh2a = Math.toRadians(-140);
public static double bx2b = 19, by2b = -40, bh2b = Math.toRadians(-140.1);
public static double bx3a = 55, by3a = -39, bh3a = Math.toRadians(-140);
public static double bx3b = 41, by3b = -59, bh3b = Math.toRadians(-140.1);
public static double bx3aG = 55, by3aG = -43, bh3aG = Math.toRadians(-140);
public static double bx4a = 75, by4a = -53, bh4a = Math.toRadians(-140);
public static double bx4b = 47, by4b = -85, bh4b = Math.toRadians(-140.1);
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
public static double rShootX = 40, rShootY = -7, rShootH = Math.toRadians(50);
public static double rxPrep = 45, ryPrep = 10, rhPrep = Math.toRadians(50);
public static double bShootX = 40, bShootY = 7, bShootH = Math.toRadians(-50);
public static double bxPrep = 45, byPrep = -10, bhPrep = Math.toRadians(-50);
public static Pose2d teleStart = new Pose2d(0, 0, 0);
public static Pose2d teleEnd = new Pose2d(0, 0, 0);
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java
View File

@@ -1,7 +1,9 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
@Disabled
@Config
public class Poses_V2 {

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

@@ -32,4 +32,14 @@ public class ServoPositions {
public static double turret_redClose = 0.42;
public static double turret_blueClose = 0.38;
// These values are ADDED to turrDefault
public static double redObeliskTurrPos1 = 0.12;
public static double redObeliskTurrPos2 = 0.13;
public static double redObeliskTurrPos3 = 0.14;
public static double blueObeliskTurrPos1 = -0.12;
public static double blueObeliskTurrPos2 = -0.13;
public static double blueObeliskTurrPos3 = -0.14;
public static double redTurretShootPos = 0.1;
public static double blueTurretShootPos = -0.14;
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java
View File

@@ -1,7 +1,9 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
@Disabled
@Config
public class ShooterVars {

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

@@ -1,10 +1,10 @@
package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import com.acmerobotics.dashboard.FtcDashboard;
@@ -22,6 +22,7 @@ import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Drivetrain;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Light;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
@@ -33,8 +34,7 @@ import java.util.List;
@Config
@TeleOp
public class TeleopV3 extends LinearOpMode {
public static double manualVel = 1000;
//Testing a new comment
public static double manualVel = 3000;
public static double hoodDefaultPos = 0.5;
public static double spinPow = 0.09;
@@ -59,6 +59,7 @@ public class TeleopV3 extends LinearOpMode {
Targeting targeting;
Targeting.Settings targetingSettings;
Drivetrain drivetrain;
MeasuringLoopTimes loopTimes;
double autoHoodOffset = 0.0;
int shooterTicker = 0;
boolean intake = false;
@@ -74,8 +75,6 @@ public class TeleopV3 extends LinearOpMode {
boolean overrideTurr = false;
int intakeTicker = 0;
boolean turretInterpolate = false;
private boolean shootAll = false;
@Override
@@ -98,6 +97,9 @@ public class TeleopV3 extends LinearOpMode {
drivetrain = new Drivetrain(robot, drive);
loopTimes = new MeasuringLoopTimes();
loopTimes.init();
PIDFController tController = new PIDFController(tp, ti, td, tf);
tController.setTolerance(0.001);
@@ -108,6 +110,9 @@ public class TeleopV3 extends LinearOpMode {
light.init(robot.light, spindexer, turret);
light.setState(StateEnums.LightState.MANUAL);
limelightUsed = true;
robot.light.setPosition(1);
while (opModeInInit()) {
robot.limelight.start();
if (redAlliance) {
@@ -127,7 +132,8 @@ public class TeleopV3 extends LinearOpMode {
waitForStart();
if (isStopRequested()) return;
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
robot.transfer.setPower(1);
while (opModeIsActive()) {
@@ -145,7 +151,7 @@ public class TeleopV3 extends LinearOpMode {
if (gamepad1.right_bumper) {
shootAll = false;
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
light.setState(StateEnums.LightState.BALL_COUNT);
@@ -156,9 +162,7 @@ public class TeleopV3 extends LinearOpMode {
light.setState(StateEnums.LightState.GOAL_LOCK);
}
robot.transfer.setPower(1);
double offset;
//TURRET TRACKING
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
@@ -205,14 +209,16 @@ public class TeleopV3 extends LinearOpMode {
}
//SHOOTER:
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
//HOOD:
if (targetingHood) {
robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset);
servo.setHoodPos(targetingSettings.hoodAngle + autoHoodOffset);
} else {
robot.hood.setPosition(hoodDefaultPos);
servo.setHoodPos(hoodDefaultPos);
}
if (gamepad2.dpadUpWasPressed()) {
@@ -244,6 +250,8 @@ public class TeleopV3 extends LinearOpMode {
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
}
if (enableSpindexerManager) {
//if (!shootAll) {
spindexer.processIntake();
@@ -251,13 +259,12 @@ public class TeleopV3 extends LinearOpMode {
// RIGHT_BUMPER
if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
robot.intake.setPower(1);
spindexer.setIntakePower(1);
} else if (gamepad1.cross) {
robot.intake.setPower(-1);
spindexer.setIntakePower(-1);
} else {
robot.intake.setPower(0);
spindexer.setIntakePower(0);
}
// LEFT_BUMPER
@@ -278,14 +285,14 @@ public class TeleopV3 extends LinearOpMode {
spindexer.prepareShootAllContinous();
//TELE.addLine("preparing to shoot");
// } else if (shooterTicker == 2) {
// //robot.transferServo.setPosition(transferServo_in);
// //servo.setTransferPos(transferServo_in);
// spindexer.shootAll();
// TELE.addLine("starting to shoot");
} else if (!spindexer.shootAllComplete()) {
robot.transferServo.setPosition(transferServo_in);
servo.setTransferPos(transferServo_in);
//TELE.addLine("shoot");
} else {
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
//spindexPos = spindexer_intakePos1;
shootAll = false;
spindexer.resetSpindexer();
@@ -297,28 +304,20 @@ public class TeleopV3 extends LinearOpMode {
}
if (gamepad1.left_stick_button) {
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
//spindexPos = spindexer_intakePos1;
shootAll = false;
spindexer.resetSpindexer();
}
}
if (gamepad2.square) {
drive.updatePoseEstimate();
teleEnd = drive.localizer.getPose();
gamepad2.rumble(1000);
}
//EXTRA STUFFINESS:
drive.updatePoseEstimate();
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}
loopTimes.loop();
//
// TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
// TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
@@ -342,7 +341,7 @@ public class TeleopV3 extends LinearOpMode {
// TELE.addData("spinTestCounter", spindexer.counter);
// TELE.addData("autoSpintake", autoSpintake);
//
TELE.addData("shootall commanded", shootAll);
// TELE.addData("shootall commanded", shootAll);
// Targeting Debug
TELE.addData("robotX", robotX);
TELE.addData("robotY", robotY);
@@ -354,6 +353,10 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.addData("Voltage", voltage); // returns alleged recorded voltage (not same as driver hub)
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.update();

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java
View File

@@ -60,7 +60,7 @@ public class LimelightTest extends LinearOpMode {
if (turretMode){
if (turretPos != 0.501){
turret.manualSetTurret(turretPos);
turret.setTurret(turretPos);
}
}

62
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
View File

@@ -1,23 +1,30 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
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_outtakeBall1;
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.teleop.TeleopV3.spinSpeedIncrease;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
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
@TeleOp
@@ -26,22 +33,23 @@ public class ShooterTest extends LinearOpMode {
public static double parameter = 0.0;
// --- CONSTANTS YOU TUNE ---
//TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED
public static double Velocity = 0.0;
public static double P = 255.0;
public static double I = 0.0;
public static double D = 0.0;
public static double F = 7.5;
public static double F = 90;
public static double transferPower = 1.0;
public static double hoodPos = 0.501;
public static double turretPos = 0.501;
public static boolean shoot = false;
public static boolean intake = false;
public static boolean turretTrack = true;
Robot robot;
Flywheel flywheel;
Servos servo;
MecanumDrive drive;
Turret turret;
double shootStamp = 0.0;
boolean shootAll = false;
@@ -51,6 +59,7 @@ public class ShooterTest extends LinearOpMode {
public double hoodAdjust = 0.0;
public static double hoodAdjustFactor = 1.0;
private int shooterTicker = 0;
public static double spinSpeed = 0.02;
Spindexer spindexer ;
@Override
@@ -62,22 +71,58 @@ public class ShooterTest extends LinearOpMode {
flywheel = new Flywheel(hardwareMap);
spindexer = new Spindexer(hardwareMap);
servo = new Servos(hardwareMap);
drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
MultipleTelemetry TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
turret = new Turret(robot, TELE, robot.limelight);
Turret.limelightUsed = true;
waitForStart();
robot.limelight.start();
if (isStopRequested()) return;
while (opModeIsActive()) {
if (redAlliance){
robot.limelight.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
}
//TURRET TRACKING
drive.updatePoseEstimate();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
if (turretTrack){
turret.trackGoal(deltaPose);
} else if (turretPos != 0.501){
turret.setTurret(turretPos);
}
double voltage = robot.voltage.getVoltage();
if (mode == 0) {
rightShooter.setPower(parameter);
leftShooter.setPower(parameter);
} else if (mode == 1) {
flywheel.setPIDF(P, I, D, F);
flywheel.setPIDF(P, I, D, F / voltage);
flywheel.manageFlywheel((int) Velocity);
}
@@ -109,7 +154,6 @@ public class ShooterTest extends LinearOpMode {
//intake = false;
//reject = false;
// TODO: Change starting position based on desired order to shoot green ball
//spindexPos = spindexer_intakePos1;
if (getRuntime() - shootStamp < 3.5) {
@@ -120,8 +164,11 @@ public class ShooterTest extends LinearOpMode {
robot.transferServo.setPosition(transferServo_in);
shooterTicker++;
double prevSpinPos = robot.spin1.getPosition();
robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease);
robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
if (prevSpinPos < 0.9){
robot.spin1.setPosition(prevSpinPos + spinSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spinSpeed);
}
}
@@ -150,6 +197,7 @@ public class ShooterTest extends LinearOpMode {
TELE.addData("Power", robot.shooter1.getPower());
TELE.addData("Steady?", flywheel.getSteady());
TELE.addData("Position", robot.shooter1.getCurrentPosition());
TELE.addData("Voltage", voltage);
TELE.update();

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
View File

@@ -12,6 +12,7 @@ public class Flywheel {
public double velo1 = 0.0;
public double velo2 = 0.0;
double targetVelocity = 0.0;
double previousTargetVelocity = 0.0;
double powPID = 0.0;
boolean steady = false;
public Flywheel (HardwareMap hardwareMap) {
@@ -37,6 +38,8 @@ public class Flywheel {
}
// Set the robot PIDF for the next cycle.
private double prevF = 0.501;
public static int voltagePIDFDifference = 5;
public void setPIDF(double p, double i, double d, double f) {
shooterPIDF1.p = p;
shooterPIDF1.i = i;
@@ -46,6 +49,10 @@ public class Flywheel {
shooterPIDF2.i = i;
shooterPIDF2.d = d;
shooterPIDF2.f = f;
if (Math.abs(prevF - f) > voltagePIDFDifference){
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
}
}
// Convert from RPM to Ticks per Second
@@ -55,20 +62,21 @@ public class Flywheel {
private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;}
public double manageFlywheel(double commandedVelocity) {
if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
targetVelocity = commandedVelocity;
// Add code here to set PIDF based on desired RPM
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
//robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
//robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
// Record Current Velocity
velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
velo = Math.max(velo1,velo2);
velo = Math.max(velo1, velo2);
}
// really should be a running average of the last 5
steady = (Math.abs(targetVelocity - velo) < 200.0);

66
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java Normal file
View File

@@ -0,0 +1,66 @@
package org.firstinspires.ftc.teamcode.utils;
import com.qualcomm.robotcore.util.ElapsedTime;
public class MeasuringLoopTimes {
private ElapsedTime elapsedtime;
private double minLoopTime = 999999999999.0;
private double maxLoopTime = 0.0;
private double mainLoopTime = 0.0;
private double MeasurementStart = 0.0;
private double currentTime = 0.0;
private double avgLoopTime = 0.0;
private int avgLoopTimeTicker = 0;
private double avgLoopTimeSum = 0;
private double getTimeSeconds ()
{
return (double) System.currentTimeMillis()/1000.0;
}
public void init() {
elapsedtime = new ElapsedTime();
elapsedtime.reset();
MeasurementStart = getTimeSeconds();
}
public double getAvgLoopTime() {
return avgLoopTime;
}
public double getMaxLoopTimeOneMin() {
return maxLoopTime;
}
public double getMinLoopTimeOneMin() {
return minLoopTime;
}
public void loop() {
currentTime = getTimeSeconds();
if ((MeasurementStart + 15.0) < currentTime)
{
minLoopTime = 9999999.0;
maxLoopTime = 0.0;
MeasurementStart = currentTime;
avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker;
avgLoopTimeSum = 0.0;
avgLoopTimeTicker = 0;
}
mainLoopTime = elapsedtime.milliseconds();
elapsedtime.reset();
avgLoopTimeSum += mainLoopTime;
avgLoopTimeTicker++;
minLoopTime = Math.min(minLoopTime,mainLoopTime);
maxLoopTime = Math.max(maxLoopTime,mainLoopTime);
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java
View File

@@ -35,7 +35,7 @@ public class PositionalServoProgrammer extends LinearOpMode {
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()){
if (spindexPos != 0.501 && !servo.spinEqual(spindexPos)){
if (spindexPos != 0.501){
robot.spin1.setPosition(spindexPos);
robot.spin2.setPosition(1-spindexPos);
}
@@ -52,16 +52,6 @@ public class PositionalServoProgrammer extends LinearOpMode {
if (light !=0.501){
robot.light.setPosition(light);
}
// To check configuration of spindexer:
// Set "mode" to 1 and spindexPow to 0.1
// If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE
// Do the previous test again to confirm
// Set "mode" to 0 but keep spindexPos at 0.501
// Manually turn the spindexer until "spindexer pos" is set close to 0
// Check each spindexer voltage:
// If "spindexer voltage 1" is closer to 0 than "spindexer voltage 2," then you are done!
// If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE
//TODO: @KeshavAnandCode do the above please
TELE.addData("spindexer pos", servo.getSpinPos());
TELE.addData("turret pos", robot.turr1.getPosition());

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

@@ -12,6 +12,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
@@ -33,7 +34,7 @@ public class Robot {
public double shooterPIDF_P = 255.0;
public double shooterPIDF_I = 0.0;
public double shooterPIDF_D = 0.0;
public double shooterPIDF_F = 7.5;
public double shooterPIDF_F = 90;
public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
public DcMotorEx shooter1;
public DcMotorEx shooter2;
@@ -56,11 +57,11 @@ public class Robot {
public RevColorSensorV3 color3;
public Limelight3A limelight;
public Servo light;
public VoltageSensor voltage;
public Robot(HardwareMap hardwareMap) {
//Define components w/ hardware map
//TODO: fix the configuration of these - I trust you to figure it out yourself @KeshavAnandCode
frontLeft = hardwareMap.get(DcMotorEx.class, "fl");
frontRight = hardwareMap.get(DcMotorEx.class, "fr");
backLeft = hardwareMap.get(DcMotorEx.class, "bl");
@@ -78,7 +79,7 @@ public class Robot {
shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
//TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode
shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F);
shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
@@ -96,7 +97,6 @@ public class Robot {
turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
//TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer
spin1 = hardwareMap.get(Servo.class, "spin2");
spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
@@ -128,5 +128,6 @@ public class Robot {
}
light = hardwareMap.get(Servo.class, "light");
voltage = hardwareMap.voltageSensor.iterator().next();
}
}

45
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
View File

@@ -18,6 +18,15 @@ public class Servos {
PIDFController spinPID;
PIDFController turretPID;
private double prevSpinPos = 0.0;
private boolean firstSpinPos = true;
private double prevTransferPos = 0.0;
private boolean firstTransferPos = true;
private double prevHoodPos = 0.0;
private boolean firstHoodPos = true;
public Servos(HardwareMap hardwareMap) {
robot = new Robot(hardwareMap);
spinPID = new PIDFController(spinP, spinI, spinD, spinF);
@@ -32,8 +41,42 @@ public class Servos {
return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3);
}
public double setSpinPos(double pos) {
public double getSpinCmdPos() {
return prevSpinPos;
}
public static boolean servoPosEqual(double pos1, double pos2) {
return (Math.abs(pos1 - pos2) < 0.005);
}
public double setTransferPos(double pos) {
if (firstTransferPos || !servoPosEqual(pos, prevTransferPos)) {
robot.transferServo.setPosition(pos);
firstTransferPos = false;
}
prevTransferPos = pos;
return pos;
}
public double setSpinPos(double pos) {
if (firstSpinPos || !servoPosEqual(pos, prevSpinPos)) {
robot.spin1.setPosition(pos);
robot.spin2.setPosition(1-pos);
firstSpinPos = false;
}
prevSpinPos = pos;
return pos;
}
public double setHoodPos(double pos){
if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) {
robot.hood.setPosition(pos);
firstHoodPos = false;
}
prevHoodPos = pos;
return pos;
}

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

@@ -1,5 +1,12 @@
package org.firstinspires.ftc.teamcode.utils;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import static org.firstinspires.ftc.teamcode.constants.Color.*;
import static org.firstinspires.ftc.teamcode.constants.Color.Light0;
import static org.firstinspires.ftc.teamcode.constants.Color.Light1;
import static org.firstinspires.ftc.teamcode.constants.Color.Light2;
@@ -44,6 +51,42 @@ public class Spindexer {
public double spindexerWiggle = 0.01;
public double spindexerOuttakeWiggle = 0.01;
public double spindexerPosOffset = 0.00;
public static int shootWaitMax = 4;
public Types.Motif desiredMotif = Types.Motif.NONE;
// For Use
enum RotatedBallPositionNames {
REARCENTER,
FRONTDRIVER,
FRONTPASSENGER
}
// Array of commandedIntakePositions with contents
// {RearCenter, FrontDriver, FrontPassenger}
static final int[][] RotatedBallPositions = {{0,2,1}, {1,0,2}, {2,1,0}};
class spindexerBallRoatation {
int rearCenter = 0; // aka commanded Position
int frontDriver = 0;
int frontPassenger = 0;
}
enum IntakeState {
UNKNOWN_START,
UNKNOWN_MOVE,
UNKNOWN_DETECT,
INTAKE,
FINDNEXT,
MOVING,
FULL,
SHOOTNEXT,
SHOOTMOVING,
SHOOTWAIT,
SHOOT_ALL_PREP,
SHOOT_ALL_READY,
SHOOT_PREP_CONTINOUS,
SHOOT_CONTINOUS
}
int shootWaitCount = 0;
public StateEnums.Motif desiredMotif = StateEnums.Motif.NONE;
public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
@@ -126,11 +169,9 @@ public class Spindexer {
if (detectRearColor) {
// Detect which color
double green = robot.color1.getNormalizedColors().green;
double red = robot.color1.getNormalizedColors().red;
double blue = robot.color1.getNormalizedColors().blue;
NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors();
double gP = green / (green + red + blue);
double gP = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue);
// FIXIT - Add filtering to improve accuracy.
if (gP >= 0.38) {
@@ -147,11 +188,9 @@ public class Spindexer {
// reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0;
if (detectFrontColor) {
double green = robot.color2.getNormalizedColors().green;
double red = robot.color2.getNormalizedColors().red;
double blue = robot.color2.getNormalizedColors().blue;
NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors();
double gP = green / (green + red + blue);
double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue);
if (gP >= 0.4) {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
@@ -176,11 +215,9 @@ public class Spindexer {
// reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0;
if (detectFrontColor) {
double green = robot.color3.getNormalizedColors().green;
double red = robot.color3.getNormalizedColors().red;
double blue = robot.color3.getNormalizedColors().blue;
NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors();
double gP = green / (green + red + blue);
double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue);
if (gP >= 0.42) {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
@@ -212,8 +249,7 @@ public class Spindexer {
// Has code to unjam spindexer
private void moveSpindexerToPos(double pos) {
robot.spin1.setPosition(pos);
robot.spin2.setPosition(1 - pos);
servos.setSpinPos(pos);
// double currentPos = servos.getSpinPos();
// if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
// if (currentPos > pos){
@@ -224,7 +260,7 @@ public class Spindexer {
// robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05);
// }
// }
// prevPos = currentPos;
// prevPos = pos;
}
// private double getTimeSeconds ()
@@ -246,6 +282,8 @@ public class Spindexer {
}
private double prevLight = 0.0;
public void ballCounterLight(){
public double getRearCenterLight() {
BallColor color = GetRearCenterColor();
if (Objects.equals(color, BallColor.GREEN)) {
@@ -291,6 +329,14 @@ public class Spindexer {
counter++;
}
double light;
if (counter == 3){
light = Light3;
} else if (counter == 2){
light = Light2;
} else if (counter == 1){
light = Light1;
if (counter == 3) {
return Light3;
} else if (counter == 2) {
@@ -298,8 +344,13 @@ public class Spindexer {
} else if (counter == 1) {
return Light1;
} else {
light = Light0;
return Light0;
}
if (light != prevLight){
robot.light.setPosition(light);
}
prevLight = light;
}
public boolean slotIsEmpty(int slot) {
@@ -316,7 +367,7 @@ public class Spindexer {
case UNKNOWN_START:
// For now just set position ONE if UNKNOWN
commandedIntakePosition = 0;
moveSpindexerToPos(intakePositions[0]);
servos.setSpinPos(intakePositions[0]);
currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE;
break;
case UNKNOWN_MOVE:
@@ -327,7 +378,7 @@ public class Spindexer {
unknownColorDetect = 0;
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
servos.setSpinPos(intakePositions[commandedIntakePosition]);
}
break;
case UNKNOWN_DETECT:
@@ -346,7 +397,7 @@ public class Spindexer {
} else {
// Maintain Position
spindexerWiggle *= -1.0;
moveSpindexerToPos(intakePositions[commandedIntakePosition] + spindexerWiggle);
servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
}
break;
case FINDNEXT:
@@ -378,7 +429,7 @@ public class Spindexer {
//commandedIntakePosition = bestFitMotif();
currentIntakeState = Spindexer.IntakeState.FULL;
}
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
servos.setSpinPos(intakePositions[commandedIntakePosition]);
break;
case MOVING:
@@ -394,7 +445,7 @@ public class Spindexer {
//detectBalls(false, false);
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
servos.setSpinPos(intakePositions[commandedIntakePosition]);
}
break;
@@ -407,7 +458,7 @@ public class Spindexer {
}
// Maintain Position
spindexerWiggle *= -1.0;
moveSpindexerToPos(intakePositions[commandedIntakePosition] + spindexerWiggle);
servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
break;
case SHOOT_ALL_PREP:
@@ -416,7 +467,7 @@ public class Spindexer {
commandedIntakePosition = 0;
if (!servos.spinEqual(outakePositions[commandedIntakePosition])) {
// Keep moving the spindexer
moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
servos.setSpinPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
}
break;
@@ -428,7 +479,7 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
}
// Maintain Position
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
servos.setSpinPos(outakePositions[commandedIntakePosition]);
break;
case SHOOTNEXT:
@@ -449,7 +500,7 @@ public class Spindexer {
// Empty return to intake state
currentIntakeState = IntakeState.FINDNEXT;
}
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
servos.setSpinPos(outakePositions[commandedIntakePosition]);
break;
case SHOOTMOVING:
@@ -458,12 +509,11 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
} else {
// Keep moving the spindexer
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
servos.setSpinPos(outakePositions[commandedIntakePosition]);
}
break;
case SHOOTWAIT:
double shootWaitMax = 4;
// Stopping when we get to the new position
if (prevIntakeState != currentIntakeState) {
if (commandedIntakePosition == 2) {
@@ -483,14 +533,14 @@ public class Spindexer {
}
// Keep moving the spindexer
spindexerOuttakeWiggle *= -1.0;
moveSpindexerToPos(outakePositions[commandedIntakePosition] + spindexerOuttakeWiggle);
servos.setSpinPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle);
break;
case SHOOT_PREP_CONTINOUS:
if (servos.spinEqual(spinStartPos)) {
currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
} else {
moveSpindexerToPos(spinStartPos);
servos.setSpinPos(spinStartPos);
}
break;
@@ -501,11 +551,11 @@ public class Spindexer {
if (servos.getSpinPos() > spinEndPos) {
currentIntakeState = IntakeState.FINDNEXT;
} else {
double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease;
if (spinPos > spinEndPos + 0.03) {
double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease;
if (spinPos > spinEndPos + 0.03){
spinPos = spinEndPos + 0.03;
}
moveSpindexerToPos(spinPos);
servos.setSpinPos(spinPos);
}
break;
@@ -657,4 +707,11 @@ public class Spindexer {
int foundEmpty = 0;
BallColor ballColor = BallColor.UNKNOWN;
}
private double prevPow = 0.501;
public void setIntakePower(double pow){
if (prevPow != 0.501 && prevPow != pow){
robot.intake.setPower(pow);
}
prevPow = pow;
}
}

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

@@ -77,6 +77,7 @@ public class Targeting {
double cancelOffsetY = 0.0; // was 7.0
double unitConversionFactor = 0.95;
int tileSize = 24; //inches
public static boolean turretInterpolate = false;
public Targeting() {
}

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

@@ -10,6 +10,7 @@ import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
import org.firstinspires.ftc.teamcode.constants.Color;
@@ -37,7 +38,8 @@ public class Turret {
// TODO: tune these values for limelight
public static double clampTolerance = 0.03;
public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
//public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
public static double B_PID_P = 0.066, B_PID_I = 0.0, B_PID_D = 0.007;
Robot robot;
MultipleTelemetry TELE;
Limelight3A webcam;
@@ -56,6 +58,9 @@ public class Turret {
private double permanentOffset = 0.0;
private PIDController bearingPID;
private double prevTurretPos = 0.0;
private boolean firstTurretPos = true;
public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
this.TELE = tele;
this.robot = rob;
@@ -76,10 +81,13 @@ public class Turret {
return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault;
}
public void manualSetTurret(double pos) {
private double prevTurrPos = 0.501;
public void setTurret(double pos) {
if (prevTurrPos != 0.501 && prevTurrPos != pos){
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1 - pos);
robot.turr2.setPosition(1-pos);
}
prevTurrPos = pos;
}
public boolean turretEqual(double pos) {
@@ -284,8 +292,7 @@ public class Turret {
}
// Set servo positions
robot.turr1.setPosition(turretPos + manualOffset);
robot.turr2.setPosition(1.0 - turretPos - manualOffset);
setTurret(turretPos + manualOffset);
/* ---------------- TELEMETRY ---------------- */