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

yayyy

Browse Source
This commit is contained in:
Keshav Anand
2026-01-17 00:53:05 -06:00
parent de6e7f2910
commit e8bf2033ad
15 changed files with 1047 additions and 1923 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java
View File

@@ -103,17 +103,17 @@ public class AutoClose_V3 extends LinearOpMode {
if (gpp || pgp || ppg) { if (gpp || pgp || ppg) {
if (redAlliance){ if (redAlliance){
robot.limelight.pipelineSwitch(3); robot.limelight.pipelineSwitch(3);
double turretPID = servo.setTurrPos(turret_redClose); double turretPID = turret_redClose;
robot.turr1.setPower(turretPID); robot.turr1.setPosition(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPosition(-turretPID);
return !servo.turretEqual(turret_redClose); return false;
} else { } else {
robot.limelight.pipelineSwitch(2); robot.limelight.pipelineSwitch(2);
double turretPID = servo.setTurrPos(turret_blueClose); double turretPID = turret_blueClose;
robot.turr1.setPower(turretPID); robot.turr1.setPosition(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPosition(-turretPID);
return !servo.turretEqual(turret_blueClose); return false;
} }
} else { } else {
return true; return true;
@@ -188,8 +188,8 @@ public class AutoClose_V3 extends LinearOpMode {
return true; return true;
} else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) { } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
robot.turr1.setPower(holdTurrPow); robot.turr1.setPosition(holdTurrPow);
robot.turr2.setPower(holdTurrPow); robot.turr2.setPosition(holdTurrPow);
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);
TELE.addLine("shot once"); TELE.addLine("shot once");
TELE.update(); TELE.update();
@@ -411,7 +411,7 @@ public class AutoClose_V3 extends LinearOpMode {
double turrPID; double turrPID;
if (redAlliance){ if (redAlliance){
turrPID = servo.setTurrPos(turret_detectRedClose); turrPID = turret_detectRedClose;
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
@@ -430,7 +430,7 @@ public class AutoClose_V3 extends LinearOpMode {
shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
} else { } else {
turrPID = servo.setTurrPos(turret_detectBlueClose); turrPID = turret_detectBlueClose;
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1); .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
@@ -450,8 +450,8 @@ public class AutoClose_V3 extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1); .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
} }
robot.turr1.setPower(turrPID); robot.turr1.setPosition(turrPID);
robot.turr2.setPower(-turrPID); robot.turr2.setPosition(-turrPID);
robot.hood.setPosition(hoodAuto); robot.hood.setPosition(hoodAuto);
@@ -566,9 +566,9 @@ public class AutoClose_V3 extends LinearOpMode {
} }
} }
double turretPos = servo.getTurrPos() - (bearing / 1300); double turretPos = servo.getTurrPos() - (bearing / 1300);
double turretPID = servo.setTurrPos(turretPos); double turretPID = turretPos;
robot.turr1.setPower(turretPID); robot.turr1.setPosition(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPosition(-turretPID);
} }
public void shootingSequence() { public void shootingSequence() {

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java
View File

@@ -103,18 +103,18 @@ public class AutoFar_V1 extends LinearOpMode {
if (gpp || pgp || ppg) { if (gpp || pgp || ppg) {
if (redAlliance){ if (redAlliance){
robot.limelight.pipelineSwitch(3); robot.limelight.pipelineSwitch(3);
double turretPID = servo.setTurrPos(turret_redFar); robot.turr1.setPosition(turret_redFar);
robot.turr1.setPower(turretPID); robot.turr2.setPosition(-turret_redFar);
robot.turr2.setPower(-turretPID);
return !servo.turretEqual(turret_redFar);
} else { } else {
robot.limelight.pipelineSwitch(2); robot.limelight.pipelineSwitch(2);
double turretPID = servo.setTurrPos(turret_blueFar);
robot.turr1.setPower(turretPID); robot.turr1.setPosition(turret_blueFar);
robot.turr2.setPower(-turretPID); robot.turr2.setPosition(-turret_blueFar);
return !servo.turretEqual(turret_blueFar);
} }
return false;
} else { } else {
return true; return true;
} }
@@ -188,8 +188,8 @@ public class AutoFar_V1 extends LinearOpMode {
return true; return true;
} else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) { } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
robot.turr1.setPower(holdTurrPow); robot.turr1.setPosition(holdTurrPow);
robot.turr2.setPower(holdTurrPow); robot.turr2.setPosition(holdTurrPow);
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);
TELE.addLine("shot once"); TELE.addLine("shot once");
TELE.update(); TELE.update();
@@ -399,13 +399,13 @@ public class AutoFar_V1 extends LinearOpMode {
double turrPID; double turrPID;
if (redAlliance){ if (redAlliance){
turrPID = servo.setTurrPos(turret_detectRedClose); turrPID = turret_detectRedClose;
} else { } else {
turrPID = servo.setTurrPos(turret_detectBlueClose); turrPID = turret_detectBlueClose;
} }
robot.turr1.setPower(turrPID); robot.turr1.setPosition(turrPID);
robot.turr2.setPower(-turrPID); robot.turr2.setPosition(-turrPID);
robot.hood.setPosition(hoodAutoFar); robot.hood.setPosition(hoodAutoFar);
@@ -467,9 +467,9 @@ public class AutoFar_V1 extends LinearOpMode {
} }
} }
double turretPos = servo.getTurrPos() - (bearing / 1300); double turretPos = servo.getTurrPos() - (bearing / 1300);
double turretPID = servo.setTurrPos(turretPos); double turretPID = turretPos;
robot.turr1.setPower(turretPID); robot.turr1.setPosition(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPosition(-turretPID);
} }
public void shootingSequence() { public void shootingSequence() {

839
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java
View File

@@ -1,839 +0,0 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
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.SequentialAction;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@Autonomous(preselectTeleOp = "TeleopV2")
public class Blue_V2 extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
AprilTagWebcam aprilTag;
Flywheel flywheel;
double velo = 0.0;
double targetVelocity = 0.0;
public static double intake1Time = 2.9;
public static double intake2Time = 2.9;
public static double colorDetect = 3.0;
boolean gpp = false;
boolean pgp = false;
boolean ppg = false;
double powPID = 0.0;
int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
boolean spindexPosEqual(double spindexer) {
TELE.addData("Velocity", velo);
TELE.addLine("spindex equal");
TELE.update();
return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 &&
scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01);
}
public Action initShooter(int vel) {
return new Action() {
double initPos = 0.0;
double stamp = 0.0;
double stamp1 = 0.0;
double ticker = 0.0;
double stamp2 = 0.0;
double currentPos = 0.0;
boolean steady = false;
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp2 = getRuntime();
}
targetVelocity = (double) vel;
ticker++;
if (ticker % 16 == 0) {
stamp = getRuntime();
stamp1 = stamp;
}
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.transfer.setPower(1);
TELE.addData("Velocity", velo);
TELE.update();
if (vel < velo && getRuntime() - stamp2 > 3.0 && !steady){
steady = true;
stamp2 = getRuntime();
return true;
} else if (steady && getRuntime() - stamp2 > 1.5){
TELE.addData("Velocity", velo);
TELE.addLine("finished init");
TELE.update();
return false;
} else {
return true;
}
}
};
}
public Action steadyShooter(int vel, boolean last) {
return new Action() {
double stamp = 0.0;
boolean steady = false;
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
steady = flywheel.getSteady();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.transfer.setPower(1);
TELE.addData("Velocity", velo);
TELE.update();
if (last && !steady){
stamp = getRuntime();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
return false;
} else if (steady) {
stamp = getRuntime();
return true;
} else {
return true;
}
}
};
}
public Action Obelisk() {
return new Action() {
double stamp = getRuntime();
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (aprilTag.getTagById(21) != null) {
gpp = true;
} else if (aprilTag.getTagById(22) != null) {
pgp = true;
} else if (aprilTag.getTagById(23) != null) {
ppg = true;
}
aprilTag.update();
TELE.addData("Velocity", velo);
TELE.addData("21", gpp);
TELE.addData("22", pgp);
TELE.addData("23", ppg);
TELE.update();
if (gpp || pgp || ppg){
robot.turr1.setPower(turret_blueClose);
robot.turr2.setPower(1 - turret_blueClose);
return false;
} else {
return true;
}
}
};
}
public Action intakeReject() {
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
double position = 0.0;
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.02;
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (getRuntime() - stamp < 0.3){
return true;
}else {
robot.intake.setPower(0);
return false;
}
}
};
}
public Action spindex (double spindexer, double vel){
return new Action() {
double currentPos = 0.0;
double stamp = 0.0;
double initPos = 0.0;
double stamp1 = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
ticker++;
if (ticker % 8 == 0) {
currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
stamp = getRuntime();
velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
initPos = currentPos;
stamp1 = stamp;
}
if (vel - velo > 500 && ticker > 16) {
powPID = 1.0;
} else if (velo - vel > 500 && ticker > 16){
powPID = 0.0;
} else if (Math.abs(vel - velo) < 100 && ticker > 16){
double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
// --- PROPORTIONAL CORRECTION ---
double error = vel - velo;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
powPID = feed + correction;
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));
}
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.spin1.setPower(spindexer);
robot.spin2.setPower(1-spindexer);
TELE.addData("Velocity", velo);
TELE.addLine("spindex");
TELE.update();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
return !spindexPosEqual(spindexer);
}
};
}
public Action Shoot(double vel) {
return new Action() {
double transferStamp = 0.0;
int ticker = 1;
boolean transferIn = false;
double currentPos = 0.0;
double stamp = 0.0;
double initPos = 0.0;
double stamp1 = 0.0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", velo);
TELE.addLine("shooting");
TELE.update();
if (ticker % 8 == 0) {
currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
stamp = getRuntime();
velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
initPos = currentPos;
stamp1 = stamp;
}
if (vel - velo > 500 && ticker > 16) {
powPID = 1.0;
} else if (velo - vel > 500 && ticker > 16){
powPID = 0.0;
} else if (Math.abs(vel - velo) < 100 && ticker > 16){
double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
// --- PROPORTIONAL CORRECTION ---
double error = vel - velo;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
powPID = feed + correction;
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));
}
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (ticker == 1) {
transferStamp = getRuntime();
ticker++;
}
if (getRuntime() - transferStamp > waitTransfer && !transferIn) {
robot.transferServo.setPosition(transferServo_in);
TELE.addData("Velocity", velo);
TELE.addData("ticker", ticker);
TELE.update();
transferIn = true;
return true;
} else if (getRuntime() - transferStamp > waitTransfer+waitTransferOut && transferIn){
robot.transferServo.setPosition(transferServo_out);
TELE.addData("Velocity", velo);
TELE.addLine("shot once");
TELE.update();
return false;
} else {
return true;
}
}
};
}
public Action intake(double intakeTime) {
return new Action() {
double position = 0.0;
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.02;
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
TELE.addData("Velocity", velo);
TELE.addLine("Intaking");
TELE.update();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
robot.intake.setPower(1);
if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) {
return false;
} else {
return true;
}
}
};
}
public Action ColorDetect() {
return new Action() {
double stamp = 0.0;
int ticker = 0;
double position = 0.0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.02;
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (s1D < 40) {
double green = robot.color1.getNormalizedColors().green;
double red = robot.color1.getNormalizedColors().red;
double blue = robot.color1.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b1 = 2;
} else {
b1 = 1;
}
}
if (s2D < 40) {
double green = robot.color2.getNormalizedColors().green;
double red = robot.color2.getNormalizedColors().red;
double blue = robot.color2.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b2 = 2;
} else {
b2 = 1;
}
}
if (s3D < 30) {
double green = robot.color3.getNormalizedColors().green;
double red = robot.color3.getNormalizedColors().red;
double blue = robot.color3.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b3 = 2;
} else {
b3 = 1;
}
}
TELE.addData("Velocity", velo);
TELE.addLine("Detecting");
TELE.addData("Distance 1", s1D);
TELE.addData("Distance 2", s2D);
TELE.addData("Distance 3", s3D);
TELE.addData("B1", b1);
TELE.addData("B2", b2);
TELE.addData("B3", b3);
TELE.update();
return (b1 + b2 + b3 < 4) && !(getRuntime() - stamp > colorDetect);
}
};
}
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
flywheel = new Flywheel();
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
drive = new MecanumDrive(hardwareMap, new Pose2d(
0, 0, 0
));
aprilTag = new AprilTagWebcam();
TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
.strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a)
.strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b);
TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
.strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a)
.strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b);
TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
aprilTag.init(robot, TELE);
while (opModeInInit()) {
if (gamepad2.dpadUpWasPressed()) {
hoodAuto-= 0.01;
}
if (gamepad2.dpadDownWasPressed()) {
hoodAuto += 0.01;
}
robot.hood.setPosition(hoodAuto);
robot.turr1.setPower(turret_detectBlueClose);
robot.turr2.setPower(1 - turret_detectBlueClose);
robot.transferServo.setPosition(transferServo_out);
robot.spin1.setPower(spindexer_intakePos1);
robot.spin2.setPower(1 - spindexer_intakePos1);
aprilTag.update();
TELE.addData("Velocity", velo);
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()) {
robot.hood.setPosition(hoodAuto);
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
initShooter(AUTO_CLOSE_VEL),
Obelisk()
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
robot.hood.setPosition(hoodAuto);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
pickup1.build(),
intake(intake1Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
shoot1.build(),
ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true),
intakeReject()
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
intake(intake2Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
shoot2.build(),
ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true),
intakeReject()
)
);
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", velo);
TELE.addLine("finished");
TELE.update();
sleep(2000);
}
}
public void shootingSequence() {
TELE.addData("Velocity", velo);
if (gpp) {
if (b1 + b2 + b3 == 4) {
if (b1 == 2 && b2 - b3 == 0) {
sequence1();
TELE.addLine("sequence1");
} else if (b2 == 2 && b1 - b3 == 0) {
sequence3();
TELE.addLine("sequence3");
} else if (b3 == 2 && b1 - b2 == 0) {
sequence6();
TELE.addLine("sequence6");
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (b1 + b2 + b3 >= 5) {
if (b1 == 2) {
sequence1();
TELE.addLine("sequence1");
} else if (b2 == 2) {
sequence3();
TELE.addLine("sequence3");
} else if (b3 == 2) {
sequence6();
TELE.addLine("sequence6");
}
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (pgp) {
if (b1 + b2 + b3 == 4) {
if (b1 == 2 && b2 - b3 == 0) {
sequence3();
TELE.addLine("sequence3");
} else if (b2 == 2 && b1 - b3 == 0) {
sequence1();
TELE.addLine("sequence1");
} else if (b3 == 2 && b1 - b2 == 0) {
sequence4();
TELE.addLine("sequence4");
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (b1 + b2 + b3 >= 5) {
if (b1 == 2) {
sequence3();
TELE.addLine("sequence3");
} else if (b2 == 2) {
sequence1();
TELE.addLine("sequence1");
} else if (b3 == 2) {
sequence4();
TELE.addLine("sequence4");
}
} else {
sequence3();
TELE.addLine("sequence3");
}
} else if (ppg) {
if (b1 + b2 + b3 == 4) {
if (b1 == 2 && b2 - b3 == 0) {
sequence6();
TELE.addLine("sequence6");
} else if (b2 == 2 && b1 - b3 == 0) {
sequence5();
TELE.addLine("sequence5");
} else if (b3 == 2 && b1 - b2 == 0) {
sequence1();
TELE.addLine("sequence1");
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (b1 + b2 + b3 >= 5) {
if (b1 == 2) {
sequence6();
TELE.addLine("sequence6");
} else if (b2 == 2) {
sequence5();
TELE.addLine("sequence5");
} else if (b3 == 2) {
sequence1();
TELE.addLine("sequence1");
}
} else {
sequence6();
TELE.addLine("sequence6");
}
} else {
sequence1();
TELE.addLine("sequence1");
}
TELE.update();
}
public void sequence1() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence2() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence3() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence4() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence5() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence6() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
}

754
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java Normal file
View File

@@ -0,0 +1,754 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
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.bh2c;
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.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.bx2c;
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.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.by2c;
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.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.rh2c;
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.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.rx2c;
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.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.ry2c;
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.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;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turret_blueClose;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turret_redClose;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.AUTO_CLOSE_VEL;
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.SequentialAction;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.FlywheelV2;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import java.util.List;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class ProtoAutoClose_V3 extends LinearOpMode {
public static double intake1Time = 2.7;
public static double intake2Time = 3.0;
public static double colorDetect = 3.0;
public static double holdTurrPow = 0.01; // power to hold turret in place
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
FlywheelV2 flywheel;
Servos servo;
double velo = 0.0;
boolean gpp = false;
boolean pgp = false;
boolean ppg = false;
double powPID = 0.0;
double bearing = 0.0;
int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
public Action initShooter(int vel) {
return new Action() {
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
TELE.addData("Velocity", velo);
TELE.update();
return !flywheel.getSteady();
}
};
}
public Action Obelisk() {
return new Action() {
int id = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
LLResult result = robot.limelight.getLatestResult();
if (result != null && result.isValid()) {
List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
for (LLResultTypes.FiducialResult fiducial : fiducials) {
id = fiducial.getFiducialId();
TELE.addData("ID", id);
TELE.update();
}
}
if (id == 21) {
gpp = true;
} else if (id == 22) {
pgp = true;
} else if (id == 23) {
ppg = true;
}
TELE.addData("Velocity", velo);
TELE.addData("21", gpp);
TELE.addData("22", pgp);
TELE.addData("23", ppg);
TELE.update();
if (gpp || pgp || ppg) {
if (redAlliance) {
robot.limelight.pipelineSwitch(3);
robot.turr1.setPosition(turret_redClose);
robot.turr2.setPosition(1 - turret_redClose);
return false;
} else {
robot.limelight.pipelineSwitch(2);
double turretPID = turret_blueClose;
robot.turr1.setPosition(turretPID);
robot.turr2.setPosition(1 - turretPID);
return false;
}
} else {
return true;
}
}
};
}
public Action spindex(double spindexer, int vel) {
return new Action() {
double spinPID = 0.0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
spinPID = servo.setSpinPos(spindexer);
robot.spin1.setPower(spinPID);
robot.spin2.setPower(-spinPID);
TELE.addData("Velocity", velo);
TELE.addLine("spindex");
TELE.update();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (servo.spinEqual(spindexer)) {
robot.spin1.setPower(0);
robot.spin2.setPower(0);
return false;
} else {
return true;
}
}
};
}
public Action Shoot(int vel) {
return new Action() {
int ticker = 1;
double initPos = 0.0;
double finalPos = 0.0;
boolean zeroNeeded = false;
boolean zeroPassed = false;
double currentPos = 0.0;
double prevPos = 0.0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", velo);
TELE.addLine("shooting");
TELE.update();
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (ticker == 1) {
robot.transferServo.setPosition(transferServo_in);
initPos = servo.getSpinPos();
finalPos = initPos + 0.6;
if (finalPos > 1.0) {
finalPos = finalPos - 1;
zeroNeeded = true;
} else if (finalPos > 0.95) {
finalPos = 0.0;
zeroNeeded = true;
}
currentPos = initPos;
}
ticker++;
if (ticker > 16) {
robot.spin1.setPower(0.08);
robot.spin2.setPower(-0.08);
}
prevPos = currentPos;
currentPos = servo.getSpinPos();
if (zeroNeeded) {
if (currentPos - prevPos < -0.5) {
zeroPassed = true;
}
if (zeroPassed) {
if (currentPos > finalPos) {
robot.spin1.setPower(0);
robot.spin2.setPower(0);
return false;
} else {
return true;
}
} else {
return true;
}
} else {
if (currentPos > finalPos) {
robot.spin1.setPower(0);
robot.spin2.setPower(0);
return false;
} else {
return true;
}
}
}
};
}
public Action intake(double intakeTime) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
double spinCurrentPos = 0.0;
double spinInitPos = 0.0;
boolean reverse = false;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (ticker % 12 < 3) {
robot.spin1.setPower(-1);
robot.spin2.setPower(1);
} else if (reverse) {
robot.spin1.setPower(1);
robot.spin2.setPower(-1);
} else {
robot.spin1.setPower(-0.15);
robot.spin2.setPower(0.15);
}
robot.intake.setPower(1);
TELE.addData("Reverse?", reverse);
TELE.update();
if (getRuntime() - stamp > intakeTime) {
if (reverse) {
robot.spin1.setPower(-1);
robot.spin2.setPower(1);
} else {
robot.spin1.setPower(1);
robot.spin2.setPower(-1);
}
return false;
} else {
if (ticker % 4 == 0) {
spinCurrentPos = servo.getSpinPos();
reverse = Math.abs(spinCurrentPos - spinInitPos) < 0.02;
spinInitPos = spinCurrentPos;
}
return true;
}
}
};
}
public Action ColorDetect(int vel) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (s1D < 43) {
double green = robot.color1.getNormalizedColors().green;
double red = robot.color1.getNormalizedColors().red;
double blue = robot.color1.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b1 = 2;
} else {
b1 = 1;
}
}
if (s2D < 60) {
double green = robot.color2.getNormalizedColors().green;
double red = robot.color2.getNormalizedColors().red;
double blue = robot.color2.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b2 = 2;
} else {
b2 = 1;
}
}
if (s3D < 33) {
double green = robot.color3.getNormalizedColors().green;
double red = robot.color3.getNormalizedColors().red;
double blue = robot.color3.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b3 = 2;
} else {
b3 = 1;
}
}
TELE.addData("Velocity", velo);
TELE.addLine("Detecting");
TELE.addData("Distance 1", s1D);
TELE.addData("Distance 2", s2D);
TELE.addData("Distance 3", s3D);
TELE.addData("B1", b1);
TELE.addData("B2", b2);
TELE.addData("B3", b3);
TELE.update();
return (b1 + b2 + b3 < 4) && !(getRuntime() - stamp > colorDetect);
}
};
}
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
flywheel = new FlywheelV2();
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
drive = new MecanumDrive(hardwareMap, new Pose2d(
0, 0, 0
));
servo = new Servos(hardwareMap);
robot.limelight.pipelineSwitch(1);
robot.limelight.start();
TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
.strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a)
.strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b);
TrajectoryActionBuilder lever = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b))
.strafeToLinearHeading(new Vector2d(bx2c, by2c), bh2c);
TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(bx2c, by2c, bh2c))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
.strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a)
.strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b);
TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
TrajectoryActionBuilder pickup3 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
.strafeToLinearHeading(new Vector2d(bx4a, by4a), bh4a)
.strafeToLinearHeading(new Vector2d(bx4b, by4b), bh4b);
TrajectoryActionBuilder shoot3 = drive.actionBuilder(new Pose2d(bx4b, by4b, bh4b))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
while (opModeInInit()) {
if (gamepad2.dpadUpWasPressed()) {
hoodAuto -= 0.01;
}
if (gamepad2.dpadDownWasPressed()) {
hoodAuto += 0.01;
}
if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance;
}
double turretPID;
if (redAlliance) {
turretPID = turret_redClose;
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
pickup1 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1))
.strafeToLinearHeading(new Vector2d(rx2a, ry2a), rh2a)
.strafeToLinearHeading(new Vector2d(rx2b, ry2b), rh2b);
lever = drive.actionBuilder(new Pose2d(rx2b, ry2b, rh2b))
.strafeToLinearHeading(new Vector2d(rx2c, ry2c), rh2c);
shoot1 = drive.actionBuilder(new Pose2d(rx2c, ry2c, rh2c))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
pickup2 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1))
.strafeToLinearHeading(new Vector2d(rx3a, ry3a), rh3a)
.strafeToLinearHeading(new Vector2d(rx3b, ry3b), rh3b);
shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
pickup3 = drive.actionBuilder(new Pose2d(rx1,ry1, rh1))
.strafeToLinearHeading(new Vector2d(rx4a, ry4a), rh4a)
.strafeToLinearHeading(new Vector2d(rx4b, ry4b), rh4b);
shoot3 = drive.actionBuilder(new Pose2d(bx4b, by4b, bh4b))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
} else {
turretPID = turret_blueClose;
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
.strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a)
.strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b);
shoot1 = drive.actionBuilder(new Pose2d(bx2c, by2c, bh2c))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
.strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a)
.strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b);
shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
}
robot.turr1.setPosition(turretPID);
robot.turr2.setPosition(1 - turretPID);
robot.hood.setPosition(hoodAuto);
robot.transferServo.setPosition(transferServo_out);
TELE.addData("Red?", redAlliance);
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()) {
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
initShooter(AUTO_CLOSE_VEL)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
robot.transfer.setPower(1);
shootingSequence();
robot.transfer.setPower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
pickup1.build(),
intake(intake1Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new SequentialAction(
lever.build(),
shoot1.build()
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
robot.transfer.setPower(1);
shootingSequence();
robot.transfer.setPower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
intake(intake2Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
shoot2.build()
)
);
robot.transfer.setPower(1);
shootingSequence();
robot.transfer.setPower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
pickup3.build(),
intake(intake2Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
shoot3.build()
)
);
robot.transfer.setPower(1);
shootingSequence();
robot.transfer.setPower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", velo);
TELE.addLine("finished");
TELE.update();
sleep(2000);
}
}
//TODO: adjust this according to Teleop numbers
public void detectTag() {
LLResult result = robot.limelight.getLatestResult();
if (result != null) {
if (result.isValid()) {
bearing = result.getTx();
}
}
double turretPos = robot.turr1Pos.getCurrentPosition() - (bearing / 1300);
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1 - turretPos);
}
public void shootingSequence() {
TELE.addLine("Shooting");
TELE.update();
Actions.runBlocking(Shoot(AUTO_CLOSE_VEL));
}
public void sequence1() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence2() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence3() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence4() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence5() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence6() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
}

