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

added transfer movement to class

Browse Source
This commit is contained in:
DanTheMan-byte
2026-04-26 16:54:54 -05:00
parent 99216c1e80
commit 2a012ea3ae
2 changed files with 122 additions and 172 deletions
Download Patch File Download Diff File Expand all files Collapse all files

292
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java
View File

@@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode.autonomous; package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
@@ -22,12 +23,12 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting; import org.firstinspires.ftc.teamcode.utils.Targeting;
import org.firstinspires.ftc.teamcode.utils.Turret; import org.firstinspires.ftc.teamcode.utils.Turret;
import com.pedropathing.util.Timer;
import java.util.List; import java.util.List;
@@ -43,30 +44,21 @@ public class Auto12BallPedroPathing extends LinearOpMode {
Turret turret; Turret turret;
Spindexer spindexer; Spindexer spindexer;
Servos servos; Servos servos;
Timer opModeTimer, shootingTimer; MeasuringLoopTimes loopTimes;
// Wait Times // Wait Times
public static double shootTime = 2; public static double shootTime = 2;
// Extra Variables // Extra Variables
public static double intakePower = 0.3; public static double intakePower = 0.3;
double shootX, shootY, shootH;
// Initialize path state machine // Initialize path state machine
private enum PathState { private enum PathState {
DRIVE_SHOOT0, DRIVE_SHOOT0, WAIT_SHOOT0,
WAIT_SHOOT0, DRIVE_PICKUP1, PICKUP1, DRIVE_SHOOT1, WAIT_SHOOT1,
DRIVE_PICKUP1, DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2,
PICKUP1, DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3
DRIVE_SHOOT1,
WAIT_SHOOT1,
DRIVE_PICKUP2,
PICKUP2,
DRIVE_SHOOT2,
WAIT_SHOOT2,
DRIVE_PICKUP3,
PICKUP3,
DRIVE_SHOOT3,
WAIT_SHOOT3
} }
PathState pathState = PathState.DRIVE_SHOOT0; PathState pathState = PathState.DRIVE_SHOOT0;
@@ -86,21 +78,10 @@ public class Auto12BallPedroPathing extends LinearOpMode {
public static double pickup3X = 132, pickup3Y = 34.5, pickup3H = 0; public static double pickup3X = 132, pickup3Y = 34.5, pickup3H = 0;
public static double shoot3ControlX = 86, shoot3ControlY = 34.5; public static double shoot3ControlX = 86, shoot3ControlY = 34.5;
public static double shoot3X = 84, shoot3Y = 102, shoot3H = -90; public static double shoot3X = 84, shoot3Y = 102, shoot3H = -90;
Pose startPose; Pose startPose, shoot0,
Pose shoot0; drivePickup1, pickup1, shoot1,
Pose drivePickup1; drivePickup2Control, drivePickup2, pickup2, shoot2Control, shoot2,
Pose pickup1; drivePickup3Control, drivePickup3, pickup3, shoot3Control, shoot3;
Pose shoot1;
Pose drivePickup2Control;
Pose drivePickup2;
Pose pickup2;
Pose shoot2Control;
Pose shoot2;
Pose drivePickup3Control;
Pose drivePickup3;
Pose pickup3;
Pose shoot3Control;
Pose shoot3;
private void initializePoses(){ private void initializePoses(){
startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH)); startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH));
shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H)); shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
@@ -120,16 +101,10 @@ public class Auto12BallPedroPathing extends LinearOpMode {
} // add poses to void } // add poses to void
//Building Paths //Building Paths
PathChain startPose_shoot0; PathChain startPose_shoot0,
PathChain shoot0_drivePickup1; shoot0_drivePickup1, drivePickup1_pickup1, pickup1_shoot1,
PathChain drivePickup1_pickup1; shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2,
PathChain pickup1_shoot1; shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3;
PathChain shoot1_drivePickup2;
PathChain drivePickup2_pickup2;
PathChain pickup2_shoot2;
PathChain shoot2_drivePickup3;
PathChain drivePickup3_pickup3;
PathChain pickup3_shoot3;
private void buildPaths(){ private void buildPaths(){
startPose_shoot0 = follower.pathBuilder() startPose_shoot0 = follower.pathBuilder()
.addPath(new BezierLine(startPose, shoot0)) .addPath(new BezierLine(startPose, shoot0))
@@ -192,103 +167,107 @@ public class Auto12BallPedroPathing extends LinearOpMode {
if (startAuto){ if (startAuto){
follower.followPath(startPose_shoot0, true); follower.followPath(startPose_shoot0, true);
startAuto = false; startAuto = false;
shootX = shoot0X;
shootY = shoot0Y;
shootH = shoot0H;
} }
if (!follower.isBusy()){ driveShoot(PathState.WAIT_SHOOT0, currentTime);
pathState = PathState.WAIT_SHOOT0;
timeStamp = currentTime;
// spindexer.prepareShootAllContinous();
}
break; break;
case WAIT_SHOOT0: case WAIT_SHOOT0:
if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){ waitShoot(PathState.DRIVE_PICKUP1, shoot0_drivePickup1, currentTime);
// spindexer.resetSpindexer();
pathState = PathState.DRIVE_PICKUP1;
follower.followPath(shoot0_drivePickup1, true);
}
break; break;
case DRIVE_PICKUP1: case DRIVE_PICKUP1:
if (!follower.isBusy()){ drivePickup(PathState.PICKUP1, drivePickup1_pickup1);
pathState = PathState.PICKUP1;
follower.followPath(drivePickup1_pickup1, intakePower, false);
}
break; break;
case PICKUP1: case PICKUP1:
if (!follower.isBusy()){ pickup(PathState.DRIVE_SHOOT1, pickup1_shoot1);
pathState = PathState.DRIVE_SHOOT1; shootX = shoot1X;
follower.followPath(pickup1_shoot1, true); shootY = shoot1Y;
} shootH = shoot1H;
break; break;
case DRIVE_SHOOT1: case DRIVE_SHOOT1:
if (!follower.isBusy()){ intakePowerDown(timeStamp, currentTime);
pathState = PathState.WAIT_SHOOT1; driveShoot(PathState.WAIT_SHOOT1, currentTime);
timeStamp = currentTime;
// spindexer.prepareShootAllContinous();
}
break; break;
case WAIT_SHOOT1: case WAIT_SHOOT1:
if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){ waitShoot(PathState.DRIVE_PICKUP2, shoot1_drivePickup2, currentTime);
// spindexer.resetSpindexer();
pathState = PathState.DRIVE_PICKUP2;
follower.followPath(shoot1_drivePickup2, true);
}
break; break;
case DRIVE_PICKUP2: case DRIVE_PICKUP2:
if (!follower.isBusy()){ drivePickup(PathState.PICKUP2, drivePickup2_pickup2);
pathState = PathState.PICKUP2;
follower.followPath(drivePickup2_pickup2, intakePower, false);
}
break; break;
case PICKUP2: case PICKUP2:
if (!follower.isBusy()){ pickup(PathState.DRIVE_SHOOT2, pickup2_shoot2);
pathState = PathState.DRIVE_SHOOT2; shootX = shoot2X;
follower.followPath(pickup2_shoot2, true); shootY = shoot2Y;
} shootH = shoot2H;
break; break;
case DRIVE_SHOOT2: case DRIVE_SHOOT2:
if (!follower.isBusy()){ intakePowerDown(timeStamp, currentTime);
pathState = PathState.WAIT_SHOOT2; driveShoot(PathState.WAIT_SHOOT2, currentTime);
timeStamp = currentTime;
// spindexer.prepareShootAllContinous();
}
break; break;
case WAIT_SHOOT2: case WAIT_SHOOT2:
if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){ waitShoot(PathState.DRIVE_PICKUP3, shoot2_drivePickup3, currentTime);
// spindexer.resetSpindexer();
pathState = PathState.DRIVE_PICKUP3;
follower.followPath(shoot2_drivePickup3, true);
}
break; break;
case DRIVE_PICKUP3: case DRIVE_PICKUP3:
if (!follower.isBusy()){ drivePickup(PathState.PICKUP3, drivePickup3_pickup3);
pathState = PathState.PICKUP3;
follower.followPath(drivePickup3_pickup3, intakePower, false);
}
break; break;
case PICKUP3: case PICKUP3:
if (!follower.isBusy()){ pickup(PathState.DRIVE_SHOOT3, pickup3_shoot3);
pathState = PathState.DRIVE_SHOOT3; shootX = shoot3X;
follower.followPath(pickup3_shoot3, true); shootY = shoot3Y;
} shootH = shoot3H;
break; break;
case DRIVE_SHOOT3: case DRIVE_SHOOT3:
if (!follower.isBusy()){ intakePowerDown(timeStamp, currentTime);
pathState = PathState.WAIT_SHOOT3; driveShoot(PathState.WAIT_SHOOT3, currentTime);
// spindexer.prepareShootAllContinous();
}
break; break;
case WAIT_SHOOT3: case WAIT_SHOOT3:
if (spindexer.shootAllComplete()){ if (spindexer.shootAllComplete()){
// spindexer.resetSpindexer(); spindexer.resetSpindexer();
TELE.addLine("Done Auto"); TELE.addLine("Done Auto");
TELE.update();
} }
break; break;
default: default:
break; break;
} }
TELE.update(); TELE.update(); // use for debugging
} }
// Voids for State Machine
private void intakePowerDown(double stamp, double currentTime) {
double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust
if (pow < -1) {pow = 0;}
spindexer.setIntakePower(pow);
}
private void driveShoot(PathState nextState, double currentTime){
if (!follower.isBusy()){
pathState = nextState;
timeStamp = currentTime;
spindexer.prepareShootAllContinous();
}
}
private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) {
if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime) {
spindexer.resetSpindexer();
pathState = nextState;
follower.followPath(nextPath, true);
spindexer.setIntakePower(1);
}
}
private void drivePickup(PathState nextState, PathChain nextPath) {
if (!follower.isBusy()) {
pathState = nextState;
follower.followPath(nextPath, intakePower, false);
}
}
private void pickup(PathState nextState, PathChain nextPath) {
if (!follower.isBusy()) {
pathState = nextState;
follower.followPath(nextPath, true);
}
}
// Helps manage spindexer
private boolean driveToShoot(){ private boolean driveToShoot(){
return pathState == PathState.DRIVE_SHOOT0 || return pathState == PathState.DRIVE_SHOOT0 ||
pathState == PathState.DRIVE_SHOOT1 || pathState == PathState.DRIVE_SHOOT1 ||
@@ -296,10 +275,8 @@ public class Auto12BallPedroPathing extends LinearOpMode {
pathState == PathState.DRIVE_SHOOT3; pathState == PathState.DRIVE_SHOOT3;
} }
private double adjustXPoseBasedOnAlliance(double pose){ // Used for changing alliance
return (144-pose); private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);}
}
private double adjustHeadingBasedOnAlliance(double heading){ private double adjustHeadingBasedOnAlliance(double heading){
heading = 180 - heading; heading = 180 - heading;
while (heading > 180) {heading-=360;} while (heading > 180) {heading-=360;}
@@ -323,8 +300,8 @@ public class Auto12BallPedroPathing extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight); turret = new Turret(robot, TELE, robot.limelight);
spindexer = new Spindexer(hardwareMap); spindexer = new Spindexer(hardwareMap);
servos = new Servos(hardwareMap); servos = new Servos(hardwareMap);
opModeTimer = new Timer(); loopTimes = new MeasuringLoopTimes();
shootingTimer = new Timer(); loopTimes.init();
robot.light.setPosition(Color.LightRed); robot.light.setPosition(Color.LightRed);
@@ -340,53 +317,22 @@ public class Auto12BallPedroPathing extends LinearOpMode {
robot.light.setPosition(Color.LightBlue); robot.light.setPosition(Color.LightBlue);
} }
startPoseX = adjustXPoseBasedOnAlliance(startPoseX); double[] xPoses = {startPoseX, shoot0X,
startPoseH = adjustHeadingBasedOnAlliance(startPoseH); drivePickup1X, pickup1X, shoot1X,
drivePickup2ControlX, drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
drivePickup3ControlX, drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
shoot0X = adjustXPoseBasedOnAlliance(shoot0X); double[] headings = {startPoseH, shoot0H,
shoot0H = adjustHeadingBasedOnAlliance(shoot0H); drivePickup1H, pickup1H, shoot1H,
drivePickup2H, pickup2H, shoot2H,
drivePickup3H, pickup3H, shoot3H};
drivePickup1X = adjustXPoseBasedOnAlliance(drivePickup1X); for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);}
drivePickup1H = adjustHeadingBasedOnAlliance(drivePickup1H); for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);}
pickup1X = adjustXPoseBasedOnAlliance(pickup1X);
pickup1H = adjustHeadingBasedOnAlliance(pickup1H);
shoot1X = adjustXPoseBasedOnAlliance(shoot1X);
shoot1H = adjustHeadingBasedOnAlliance(shoot1H);
drivePickup2ControlX = adjustXPoseBasedOnAlliance(drivePickup2ControlX);
drivePickup2X = adjustXPoseBasedOnAlliance(drivePickup2X);
drivePickup2H = adjustHeadingBasedOnAlliance(drivePickup2H);
pickup2X = adjustXPoseBasedOnAlliance(pickup2X);
pickup2H = adjustHeadingBasedOnAlliance(pickup2H);
shoot2ControlX = adjustXPoseBasedOnAlliance(shoot2ControlX);
shoot2X = adjustXPoseBasedOnAlliance(shoot2X);
shoot2H = adjustHeadingBasedOnAlliance(shoot2H);
drivePickup3ControlX = adjustXPoseBasedOnAlliance(drivePickup3ControlX);
drivePickup3X = adjustXPoseBasedOnAlliance(drivePickup3X);
drivePickup3H = adjustHeadingBasedOnAlliance(drivePickup3H);
pickup3X = adjustXPoseBasedOnAlliance(pickup3X);
pickup3H = adjustHeadingBasedOnAlliance(pickup3H);
shoot3ControlX = adjustXPoseBasedOnAlliance(shoot3ControlX);
shoot3X = adjustXPoseBasedOnAlliance(shoot3X);
shoot3H = adjustHeadingBasedOnAlliance(shoot3H);
} }
if (gamepad1.triangleWasPressed()){ if (gamepad1.triangleWasPressed()){
initializeRobot = true; initializeRobot = true;
}
if (initializeRobot){
initializePoses(); initializePoses();
follower.setPose(startPose); follower.setPose(startPose);
buildPaths(); buildPaths();
@@ -408,34 +354,38 @@ public class Auto12BallPedroPathing extends LinearOpMode {
robot.transfer.setPower(1); robot.transfer.setPower(1);
limelightUsed = false; limelightUsed = false;
opModeTimer.resetTimer();
while (opModeIsActive()){ while (opModeIsActive()){
follower.update(); follower.update();
pathStateMachine(); pathStateMachine();
Pose currentPose = follower.getPose(); Pose currentPose = follower.getPose();
// teleStartPoseX = currentPose.getX(); teleStartPoseX = currentPose.getX();
// teleStartPoseY = currentPose.getY(); teleStartPoseY = currentPose.getY();
// teleStartPoseH = Math.toDegrees(currentPose.getHeading()); teleStartPoseH = Math.toDegrees(currentPose.getHeading());
// turret.trackGoal(currentPose); turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH)));
// targetingSettings = targeting.calculateSettings(currentPose.getX(), currentPose.getY(), currentPose.getHeading(), 0, turretInterpolate); targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate);
//
// double voltage = robot.voltage.getVoltage();
// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
// flywheel.manageFlywheel(targetingSettings.flywheelRPM);
// servos.setHoodPos(targetingSettings.hoodAngle);
//
// if (driveToShoot()){
// servos.setSpinPos(spinStartPos);
// } else {
// spindexer.processIntake();
// }
// TELE.addData("X:", currentPose.getX()); double voltage = robot.voltage.getVoltage();
// TELE.addData("Y:", currentPose.getY()); flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
// TELE.addData("H:", currentPose.getHeading()); flywheel.manageFlywheel(targetingSettings.flywheelRPM);
// TELE.update(); servos.setHoodPos(targetingSettings.hoodAngle);
if (driveToShoot()){servos.setSpinPos(spinStartPos);}
else {spindexer.processIntake();}
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}
loopTimes.loop();
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.update();
} }
} }
} }

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

@@ -215,7 +215,7 @@ public class TeleopV3 extends LinearOpMode {
} }
if (gamepad1.left_stick_button) { if (gamepad1.left_stick_button) {
servo.setTransferPos(transferServo_out); // servo.setTransferPos(transferServo_out);
//spindexPos = spindexer_intakePos1; //spindexPos = spindexer_intakePos1;
shootAll = false; shootAll = false;
spindexer.resetSpindexer(); spindexer.resetSpindexer();