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

Fixed some stuff presumably..untested

Browse Source
This commit is contained in:
Keshav Anand
2026-01-27 17:47:17 -06:00
parent 0549902505
commit 80f095cd57
5 changed files with 309 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

260
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT.java Normal file
View File

@@ -0,0 +1,260 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Poses_V2.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto;
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.spinPow;
import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease;
import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spindexPos;
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.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting;
import org.firstinspires.ftc.teamcode.utils.Turret;
@Autonomous(preselectTeleOp = "TeleopV3")
@Config
public class Auto_LT extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
Servos servos;
Spindexer spindexer;
Flywheel flywheel;
private double x1, y1, h1;
public static double shoot0Vel = 2300, shoot0Hood = 0.93;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.03;
public Action shootAll(int vel, double shootTime) {
return new Action() {
int ticker = 1;
double stamp = 0.0;
double velo = vel;
int shooterTicker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", velo);
TELE.addLine("shooting");
TELE.update();
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
robot.intake.setPower(0);
if (getRuntime() - stamp < shootTime) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)){
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1-autoSpinStartPos);
} else {
robot.transferServo.setPosition(transferServo_in);
shooterTicker++;
double prevSpinPos = robot.spin1.getPosition();
robot.spin1.setPosition(prevSpinPos + shoot0SpinSpeedIncrease);
robot.spin2.setPosition(1 - prevSpinPos - shoot0SpinSpeedIncrease);
}
return true;
} else {
robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1;
spindexer.resetSpindexer();
spindexer.processIntake();
return false;
}
}
};
}
public Action manageFlywheel(
double vel,
double hoodPos,
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance
) {
boolean timeFallback = (time != 0.501);
boolean posXFallback = (posX != 0.501);
boolean posYFallback = (posY != 0.501);
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
flywheel.manageFlywheel(vel);
robot.hood.setPosition(hoodPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
return !shouldFinish;
}
};
}
@Override
public void runOpMode() throws InterruptedException {
hardwareMap.get(Servo.class, "light").setPosition(0);
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
flywheel = new Flywheel(hardwareMap);
Targeting targeting = new Targeting();
Targeting.Settings targetingSettings = new Targeting.Settings(0.0, 0.0);
spindexer = new Spindexer(hardwareMap);
robot.limelight.pipelineSwitch(1);
Turret turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(0.4);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1-autoSpinStartPos);
robot.light.setPosition(1);
while (opModeInInit()) {
robot.hood.setPosition(shoot0Hood);
if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance;
}
if (redAlliance) {
robot.light.setPosition(0.28);
x1 = rx1;
y1 = ry1;
h1 = rh1;
} else {
robot.light.setPosition(0.6);
x1 = bx1;
y1 = by1;
h1 = bh1;
}
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()) {
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
manageFlywheel(
shoot0Vel,
shoot0Hood,
3.0,
x1,
0.501,
1,
0.501
)
)
);
Actions.runBlocking(
shootAll(2300, 3.0)
);
sleep(5000);
}
}
}

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

@@ -203,6 +203,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
};
}
public Action Shoot(int vel) {
return new Action() {
int ticker = 1;

43
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java Normal file
View File

@@ -0,0 +1,43 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Poses_V2 {
public static double goalHeight = 42; //in inches
public static double turretHeight = 12;
public static double relativeGoalHeight = goalHeight - turretHeight;
public static Pose2d goalPose = new Pose2d(-10, 0, 0);
public static double rx1 = 20, ry1 = 0, rh1 = 0;
public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140);
public static double rx2b = 23, ry2b = 36, rh2b = Math.toRadians(140);
public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140);
public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140);
public static double rx4a = 72, ry4a = 55, rh4a = Math.toRadians(140);
public static double rx4b = 48, ry4b = 79, rh4b = Math.toRadians(140);
public static double bx1 = 20, by1 = 0, bh1 = 0;
public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140);
public static double bx2b = 25, by2b = -38, bh2b = Math.toRadians(-140);
public static double bx2c = 34, by2c = -50, bh2c = Math.toRadians(-140);
public static double bx3a = 55, by3a = -43, bh3a = Math.toRadians(-140);
public static double bx3b = 37, by3b = -61, bh3b = Math.toRadians(-140);
public static double bx4a = 72, by4a = -55, bh4a = Math.toRadians(-140);
public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140);
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
public static Pose2d teleStart = new Pose2d(0, 0, 0);
}

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

@@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode.utils;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.hardware.ServoEx;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.robotcore.hardware.AnalogInput;