771
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java
View File

@@ -1,771 +0,0 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
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.ParallelAction;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ftc.Actions;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
@Config
@Autonomous(preselectTeleOp = "TeleopV2")
public class Red_V2 extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
AprilTagWebcam aprilTag;
Flywheel flywheel;
Servos servo;
double velo = 0.0;
public static double intake1Time = 2.9;
public static double intake2Time = 2.9;
public static double colorDetect = 3.0;
boolean gpp = false;
boolean pgp = false;
boolean ppg = false;
double powPID = 0.0;
double bearing = 0.0;
int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
public Action initShooter(int vel) {
return new Action() {
double stamp = 0.0;
double stamp1 = 0.0;
double ticker = 0.0;
double stamp2 = 0.0;
boolean steady = false;
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp2 = getRuntime();
}
ticker++;
if (ticker % 16 == 0) {
stamp = getRuntime();
stamp1 = stamp;
}
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.transfer.setPower(1);
TELE.addData("Velocity", velo);
TELE.update();
if (vel < velo && getRuntime() - stamp2 > 3.0 && !steady){
steady = true;
stamp2 = getRuntime();
return true;
} else if (steady && getRuntime() - stamp2 > 1.5){
TELE.addData("Velocity", velo);
TELE.addLine("finished init");
TELE.update();
return false;
} else {
return true;
}
}
};
}
public Action steadyShooter(int vel, boolean last) {
return new Action() {
double stamp = 0.0;
boolean steady = false;
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
steady = flywheel.getSteady();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.transfer.setPower(1);
TELE.addData("Velocity", velo);
TELE.update();
detectTag();
if (last && !steady){
stamp = getRuntime();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
return false;
} else if (steady) {
stamp = getRuntime();
return true;
} else {
return true;
}
}
};
}
public Action Obelisk() {
return new Action() {
double stamp = getRuntime();
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (aprilTag.getTagById(21) != null) {
gpp = true;
} else if (aprilTag.getTagById(22) != null) {
pgp = true;
} else if (aprilTag.getTagById(23) != null) {
ppg = true;
}
aprilTag.update();
TELE.addData("Velocity", velo);
TELE.addData("21", gpp);
TELE.addData("22", pgp);
TELE.addData("23", ppg);
TELE.update();
if (gpp || pgp || ppg){
double turretPID = servo.setTurrPos(turret_redClose);
robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID);
return !servo.turretEqual(turret_redClose);
} else {
return true;
}
}
};
}
public Action spindex (double spindexer, int vel){
return new Action() {
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.spin1.setPower(spindexer);
robot.spin2.setPower(1-spindexer);
TELE.addData("Velocity", velo);
TELE.addLine("spindex");
TELE.update();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
return !servo.spinEqual(spindexer);
}
};
}
public Action Shoot(int vel) {
return new Action() {
double transferStamp = 0.0;
int ticker = 1;
boolean transferIn = false;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", velo);
TELE.addLine("shooting");
TELE.update();
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
drive.updatePoseEstimate();
detectTag();
teleStart = drive.localizer.getPose();
if (ticker == 1) {
transferStamp = getRuntime();
ticker++;
}
if (getRuntime() - transferStamp > waitTransfer && !transferIn) {
robot.transferServo.setPosition(transferServo_in);
TELE.addData("Velocity", velo);
TELE.addData("ticker", ticker);
TELE.update();
transferIn = true;
return true;
} else if (getRuntime() - transferStamp > waitTransfer+waitTransferOut && transferIn){
robot.transferServo.setPosition(transferServo_out);
TELE.addData("Velocity", velo);
TELE.addLine("shot once");
TELE.update();
return false;
} else {
return true;
}
}
};
}
public Action intake(double intakeTime) {
return new Action() {
double position = 0.0;
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.02;
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
TELE.addData("Velocity", velo);
TELE.addLine("Intaking");
TELE.update();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
robot.intake.setPower(1);
if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) {
return false;
} else {
return true;
}
}
};
}
public Action intakeReject() {
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (getRuntime() - stamp < 0.3){
return true;
}else {
robot.intake.setPower(0);
return false;
}
}
};
}
public Action ColorDetect() {
return new Action() {
double stamp = 0.0;
int ticker = 0;
double position = 0.0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.02;
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (s1D < 40) {
double green = robot.color1.getNormalizedColors().green;
double red = robot.color1.getNormalizedColors().red;
double blue = robot.color1.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b1 = 2;
} else {
b1 = 1;
}
}
if (s2D < 40) {
double green = robot.color2.getNormalizedColors().green;
double red = robot.color2.getNormalizedColors().red;
double blue = robot.color2.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b2 = 2;
} else {
b2 = 1;
}
}
if (s3D < 30) {
double green = robot.color3.getNormalizedColors().green;
double red = robot.color3.getNormalizedColors().red;
double blue = robot.color3.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b3 = 2;
} else {
b3 = 1;
}
}
TELE.addData("Velocity", velo);
TELE.addLine("Detecting");
TELE.addData("Distance 1", s1D);
TELE.addData("Distance 2", s2D);
TELE.addData("Distance 3", s3D);
TELE.addData("B1", b1);
TELE.addData("B2", b2);
TELE.addData("B3", b3);
TELE.update();
return (b1 + b2 + b3 < 4) && !(getRuntime() - stamp > colorDetect);
}
};
}
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
flywheel = new Flywheel();
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
drive = new MecanumDrive(hardwareMap, new Pose2d(
0, 0, 0
));
aprilTag = new AprilTagWebcam();
TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1))
.strafeToLinearHeading(new Vector2d(rx2a, ry2a), rh2a)
.strafeToLinearHeading(new Vector2d(rx2b, ry2b), rh2b);
TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(rx2b, ry2b, rh2b))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1))
.strafeToLinearHeading(new Vector2d(rx3a, ry3a), rh3a)
.strafeToLinearHeading(new Vector2d(rx3b, ry3b), rh3b);
TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
aprilTag.init(robot, TELE);
while (opModeInInit()) {
if (gamepad2.dpadUpWasPressed()) {
hoodAuto-= 0.01;
}
if (gamepad2.dpadDownWasPressed()) {
hoodAuto += 0.01;
}
robot.hood.setPosition(hoodAuto);
robot.transferServo.setPosition(transferServo_out);
robot.spin1.setPower(spindexer_intakePos1);
robot.spin2.setPower(1 - spindexer_intakePos1);
aprilTag.update();
TELE.addData("Velocity", velo);
TELE.addData("Turret Pos", servo.getTurrPos());
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()) {
robot.hood.setPosition(hoodAuto);
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
initShooter(AUTO_CLOSE_VEL),
Obelisk()
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
robot.hood.setPosition(hoodAuto);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
pickup1.build(),
intake(intake1Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
shoot1.build(),
ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true),
intakeReject()
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
intake(intake2Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
shoot2.build(),
ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true),
intakeReject()
)
);
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", velo);
TELE.addLine("finished");
TELE.update();
sleep(2000);
}
}
public void detectTag(){
AprilTagDetection d20 = aprilTag.getTagById(20);
AprilTagDetection d24 = aprilTag.getTagById(24);
if (d20 != null) {
bearing = d20.ftcPose.bearing;
TELE.addData("Bear", bearing);
}
if (d24 != null) {
bearing = d24.ftcPose.bearing;
TELE.addData("Bear", bearing);
}
double turretPos = servo.getTurrPos() - (bearing / 1300);
double turretPID = servo.setTurrPos(turretPos);
robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID);
}
public void shootingSequence() {
TELE.addData("Velocity", velo);
if (gpp) {
if (b1 + b2 + b3 == 4) {
if (b1 == 2 && b2 - b3 == 0) {
sequence1();
TELE.addLine("sequence1");
} else if (b2 == 2 && b1 - b3 == 0) {
sequence3();
TELE.addLine("sequence3");
} else if (b3 == 2 && b1 - b2 == 0) {
sequence6();
TELE.addLine("sequence6");
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (b1 + b2 + b3 >= 5) {
if (b1 == 2) {
sequence1();
TELE.addLine("sequence1");
} else if (b2 == 2) {
sequence3();
TELE.addLine("sequence3");
} else if (b3 == 2) {
sequence6();
TELE.addLine("sequence6");
}
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (pgp) {
if (b1 + b2 + b3 == 4) {
if (b1 == 2 && b2 - b3 == 0) {
sequence3();
TELE.addLine("sequence3");
} else if (b2 == 2 && b1 - b3 == 0) {
sequence1();
TELE.addLine("sequence1");
} else if (b3 == 2 && b1 - b2 == 0) {
sequence4();
TELE.addLine("sequence4");
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (b1 + b2 + b3 >= 5) {
if (b1 == 2) {
sequence3();
TELE.addLine("sequence3");
} else if (b2 == 2) {
sequence1();
TELE.addLine("sequence1");
} else if (b3 == 2) {
sequence4();
TELE.addLine("sequence4");
}
} else {
sequence3();
TELE.addLine("sequence3");
}
} else if (ppg) {
if (b1 + b2 + b3 == 4) {
if (b1 == 2 && b2 - b3 == 0) {
sequence6();
TELE.addLine("sequence6");
} else if (b2 == 2 && b1 - b3 == 0) {
sequence5();
TELE.addLine("sequence5");
} else if (b3 == 2 && b1 - b2 == 0) {
sequence1();
TELE.addLine("sequence1");
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (b1 + b2 + b3 >= 5) {
if (b1 == 2) {
sequence6();
TELE.addLine("sequence6");
} else if (b2 == 2) {
sequence5();
TELE.addLine("sequence5");
} else if (b3 == 2) {
sequence1();
TELE.addLine("sequence1");
}
} else {
sequence6();
TELE.addLine("sequence6");
}
} else {
sequence1();
TELE.addLine("sequence1");
}
TELE.update();
}
public void sequence1() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence2() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence3() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence4() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence5() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence6() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/blank.java
View File

@@ -1,4 +0,0 @@
package org.firstinspires.ftc.teamcode.autonomous;
public class blank {
}

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

@@ -17,15 +17,25 @@ public class Poses {
public static double rx1 = 45, ry1 = -7, rh1 = 0; public static double rx1 = 45, ry1 = -7, rh1 = 0;
public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140); public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140);
public static double rx2b = 31, ry2b = 32, rh2b = Math.toRadians(140); public static double rx2b = 31, ry2b = 32, rh2b = Math.toRadians(140);
public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140);
public static double rx3a = 58, ry3a = 42, rh3a = Math.toRadians(140); public static double rx3a = 58, ry3a = 42, rh3a = Math.toRadians(140);
public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140); public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140);
public static double rx4a = 71, ry4a = 60, rh4a = Math.toRadians(140);
public static double rx4b = 79, ry4b = 79, rh4b = Math.toRadians(140);
public static double bx1 = 45, by1 = 6, bh1 = 0; public static double bx1 = 45, by1 = 6, bh1 = 0;
public static double bx2a = 53, by2a = -7, bh2a = Math.toRadians(-140); public static double bx2a = 53, by2a = -7, bh2a = Math.toRadians(-140);
public static double bx2b = 23, by2b = -39, bh2b = Math.toRadians(-140); public static double bx2b = 23, by2b = -39, bh2b = Math.toRadians(-140);
public static double bx2c = 40, by2c = -50, bh2c = Math.toRadians(-140);
public static double bx3a = 56, by3a = -34, bh3a = Math.toRadians(-140); public static double bx3a = 56, by3a = -34, bh3a = Math.toRadians(-140);
public static double bx3b = 34, by3b = -58, bh3b = Math.toRadians(-140); public static double bx3b = 34, by3b = -58, bh3b = Math.toRadians(-140);
public static double bx4a = 69, by4a = -60, bh4a = Math.toRadians(-140);
public static double bx4b = 75, by4b = -79, bh4b = Math.toRadians(-140);
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1); public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1);

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

@@ -40,6 +40,6 @@ public class ServoPositions {
public static double turret_detectRedClose = 0.2; public static double turret_detectRedClose = 0.2;
public static double turret_detectBlueClose = 0.6; public static double turret_detectBlueClose = 0.6;
public static double turrDefault = 0; public static double turrDefault = 0.4;
} }

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

@@ -5,7 +5,9 @@ import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.canvas.Canvas; import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.*; import com.acmerobotics.roadrunner.AccelConstraint;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.Actions;
import com.acmerobotics.roadrunner.AngularVelConstraint; import com.acmerobotics.roadrunner.AngularVelConstraint;
import com.acmerobotics.roadrunner.DualNum; import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.HolonomicController; import com.acmerobotics.roadrunner.HolonomicController;
@@ -14,12 +16,20 @@ import com.acmerobotics.roadrunner.MinVelConstraint;
import com.acmerobotics.roadrunner.MotorFeedforward; import com.acmerobotics.roadrunner.MotorFeedforward;
import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Pose2dDual; import com.acmerobotics.roadrunner.Pose2dDual;
import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.acmerobotics.roadrunner.PoseVelocity2dDual;
import com.acmerobotics.roadrunner.ProfileAccelConstraint; import com.acmerobotics.roadrunner.ProfileAccelConstraint;
import com.acmerobotics.roadrunner.ProfileParams;
import com.acmerobotics.roadrunner.Rotation2d;
import com.acmerobotics.roadrunner.Time; import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.TimeTrajectory; import com.acmerobotics.roadrunner.TimeTrajectory;
import com.acmerobotics.roadrunner.TimeTurn; import com.acmerobotics.roadrunner.TimeTurn;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TrajectoryBuilderParams;
import com.acmerobotics.roadrunner.TurnConstraints; import com.acmerobotics.roadrunner.TurnConstraints;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.VelConstraint; import com.acmerobotics.roadrunner.VelConstraint;
import com.acmerobotics.roadrunner.ftc.DownsampledWriter; import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
import com.acmerobotics.roadrunner.ftc.Encoder; import com.acmerobotics.roadrunner.ftc.Encoder;
@@ -46,56 +56,15 @@ import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumCommandMessage;
import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumLocalizerInputsMessage; import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumLocalizerInputsMessage;
import org.firstinspires.ftc.teamcode.libs.RR.messages.PoseMessage; import org.firstinspires.ftc.teamcode.libs.RR.messages.PoseMessage;
import java.lang.Math;
import java.util.Arrays; import java.util.Arrays;
import java.util.LinkedList; import java.util.LinkedList;
import java.util.List; import java.util.List;
@Config @Config
public final class MecanumDrive { public final class MecanumDrive {
public static class Params {
// IMU orientation
// TODO: fill in these values based on
// see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
RevHubOrientationOnRobot.LogoFacingDirection.RIGHT;
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD;
// drive model parameters
public double inPerTick = 0.001978956;
public double lateralInPerTick = 0.0013863732202094405;
public double trackWidthTicks = 6488.883015684446;
// feedforward parameters (in tick units)
public double kS = 1.2147826978829488;
public double kV = 0.00032;
public double kA = 0.000046;
// path profile parameters (in inches)
public double maxWheelVel = 180;
public double minProfileAccel = -40;
public double maxProfileAccel = 180;
// turn profile parameters (in radians)
public double maxAngVel = 4* Math.PI; // shared with path
public double maxAngAccel = 4* Math.PI;
// path controller gains
public double axialGain = 4;
public double lateralGain = 4;
public double headingGain = 4; // shared with turn
public double axialVelGain = 0.0;
public double lateralVelGain = 0.0;
public double headingVelGain = 0.0; // shared with turn
}
public static Params PARAMS = new Params(); public static Params PARAMS = new Params();
public final MecanumKinematics kinematics = new MecanumKinematics( public final MecanumKinematics kinematics = new MecanumKinematics(
PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick); PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick);
public final TurnConstraints defaultTurnConstraints = new TurnConstraints( public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel); PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
public final VelConstraint defaultVelConstraint = public final VelConstraint defaultVelConstraint =
@@ -105,117 +74,15 @@ public final class MecanumDrive {
)); ));
public final AccelConstraint defaultAccelConstraint = public final AccelConstraint defaultAccelConstraint =
new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel); new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);
public final DcMotorEx leftFront, leftBack, rightBack, rightFront; public final DcMotorEx leftFront, leftBack, rightBack, rightFront;
public final VoltageSensor voltageSensor; public final VoltageSensor voltageSensor;
public final LazyImu lazyImu; public final LazyImu lazyImu;
public final Localizer localizer; public final Localizer localizer;
private final LinkedList<Pose2d> poseHistory = new LinkedList<>(); private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000); private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000); private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000); private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000); private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000);
public class DriveLocalizer implements Localizer {
public final Encoder leftFront, leftBack, rightBack, rightFront;
public final IMU imu;
private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos;
private Rotation2d lastHeading;
private boolean initialized;
private Pose2d pose;
public DriveLocalizer(Pose2d pose) {
leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack));
rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));
imu = lazyImu.get();
// TODO: reverse encoders if needed
// leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
this.pose = pose;
}
@Override
public void setPose(Pose2d pose) {
this.pose = pose;
}
@Override
public Pose2d getPose() {
return pose;
}
@Override
public PoseVelocity2d update() {
PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity();
PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage(
leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles));
Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS));
if (!initialized) {
initialized = true;
lastLeftFrontPos = leftFrontPosVel.position;
lastLeftBackPos = leftBackPosVel.position;
lastRightBackPos = rightBackPosVel.position;
lastRightFrontPos = rightFrontPosVel.position;
lastHeading = heading;
return new PoseVelocity2d(new Vector2d(0.0, 0.0), 0.0);
}
double headingDelta = heading.minus(lastHeading);
Twist2dDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
new DualNum<Time>(new double[]{
(leftFrontPosVel.position - lastLeftFrontPos),
leftFrontPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(leftBackPosVel.position - lastLeftBackPos),
leftBackPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(rightBackPosVel.position - lastRightBackPos),
rightBackPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(rightFrontPosVel.position - lastRightFrontPos),
rightFrontPosVel.velocity,
}).times(PARAMS.inPerTick)
));
lastLeftFrontPos = leftFrontPosVel.position;
lastLeftBackPos = leftBackPosVel.position;
lastRightBackPos = rightBackPosVel.position;
lastRightFrontPos = rightFrontPosVel.position;
lastHeading = heading;
pose = pose.plus(new Twist2d(
twist.line.value(),
headingDelta
));
return twist.velocity().value();
}
}
public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
LynxFirmware.throwIfModulesAreOutdated(hardwareMap); LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
@@ -268,11 +135,190 @@ public final class MecanumDrive {
rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag); rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag);
} }
public PoseVelocity2d updatePoseEstimate() {
PoseVelocity2d vel = localizer.update();
poseHistory.add(localizer.getPose());
while (poseHistory.size() > 100) {
poseHistory.removeFirst();
}
estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
return vel;
}
private void drawPoseHistory(Canvas c) {
double[] xPoints = new double[poseHistory.size()];
double[] yPoints = new double[poseHistory.size()];
int i = 0;
for (Pose2d t : poseHistory) {
xPoints[i] = t.position.x;
yPoints[i] = t.position.y;
i++;
}
c.setStrokeWidth(1);
c.setStroke("#3F51B5");
c.strokePolyline(xPoints, yPoints);
}
public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
return new TrajectoryActionBuilder(
TurnAction::new,
FollowTrajectoryAction::new,
new TrajectoryBuilderParams(
1e-6,
new ProfileParams(
0.25, 0.1, 1e-2
)
),
beginPose, 0.0,
defaultTurnConstraints,
defaultVelConstraint, defaultAccelConstraint
);
}
public static class Params {
// IMU orientation
// TODO: fill in these values based on
// see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
RevHubOrientationOnRobot.LogoFacingDirection.RIGHT;
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD;
// drive model parameters
public double inPerTick = 0.001978956;
public double lateralInPerTick = 0.0013863732202094405;
public double trackWidthTicks = 6488.883015684446;
// feedforward parameters (in tick units)
public double kS = 1.2147826978829488;
public double kV = 0.00032;
public double kA = 0.000046;
// path profile parameters (in inches)
public double maxWheelVel = 180;
public double minProfileAccel = -40;
public double maxProfileAccel = 180;
// turn profile parameters (in radians)
public double maxAngVel = 4 * Math.PI; // shared with path
public double maxAngAccel = 4 * Math.PI;
// path controller gains
public double axialGain = 4;
public double lateralGain = 4;
public double headingGain = 4; // shared with turn
public double axialVelGain = 0.0;
public double lateralVelGain = 0.0;
public double headingVelGain = 0.0; // shared with turn
}
public class DriveLocalizer implements Localizer {
public final Encoder leftFront, leftBack, rightBack, rightFront;
public final IMU imu;
private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos;
private Rotation2d lastHeading;
private boolean initialized;
private Pose2d pose;
public DriveLocalizer(Pose2d pose) {
leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack));
rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));
imu = lazyImu.get();
// TODO: reverse encoders if needed
// leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
this.pose = pose;
}
@Override
public Pose2d getPose() {
return pose;
}
@Override
public void setPose(Pose2d pose) {
this.pose = pose;
}
@Override
public PoseVelocity2d update() {
PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity();
PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage(
leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles));
Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS));
if (!initialized) {
initialized = true;
lastLeftFrontPos = leftFrontPosVel.position;
lastLeftBackPos = leftBackPosVel.position;
lastRightBackPos = rightBackPosVel.position;
lastRightFrontPos = rightFrontPosVel.position;
lastHeading = heading;
return new PoseVelocity2d(new Vector2d(0.0, 0.0), 0.0);
}
double headingDelta = heading.minus(lastHeading);
Twist2dDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
new DualNum<Time>(new double[]{
(leftFrontPosVel.position - lastLeftFrontPos),
leftFrontPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(leftBackPosVel.position - lastLeftBackPos),
leftBackPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(rightBackPosVel.position - lastRightBackPos),
rightBackPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(rightFrontPosVel.position - lastRightFrontPos),
rightFrontPosVel.velocity,
}).times(PARAMS.inPerTick)
));
lastLeftFrontPos = leftFrontPosVel.position;
lastLeftBackPos = leftBackPosVel.position;
lastRightBackPos = rightBackPosVel.position;
lastRightFrontPos = rightFrontPosVel.position;
lastHeading = heading;
pose = pose.plus(new Twist2d(
twist.line.value(),
headingDelta
));
return twist.velocity().value();
}
}
public final class FollowTrajectoryAction implements Action { public final class FollowTrajectoryAction implements Action {
public final TimeTrajectory timeTrajectory; public final TimeTrajectory timeTrajectory;
private double beginTs = -1;
private final double[] xPoints, yPoints; private final double[] xPoints, yPoints;
private double beginTs = -1;
public FollowTrajectoryAction(TimeTrajectory t) { public FollowTrajectoryAction(TimeTrajectory t) {
timeTrajectory = t; timeTrajectory = t;
@@ -450,51 +496,4 @@ public final class MecanumDrive {
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2); c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
} }
} }
public PoseVelocity2d updatePoseEstimate() {
PoseVelocity2d vel = localizer.update();
poseHistory.add(localizer.getPose());
while (poseHistory.size() > 100) {
poseHistory.removeFirst();
}
estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
return vel;
}
private void drawPoseHistory(Canvas c) {
double[] xPoints = new double[poseHistory.size()];
double[] yPoints = new double[poseHistory.size()];
int i = 0;
for (Pose2d t : poseHistory) {
xPoints[i] = t.position.x;
yPoints[i] = t.position.y;
i++;
}
c.setStrokeWidth(1);
c.setStroke("#3F51B5");
c.strokePolyline(xPoints, yPoints);
}
public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
return new TrajectoryActionBuilder(
TurnAction::new,
FollowTrajectoryAction::new,
new TrajectoryBuilderParams(
1e-6,
new ProfileParams(
0.25, 0.1, 1e-2
)
),
beginPose, 0.0,
defaultTurnConstraints,
defaultVelConstraint, defaultAccelConstraint
);
}
} }

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
View File

