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

a lot of changes

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-06 19:35:13 -05:00
parent d1626b20da
commit 47ef898127
9 changed files with 89 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

75
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
View File

@@ -16,8 +16,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.constants.TeleStart;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.teleop.TeleopV4;
import org.firstinspires.ftc.teamcode.utilsv2.*;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
@@ -37,8 +37,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
SpindexerTransferIntake spindexer;
// Wait Times
public static double sortedShootTime = 2;
public static double rapidWaitTime = 0.25;
public static double sortedShootTime = 2.6;
public static double rapidWaitTime = 0.5;
public static double rapidShootTime = 0.8;
public static double openGateTime = 2.5;
public static double pushTime = 2;
@@ -57,23 +57,23 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
PathState pathState = PathState.PUSHBOT;
// Poses
public static double startPoseX = 91.5, startPoseY = 15, startPoseH = 90;
public static double pushBotX = 97, pushBotY = 18, pushBotH = 100;
public static double shoot0ControlX = 94.29667812142038, shoot0ControlY = 55.03493699885454;
public static double shoot0X = 95, shoot0Y = 83, shoot0H = 0;
public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0;
public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165;
public static double openGateX = 131, openGateY = 74, openGateH = 0;
public static double shoot1ControlX = 112, shoot1ControlY = 75;
public static double shoot1X = 95, shoot1Y = 83, shoot1H = 0;
public static double drivePickup2X = 98, drivePickup2Y = 58.5, drivePickup2H = 0;
public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0;
public static double shoot2ControlX = 102, shoot2ControlY = 63;
public static double shoot2X = 95, shoot2Y = 83, shoot2H = 0;
public static double drivePickup3X = 98, drivePickup3Y = 34.5, drivePickup3H = 0;
public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0;
public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446;
public static double shoot3X = 84, shoot3Y = 120, shoot3H = -90;
public static double startPoseX = 12, startPoseY = -64, startPoseH = 90;
public static double pushBotX = 19, pushBotY = -63, pushBotH = 100;
public static double shoot0ControlX = 16.29667812142038, shoot0ControlY = -19.67493699885454;
public static double shoot0X = 19, shoot0Y = 10, shoot0H = 0;
public static double pickup1X = 54, pickup1Y = 10, pickup1H = 0;
public static double openGateControlX = 37.184421534937, openGateControlY = 2.24455899198165;
public static double openGateX = 59, openGateY = 2, openGateH = 0;
public static double shoot1ControlX = 40, shoot1ControlY = 3;
public static double shoot1X = 19, shoot1Y = 10, shoot1H = 0;
public static double drivePickup2X = 26, drivePickup2Y = -14, drivePickup2H = 0;
public static double pickup2X = 61, pickup2Y = -14.5, pickup2H = 0;
public static double shoot2ControlX = 30, shoot2ControlY = -9;
public static double shoot2X = 19, shoot2Y = 10, shoot2H = 0;
public static double drivePickup3X = 26, drivePickup3Y = -37.5, drivePickup3H = 0;
public static double pickup3X = 61, pickup3Y = -37.5, pickup3H = 0;
public static double shoot3ControlX = 25.62371134020621, shoot3ControlY = -38.813287514318446;
public static double shoot3X = 12, shoot3Y = 40, shoot3H = -90;
double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X,
pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X,
drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
@@ -193,7 +193,6 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
if (!follower.isBusy() || currentTime - timeStamp > pushTime){
follower.followPath(pushBot_shoot0, true);
pathState = PathState.DRIVE_SHOOT0;
timeStamp = currentTime;
}
break;
case DRIVE_SHOOT0:
@@ -201,6 +200,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT0;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
} else if (follower.isBusy()){
timeStamp = currentTime;
}
break;
case WAIT_SHOOT0:
@@ -220,6 +221,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
shootY = shoot1Y;
shootH = shoot1H;
timeStamp = currentTime;
shooter.setFlywheelVelocity(2300);
robot.setHoodPos(0.68);
}
break;
case OPENGATE:
@@ -271,8 +274,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
break;
case DRIVE_PICKUP3:
if (!follower.isBusy()){
shooter.setFlywheelVelocity(2300);
robot.setHoodPos(0.68);
shooter.setFlywheelVelocity(2200);
robot.setHoodPos(0.75);
follower.followPath(drivePickup3_pickup3, intakePower, false);
pathState = PathState.PICKUP3;
}
@@ -302,7 +305,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
}
// Used for changing alliance
private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);}
private double adjustXPoseBasedOnAlliance(double pose) {return -pose;}
private double adjustHeadingBasedOnAlliance(double heading){
heading = 180 - heading;
while (heading > 180) {heading-=360;}
@@ -321,7 +324,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
follower = Constants.createFollower(hardwareMap);
sleep(1000);
follower.setStartingPose(new Pose(72,72,0));
follower.setStartingPose(new Pose(0,0,0));
loopTimes = new MeasuringLoopTimes();
loopTimes.init();
turret = new Turret(robot);
@@ -344,9 +347,14 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
if (gamepad1.crossWasPressed() && !initializeRobot){
Color.redAlliance = !Color.redAlliance;
shooter.setRedAlliance(Color.redAlliance);
}
for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);}
for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);}
if (!initializeRobot){
if ((Color.redAlliance && xPoses[0] < 0)
|| (!Color.redAlliance && xPoses[0] > 0)){
for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);}
for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);}
}
}
if (gamepad1.triangleWasPressed()){
@@ -385,10 +393,11 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
shooter.update(robot.voltage.getVoltage());
follower.update();
if (!isStopRequested()){
follower.update();
}
pathStateMachine();
Pose currentPose = follower.getPose();
TeleStart.teleStart = currentPose;
TeleopV4.teleStart = follower.getPose();
spindexer.update();
@@ -400,9 +409,9 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.addData("X:", currentPose.getX());
TELE.addData("Y:", currentPose.getY());
TELE.addData("H:", currentPose.getHeading());
TELE.addData("X:", TeleopV4.teleStart.getX());
TELE.addData("Y:", TeleopV4.teleStart.getY());
TELE.addData("H:", TeleopV4.teleStart.getHeading());
TELE.update();
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TeleStart.java
View File

@@ -1,7 +0,0 @@
package org.firstinspires.ftc.teamcode.constants;
import com.pedropathing.geometry.Pose;
public class TeleStart {
public static Pose teleStart = new Pose(72,72,0);
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
View File

@@ -42,8 +42,8 @@ public class Constants {
public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1);
public static PinpointConstants localizerConstants = new PinpointConstants()
.forwardPodY(3.7795)
.strafePodX(-3.676)
.forwardPodY(-3.7795)
.strafePodX(-3.769)
.distanceUnit(DistanceUnit.INCH)
.hardwareMapName("pinpoint")
.encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD)

26
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
View File

@@ -148,7 +148,7 @@ class LocalizationTest extends OpMode {
@Override
public void init() {
follower.setStartingPose(new Pose(72,72, 0));
follower.setStartingPose(new Pose(0,0, 0));
}
/** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */
@@ -1272,7 +1272,7 @@ class CentripetalTuner extends OpMode {
public void start() {
follower.activateAllPIDFs();
forwards = new Path(new BezierCurve(new Pose(0,0), new Pose(Math.abs(DISTANCE) ,0), new Pose(Math.abs(DISTANCE) ,DISTANCE )));
backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) ,0), new Pose(0,0)));
backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 0,DISTANCE + 0), new Pose(Math.abs(DISTANCE) ,0), new Pose(0,0)));
backwards.setTangentHeadingInterpolation();
backwards.reverseHeadingInterpolation();
@@ -1382,14 +1382,14 @@ class Circle extends OpMode {
public void start() {
circle = follower.pathBuilder()
.addPath(new BezierCurve(new Pose(72, 72), new Pose(RADIUS + 72, 72), new Pose(RADIUS + 72, RADIUS + 72)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
.addPath(new BezierCurve(new Pose(RADIUS + 72, RADIUS + 72), new Pose(RADIUS + 72, (2 * RADIUS) + 72), new Pose(72, (2 * RADIUS) + 72)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
.addPath(new BezierCurve(new Pose(72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, RADIUS + 72)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
.addPath(new BezierCurve(new Pose(-RADIUS + 72, RADIUS + 72), new Pose(-RADIUS + 72, 72), new Pose(72, 72)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
.addPath(new BezierCurve(new Pose(0, 0), new Pose(RADIUS + 0, 0), new Pose(RADIUS + 0, RADIUS + 0)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS + 0))
.addPath(new BezierCurve(new Pose(RADIUS + 0, RADIUS + 0), new Pose(RADIUS + 0, (2 * RADIUS) + 0), new Pose(0, (2 * RADIUS) + 0)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS + 0))
.addPath(new BezierCurve(new Pose(0, (2 * RADIUS) + 0), new Pose(-RADIUS + 0, (2 * RADIUS) + 0), new Pose(-RADIUS + 0, RADIUS + 0)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS + 0))
.addPath(new BezierCurve(new Pose(-RADIUS + 0, RADIUS + 0), new Pose(-RADIUS + 0, 0), new Pose(0, 0)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS + 0))
.build();
follower.followPath(circle);
}
@@ -1406,7 +1406,7 @@ class Circle extends OpMode {
@Override
public void init() {
follower.setStartingPose(new Pose(72, 72));
follower.setStartingPose(new Pose(0, 0));
}
/**
@@ -1639,8 +1639,8 @@ class OffsetsTuner extends OpMode {
telemetry.addLine("Total Angle: " + follower.getTotalHeading());
telemetry.addLine("The following values are the offsets in inches that should be applied to your localizer.");
telemetry.addLine("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0));
telemetry.addLine("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0));
telemetry.addLine("strafeX: " + ((0.0-follower.getPose().getX()) / 2.0));
telemetry.addLine("forwardY: " + ((0.0-follower.getPose().getY()) / 2.0));
telemetry.update();
drawCurrentAndHistory();

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
View File

@@ -9,7 +9,6 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.TeleStart;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utilsv2.*;
@@ -31,7 +30,8 @@ public class TeleopV4 extends LinearOpMode {
ParkTilter parkTilter;
MeasuringLoopTimes loopTimes;
public static Pose relocalizePose = new Pose(128, 83, 0);
public static Pose relocalizePose = new Pose(56, 11, 0);
public static Pose teleStart = new Pose(0,0,0);
@Override
public void runOpMode() throws InterruptedException {
@@ -47,9 +47,9 @@ public class TeleopV4 extends LinearOpMode {
commander = new VelocityCommander();
drivetrain = new Drivetrain(robot, TELE);
follower = Constants.createFollower(hardwareMap);
follower.setStartingPose(TeleStart.teleStart);
follower.setStartingPose(teleStart);
sleep(500);
follower.setPose(TeleStart.teleStart);
follower.setPose(teleStart);
follower.update();
flywheel = new Flywheel(robot);
@@ -73,6 +73,8 @@ public class TeleopV4 extends LinearOpMode {
limelightUsed = true;
TELE.addLine("Initialization is done");
TELE.addData("Starting Position", follower.getPose());
TELE.addData("TELE START", teleStart);
TELE.update();
waitForStart();
@@ -89,10 +91,12 @@ public class TeleopV4 extends LinearOpMode {
if (gamepad1.crossWasPressed()){
if (Color.redAlliance){
relocalizePose = new Pose(128, 83, 0);
relocalizePose = new Pose(57.5, 5, 0);
} else {
relocalizePose = new Pose(16, 83, 180);
relocalizePose = new Pose(-57.5, 5, Math.toRadians(180));
}
follower.setPose(relocalizePose);
sleep(500);
gamepad1.rumble(100);
}

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

@@ -131,7 +131,7 @@ public class Hardware_Tester extends LinearOpMode {
NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.CM));
TELE.addData("REV Green", revColor.green / (revColor.red + revColor.blue + revColor.green));
TELE.addData("REV Green", revColor.blue / (revColor.green + revColor.blue + revColor.red));
TELE.addData("Voltage Sensor", robot.voltage.getVoltage());

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java
View File

@@ -48,9 +48,9 @@ public class NewShooterTest extends LinearOpMode {
commander = new VelocityCommander();
drivetrain = new Drivetrain(robot, TELE);
follower = Constants.createFollower(hardwareMap);
follower.setStartingPose(new Pose(72,72,0));
follower.setStartingPose(new Pose(0,0,0));
sleep(500);
follower.setPose(new Pose(72,72,0));
follower.setPose(new Pose(0,0,0));
flywheel = new Flywheel(robot);
turret = new Turret(robot);
@@ -86,11 +86,11 @@ public class NewShooterTest extends LinearOpMode {
if (gamepad1.crossWasPressed()){
if (Color.redAlliance){
TeleopV4.relocalizePose = new Pose(128, 83, 0);
TeleopV4.relocalizePose = new Pose(56, 11, 0);
} else {
TeleopV4.relocalizePose = new Pose(16, 83, 180);
TeleopV4.relocalizePose = new Pose(-56, 11, 180);
}
gamepad1.rumble(100);
follower.setPose(TeleopV4.relocalizePose);
gamepad1.rumble(100);
}

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java
View File

@@ -6,6 +6,8 @@ import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.pedropathing.follower.Follower;
import org.firstinspires.ftc.teamcode.constants.Color;
@Config
public class Shooter {
@@ -42,14 +44,14 @@ public class Shooter {
this.red = input;
if (this.red) {
goalX = 144;
turretGoalX = 140;
goalX = 72;
turretGoalX = 68;
} else {
goalX = 0;
turretGoalX = 8;
goalX = -72;
turretGoalX = -68;
}
goalY = 144;
turretGoalY = 132;
goalY = 72;
turretGoalY = 68;
}
private double flywheelVelocity = 0.0;
@@ -89,7 +91,7 @@ public class Shooter {
private final double shooterDistFromCenter = 1.545;
public void update(double voltage) {
setRedAlliance(Color.redAlliance);
switch (state) {
case NOTHING:
break;

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
View File

@@ -53,8 +53,8 @@ public class SpindexerTransferIntake {
}
int[] shootOrder = {0, 1, 2};
private final double sensorDistanceThreshold = 6.0;
private final long pulseTime = 100; // ms
final double sensorDistanceThreshold = 5.3;
final long pulseTime = 100; // ms
private DesiredPattern desiredPattern = DesiredPattern.GPP;
@@ -203,8 +203,8 @@ public class SpindexerTransferIntake {
private RapidMode rapidMode = RapidMode.INTAKE;
private SortedIntakeStates sortedIntakeStates = SortedIntakeStates.IDLE;
private BallStates[] ballColors = {BallStates.UNKNOWN, BallStates.UNKNOWN, BallStates.UNKNOWN};
private final double greenThresh = 0.39;
private final double spinMovementTime = 250;
final double greenThresh = 0.39;
final double spinMovementTime = 250;
/**
* Time when current state was entered.
@@ -571,7 +571,7 @@ public class SpindexerTransferIntake {
break;
case WAIT_FOR_1:
if (shootStateTime() > 250) {
if (shootStateTime() > 400) {
setShootState(
SortedShootState.SHOOT_1
@@ -618,7 +618,7 @@ public class SpindexerTransferIntake {
break;
case WAIT_FOR_2:
if (shootStateTime() > 250) {
if (shootStateTime() > 400) {
setShootState(
SortedShootState.SHOOT_2
@@ -664,7 +664,7 @@ public class SpindexerTransferIntake {
break;
case WAIT_FOR_3:
if (shootStateTime() > 250) {
if (shootStateTime() > 400) {
setShootState(
SortedShootState.SHOOT_3