@@ -158,10 +158,6 @@ public class TeleopV2 extends LinearOpMode {
robot.frontRight.setPower(frontRightPower); robot.frontRight.setPower(frontRightPower);
robot.backRight.setPower(backRightPower); robot.backRight.setPower(backRightPower);
// PID SERVOS
turretPID = servo.setTurrPos(turretPos);
robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID);
//TODO: make sure changing position works throughout opmode //TODO: make sure changing position works throughout opmode
if (!servo.spinEqual(spindexPos)){ if (!servo.spinEqual(spindexPos)){

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

@@ -117,8 +117,8 @@ public class TeleopV3 extends LinearOpMode {
if (distance < 30) { if (distance < 30) {
return 2750; return 2750;
} else if (distance > 100) { } else if (distance > 100) {
if (distance > 160) { if (distance > 120) {
return 4200; return 4500;
} }
return 3700; return 3700;
} else { } else {
@@ -146,6 +146,8 @@ public class TeleopV3 extends LinearOpMode {
tController.setTolerance(0.001); tController.setTolerance(0.001);
if (redAlliance) { if (redAlliance) {
robot.limelight.pipelineSwitch(3); robot.limelight.pipelineSwitch(3);
} else { } else {
@@ -156,6 +158,9 @@ public class TeleopV3 extends LinearOpMode {
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
robot.transferServo.setPosition(transferServo_out);
while (opModeIsActive()) { while (opModeIsActive()) {
//DRIVETRAIN: //DRIVETRAIN:
@@ -228,6 +233,8 @@ public class TeleopV3 extends LinearOpMode {
} }
if (gamepad1.right_bumper) { if (gamepad1.right_bumper) {
robot.transferServo.setPosition(transferServo_out);
intakeTicker++; intakeTicker++;
if (intakeTicker % 4 == 0) { if (intakeTicker % 4 == 0) {
@@ -381,12 +388,25 @@ public class TeleopV3 extends LinearOpMode {
desiredTurretAngle += manualOffset; desiredTurretAngle += manualOffset;
offset = (desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset))); offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset));
if (offset > 135) { if (offset > 135) {
offset -= 360; offset -= 360;
} }
double pos = turrDefault;
TELE.addData("offset", offset);
pos -= offset * ((double) 1 / 360);
if (pos < 0.13) {
pos = 0.13;
} else if (pos > 0.83) {
pos = 0.83;
}
//SHOOTER: //SHOOTER:
double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
@@ -419,19 +439,7 @@ public class TeleopV3 extends LinearOpMode {
} }
//TODO: test the camera teleop code //TODO: test the camera teleop code
double pos = turrDefault + (error / 8); // adds the overall error to the default
TELE.addData("offset", offset);
pos -= offset * (4.04 / 360);
TELE.addData("posS1", pos);
if (pos < -1.5) {
pos = -1.5;
} else if (pos > 1.8) {
pos = 1.8;
}
TELE.addData("posS2", pos); TELE.addData("posS2", pos);
@@ -467,34 +475,27 @@ public class TeleopV3 extends LinearOpMode {
turretPos = pos; turretPos = pos;
} }
if (gamepad2.dpadRightWasPressed()) {
manualOffset += 2;
} else if (gamepad2.dpadLeftWasPressed()) {
manualOffset -= 2;
}
TELE.addData("posS3", turretPos); TELE.addData("posS3", turretPos);
// PID SERVOS if (manualTurret) {
double turrettopos = (double) ((double) robot.turr1Pos.getCurrentPosition() / 1024.0) * ((double) 44.0 / (double) 77.0); pos = turrDefault + (manualOffset / 100);
TELE.addData("currentTpos", turrettopos);
double tPid;
if (!fixedTurret) {
tPid = tController.calculate(turrettopos, turretPos);
} else {
tPid = tController.calculate(turrettopos, (manualOffset/90));
} }
TELE.addData("tPID", tPid); if (!overrideTurr) {
turretPos = pos;
}
robot.turr1.setPower(tPid); if (gamepad2.dpad_right || gamepad1.dpad_right) {
robot.turr2.setPower(-tPid); manualOffset -= 2;
} else if (gamepad2.dpad_left || gamepad1.dpad_left) {
manualOffset += 2;
}
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1-pos);
//HOOD: //HOOD:
@@ -504,11 +505,11 @@ public class TeleopV3 extends LinearOpMode {
robot.hood.setPosition(hoodDefaultPos + hoodOffset); robot.hood.setPosition(hoodDefaultPos + hoodOffset);
} }
if (gamepad2.dpadUpWasPressed()) { if (gamepad2.dpadUpWasPressed() || gamepad1.dpadUpWasPressed()) {
hoodOffset -= 0.03; hoodOffset -= 0.03;
autoHoodOffset -= 0.02; autoHoodOffset -= 0.02;
} else if (gamepad2.dpadDownWasPressed()) { } else if (gamepad2.dpadDownWasPressed() || gamepad1.dpadDownWasPressed()) {
hoodOffset += 0.03; hoodOffset += 0.03;
autoHoodOffset += 0.02; autoHoodOffset += 0.02;
@@ -799,20 +800,19 @@ public class TeleopV3 extends LinearOpMode {
// } // }
// //
public double hoodAnglePrediction(double x) { public double hoodAnglePrediction(double x) {
if (x < 34) { double a = 1.44304;
double L = 1.04471; double b = 0.0313707;
double U = 0.711929; double c = 0.0931136;
double Q = 120.02263;
double B = 0.780982;
double M = 20.61191;
double v = 10.40506;
double inner = 1 + Q * Math.exp(-B * (x - M)); double result = a * Math.exp(-b * x) + c;
return L + (U - L) / Math.pow(inner, 1.0 / v);
// Clamp between min and max
if (result < 0.1) {
return 0.1;
} else if (result > 0.96) {
return 0.96;
} else { } else {
// x >= 34 return result;
return 1.94372 * Math.exp(-0.0528731 * x) + 0.39;
} }
} }

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java
View File

@@ -42,17 +42,7 @@ public class PIDServoTest extends LinearOpMode {
while (opModeIsActive()) { while (opModeIsActive()) {
controller.setPIDF(p, i, d, f); controller.setPIDF(p, i, d, f);
if (mode == 1) {
if (mode == 0) {
pos = (double) ((double)robot.turr1Pos.getCurrentPosition() /1024.0) * ((double) 44.0 /(double)77.0);
double pid = controller.calculate(pos, target);
robot.turr1.setPower(pid);
robot.turr2.setPower(-pid);
} else if (mode == 1) {
pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3); pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3);
double pid = controller.calculate(pos, target); double pid = controller.calculate(pos, target);

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

@@ -60,10 +60,6 @@ public class ShooterTest extends LinearOpMode {
robot.hood.setPosition(hoodPos); robot.hood.setPosition(hoodPos);
} }
if (turretPos != 0.501) {
robot.turr1.setPower(turretPos);
robot.turr2.setPower(turretPos);
}
robot.transfer.setPower(transferPower); robot.transfer.setPower(transferPower);
if (shoot) { if (shoot) {

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

@@ -44,16 +44,9 @@ public class PositionalServoProgrammer extends LinearOpMode {
robot.spin1.setPower(spindexPow); robot.spin1.setPower(spindexPow);
robot.spin2.setPower(-spindexPow); robot.spin2.setPower(-spindexPow);
} }
if (turretPos != 0.501 && !servo.turretEqual(turretPos)){ if (turretPos != 0.501){
double pos = servo.setTurrPos(turretPos); robot.turr1.setPosition(turretPos);
robot.turr1.setPower(pos); robot.turr2.setPosition(1-turretPos);
robot.turr2.setPower(-pos);
} else if (mode == 0){
robot.turr1.setPower(turrHoldPow);
robot.turr2.setPower(turrHoldPow);
} else {
robot.turr1.setPower(turretPow);
robot.turr2.setPower(-turretPow);
} }
if (transferPos != 0.501){ if (transferPos != 0.501){
robot.transferServo.setPosition(transferPos); robot.transferServo.setPosition(transferPos);

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

@@ -27,8 +27,8 @@ public class Robot {
public DcMotorEx shooter2; public DcMotorEx shooter2;
public Servo hood; public Servo hood;
public Servo transferServo; public Servo transferServo;
public CRServo turr1; public Servo turr1;
public CRServo turr2; public Servo turr2;
public CRServo spin1; public CRServo spin1;
public CRServo spin2; public CRServo spin2;
public AnalogInput spin1Pos; public AnalogInput spin1Pos;
@@ -74,9 +74,9 @@ public class Robot {
hood = hardwareMap.get(Servo.class, "hood"); hood = hardwareMap.get(Servo.class, "hood");
turr1 = hardwareMap.get(CRServo.class, "t1"); turr1 = hardwareMap.get(Servo.class, "t1");
turr2 = hardwareMap.get(CRServo.class, "t2"); turr2 = hardwareMap.get(Servo.class, "t2");
turr1Pos = intake; // Encoder of turret plugged in intake port turr1Pos = intake; // Encoder of turret plugged in intake port