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

night before

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-08 22:29:36 -05:00
parent b2a1ea32d9
commit d0ee294d26
10 changed files with 107 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -393,7 +393,6 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
if (isStopRequested()) return; if (isStopRequested()) return;
while (opModeIsActive()){ while (opModeIsActive()){
robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed);
park.unpark(); park.unpark();
shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);

77
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java
View File

@@ -14,6 +14,7 @@ import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
@@ -39,13 +40,15 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
double runtime = 0; double runtime = 0;
// Wait Times // Wait Times
public static double rapidWaitTime = 0.4; public static double rapidWaitTimeShoot0 = 0.2;
public static double rapidShootTime = 0.45; public static double rapidWaitTimeShoot1 = 0.2;
public static double openGate1Time = 1.5; public static double rapidWaitTimeShoot2 = 0.2;
public static double openGate2Time = 1.5; public static double rapidWaitTimeShootGate = 0.4;
public static double openGateWaitTimeMax = 3; public static double rapidShootTime = 0.4;
public static double openGateWaitTimeMin = 1.75; public static double openGate1Time = 1.8;
public static int maxLoopCycles = 3; public static double openGate2Time = 1;
public static double openGateWaitTimeMax = 3.5;
public static int maxLoopCycles = 4;
// Initialize path state machine // Initialize path state machine
private enum PathState { private enum PathState {
@@ -70,11 +73,12 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
public static double openGate2ControlX = 45.9782359679267, openGate2ControlY = -15.106643757159245; public static double openGate2ControlX = 45.9782359679267, openGate2ControlY = -15.106643757159245;
public static double openGate2X = 57, openGate2Y = -8, openGate2H = 0; public static double openGate2X = 57, openGate2Y = -8, openGate2H = 0;
public static double shoot2ControlX = 57, shoot2ControlY = -8; public static double shoot2ControlX = 57, shoot2ControlY = -8;
public static double shoot2X = 25, shoot2Y = 11, shoot2H = -30; public static double shoot2X = 16, shoot2Y = 4, shoot2H = -30;
public static double intakeGateControlX = 61, intakeGateControlY = -11; public static double intakeGateControlX = 60, intakeGateControlY = -12;
public static double intakeGateX = 61, intakeGateY = -11, intakeGateH = 20; public static double toGateX = 60, toGateY = -10;
public static double shootGateControlX = 56, shootGateControlY = -10; public static double intakeGateX = 62, intakeGateY = -12.5, intakeGateH = 25;
public static double shootGateX = 25, shootGateY = 11, shootGateH = -30; public static double shootGateControlX = 40, shootGateControlY = -5;
public static double shootGateX = 16, shootGateY = 4, shootGateH = -30;
public static double shootLeaveControlX = 56, shootLeaveControlY = -10; public static double shootLeaveControlX = 56, shootLeaveControlY = -10;
public static double shootLeaveX = 16, shootLeaveY = 36, shootLeaveH = -50; public static double shootLeaveX = 16, shootLeaveY = 36, shootLeaveH = -50;
public static double leaveX = 45, leaveY = 10, leaveH = 0; public static double leaveX = 45, leaveY = 10, leaveH = 0;
@@ -83,22 +87,22 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
pickup1ControlX, pickup1X, openGate1ControlX, openGate1X, shoot1ControlX, shoot1X, pickup1ControlX, pickup1X, openGate1ControlX, openGate1X, shoot1ControlX, shoot1X,
pickup2ControlX, pickup2X, openGate2ControlX, openGate2X, shoot2ControlX, shoot2X, pickup2ControlX, pickup2X, openGate2ControlX, openGate2X, shoot2ControlX, shoot2X,
intakeGateControlX, intakeGateX, shootGateControlX, shootGateX, intakeGateControlX, intakeGateX, shootGateControlX, shootGateX,
shootLeaveControlX, shootLeaveX, leaveX, awayFromGateX}; shootLeaveControlX, shootLeaveX, leaveX, awayFromGateX, toGateX};
double[] yPoses = {startPoseY, shoot0Y, double[] yPoses = {startPoseY, shoot0Y,
pickup1ControlY, pickup1Y, openGate1ControlY, openGate1Y, shoot1ControlY, shoot1Y, pickup1ControlY, pickup1Y, openGate1ControlY, openGate1Y, shoot1ControlY, shoot1Y,
pickup2ControlY, pickup2Y, openGate2ControlY, openGate2Y, shoot2ControlY, shoot2Y, pickup2ControlY, pickup2Y, openGate2ControlY, openGate2Y, shoot2ControlY, shoot2Y,
intakeGateControlY, intakeGateY, shootGateControlY, shootGateY, intakeGateControlY, intakeGateY, shootGateControlY, shootGateY,
shootLeaveControlY, shootLeaveY, leaveY, awayFromGateY}; shootLeaveControlY, shootLeaveY, leaveY, awayFromGateY, toGateY};
double[] headings = {startPoseH, shoot0H, double[] headings = {startPoseH, shoot0H,
0, pickup1H, 0, openGate1H, 0, shoot1H, 0, pickup1H, 0, openGate1H, 0, shoot1H,
0, pickup2H, 0, openGate2H, 0, shoot2H, 0, pickup2H, 0, openGate2H, 0, shoot2H,
0, intakeGateH, 0, shootGateH, 0, intakeGateH, 0, shootGateH,
0, shootLeaveH, leaveH, awayFromGateH}; 0, shootLeaveH, leaveH, awayFromGateH, 0};
Pose startPose, shoot0, Pose startPose, shoot0,
pickup1Control, pickup1, openGate1Control, openGate1, shoot1Control, shoot1, pickup1Control, pickup1, openGate1Control, openGate1, shoot1Control, shoot1,
pickup2Control, pickup2, openGate2Control, openGate2, shoot2Control, shoot2, pickup2Control, pickup2, openGate2Control, openGate2, shoot2Control, shoot2,
intakeGateControl, intakeGate, shootGateControl, shootGate, intakeGateControl, intakeGate, shootGateControl, shootGate,
shootLeaveControl, shootLeave, leave, awayFromGate; shootLeaveControl, shootLeave, leave, awayFromGate, toGate;
private void initializePoses(){ private void initializePoses(){
startPose = new Pose(xPoses[0], yPoses[0], Math.toRadians(headings[0])); startPose = new Pose(xPoses[0], yPoses[0], Math.toRadians(headings[0]));
shoot0 = new Pose(xPoses[1], yPoses[1], Math.toRadians(headings[1])); shoot0 = new Pose(xPoses[1], yPoses[1], Math.toRadians(headings[1]));
@@ -122,12 +126,13 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
shootLeave = new Pose(xPoses[19], yPoses[19], Math.toRadians(headings[19])); shootLeave = new Pose(xPoses[19], yPoses[19], Math.toRadians(headings[19]));
leave = new Pose(xPoses[20], yPoses[20], Math.toRadians(headings[20])); leave = new Pose(xPoses[20], yPoses[20], Math.toRadians(headings[20]));
awayFromGate = new Pose(xPoses[21], yPoses[21], Math.toRadians(headings[21])); awayFromGate = new Pose(xPoses[21], yPoses[21], Math.toRadians(headings[21]));
toGate = new Pose(xPoses[22], yPoses[22]);
} }
//Building Paths //Building Paths
PathChain startPose_shoot0, shoot0_pickup1, pickup1_openGate1, openGate1_shoot1, PathChain startPose_shoot0, shoot0_pickup1, pickup1_openGate1, openGate1_shoot1,
shoot1_pickup2, pickup2_openGate2, openGate2_shoot2, shoot1_pickup2, pickup2_openGate2, openGate2_shoot2, shootGate_intakeGate,
shoot2_intakeGate, intakeGate_shootGate, shootGate_intakeGate, intakeGate_shootLeave, shootGate_leave, intakeGate_awayFromGate; shoot2_intakeGate, intakeGate_shootGate, intakeGate_shootLeave, shootGate_leave, intakeGate_awayFromGate;
private void buildPaths(){ private void buildPaths(){
startPose_shoot0 = follower.pathBuilder() startPose_shoot0 = follower.pathBuilder()
.addPath(new BezierLine(startPose, shoot0)) .addPath(new BezierLine(startPose, shoot0))
@@ -160,27 +165,29 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
.build(); .build();
openGate2_shoot2 = follower.pathBuilder() openGate2_shoot2 = follower.pathBuilder()
.addPath(new BezierCurve(openGate2, shoot2Control, shoot2)) .addPath(new BezierLine(openGate2, shoot2))
.setLinearHeadingInterpolation(openGate2.getHeading(), shoot2.getHeading()) .setTangentHeadingInterpolation()
.setReversed()
.build(); .build();
shoot2_intakeGate = follower.pathBuilder() shoot2_intakeGate = follower.pathBuilder()
.addPath(new BezierCurve(shoot2, intakeGateControl, intakeGate)) .addPath(new BezierCurve(shoot2, intakeGateControl, toGate))
.setLinearHeadingInterpolation(shoot2.getHeading(), intakeGate.getHeading()) .setLinearHeadingInterpolation(shoot2.getHeading(), intakeGate.getHeading())
.build(); .build();
intakeGate_shootGate = follower.pathBuilder() intakeGate_shootGate = follower.pathBuilder()
.addPath(new BezierCurve(intakeGate, shootGateControl, shootGate)) .addPath(new BezierCurve(intakeGate, shootGateControl, shootGate))
.setLinearHeadingInterpolation(intakeGate.getHeading(), shootGate.getHeading()) .setTangentHeadingInterpolation()
.setReversed()
.build(); .build();
shootGate_intakeGate = follower.pathBuilder() shootGate_intakeGate = follower.pathBuilder()
.addPath(new BezierCurve(shootGate, intakeGateControl, intakeGate)) .addPath(new BezierLine(shootGate, intakeGate))
.setLinearHeadingInterpolation(shootGate.getHeading(), intakeGate.getHeading()) .setLinearHeadingInterpolation(shootGate.getHeading(), intakeGate.getHeading())
.build(); .build();
intakeGate_shootLeave = follower.pathBuilder() intakeGate_shootLeave = follower.pathBuilder()
.addPath(new BezierCurve(intakeGate, shootLeaveControl, shootLeave)) .addPath(new BezierCurve(intakeGate, pickup2Control, shootLeave))
.setLinearHeadingInterpolation(intakeGate.getHeading(), shootLeave.getHeading()) .setLinearHeadingInterpolation(intakeGate.getHeading(), shootLeave.getHeading())
.build(); .build();
@@ -199,6 +206,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
private boolean startAuto = true; private boolean startAuto = true;
private double timeStamp = 0; private double timeStamp = 0;
private int cycle = 0; private int cycle = 0;
boolean toGateBool = false;
private void pathStateMachine(){ private void pathStateMachine(){
double currentTime = (double) System.currentTimeMillis() / 1000; double currentTime = (double) System.currentTimeMillis() / 1000;
switch(pathState){ switch(pathState){
@@ -210,11 +218,11 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
follower.followPath(startPose_shoot0, false); follower.followPath(startPose_shoot0, false);
startAuto = false; startAuto = false;
} }
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ if ((follower.atParametricEnd() || !follower.isBusy()) && currentTime - timeStamp > rapidWaitTimeShoot0){
timeStamp = currentTime; timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT0; pathState = PathState.WAIT_SHOOT0;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
} else if (follower.isBusy()){ } else if (follower.isBusy() && !follower.atParametricEnd()){
timeStamp = currentTime; timeStamp = currentTime;
} }
break; break;
@@ -241,7 +249,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
} }
break; break;
case DRIVE_SHOOT1: case DRIVE_SHOOT1:
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTimeShoot1){
timeStamp = currentTime; timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT1; pathState = PathState.WAIT_SHOOT1;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
@@ -259,6 +267,8 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
break; break;
case PICKUP2: case PICKUP2:
if (!follower.isBusy()){ if (!follower.isBusy()){
shooter.setFlywheelVelocity(2500);
robot.setHoodPos(0.6);
follower.followPath(pickup2_openGate2, false); follower.followPath(pickup2_openGate2, false);
pathState = PathState.OPENGATE2; pathState = PathState.OPENGATE2;
timeStamp = currentTime; timeStamp = currentTime;
@@ -272,7 +282,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
} }
break; break;
case DRIVE_SHOOT2: case DRIVE_SHOOT2:
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTimeShoot2){
timeStamp = currentTime; timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT2; pathState = PathState.WAIT_SHOOT2;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
@@ -287,17 +297,19 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
follower.followPath(shoot2_intakeGate, false); follower.followPath(shoot2_intakeGate, false);
pathState = PathState.INTAKE_GATE; pathState = PathState.INTAKE_GATE;
timeStamp = currentTime; timeStamp = currentTime;
toGateBool = true;
} }
break; break;
case INTAKE_GATE: case INTAKE_GATE:
if ((currentTime - timeStamp > openGateWaitTimeMax || (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed())) if ((currentTime - timeStamp > openGateWaitTimeMax)){
&& (currentTime - timeStamp > openGateWaitTimeMin)){
if (getRuntime() - runtime > 27){ if (getRuntime() - runtime > 27){
follower.followPath(intakeGate_awayFromGate, true); follower.followPath(intakeGate_awayFromGate, true);
pathState = PathState.WAIT_SHOOT_LEAVE; pathState = PathState.WAIT_SHOOT_LEAVE;
} else if (getRuntime() - runtime > 22 || cycle >= maxLoopCycles - 1){ } else if (getRuntime() - runtime > 22 || cycle >= maxLoopCycles - 1){
follower.followPath(intakeGate_shootLeave, true); follower.followPath(intakeGate_shootLeave, true);
pathState = PathState.DRIVE_SHOOT_LEAVE; pathState = PathState.DRIVE_SHOOT_LEAVE;
shooter.setFlywheelVelocity(2300);
robot.setHoodPos(0.68);
} else { } else {
follower.followPath(intakeGate_shootGate, false); follower.followPath(intakeGate_shootGate, false);
pathState = PathState.DRIVE_SHOOT_GATE; pathState = PathState.DRIVE_SHOOT_GATE;
@@ -307,7 +319,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
// TODO: add logic to shoot gate // TODO: add logic to shoot gate
break; break;
case DRIVE_SHOOT_GATE: case DRIVE_SHOOT_GATE:
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTimeShootGate){
timeStamp = currentTime; timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT_GATE; pathState = PathState.WAIT_SHOOT_GATE;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
@@ -326,12 +338,13 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
} else { } else {
follower.followPath(shootGate_intakeGate, false); follower.followPath(shootGate_intakeGate, false);
pathState = PathState.INTAKE_GATE; pathState = PathState.INTAKE_GATE;
toGateBool = true;
} }
timeStamp = currentTime; timeStamp = currentTime;
} }
break; break;
case DRIVE_SHOOT_LEAVE: case DRIVE_SHOOT_LEAVE:
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){ if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTimeShootGate){
timeStamp = currentTime; timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT_LEAVE; pathState = PathState.WAIT_SHOOT_LEAVE;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);

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

@@ -37,7 +37,7 @@ public class ServoPositions {
public static double transferServo_out = 0.28; public static double transferServo_out = 0.28;
public static double transferServo_in = 0.54; public static double transferServo_in = 0.52;
public static double hoodAuto = 0.27; public static double hoodAuto = 0.27;

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

@@ -133,6 +133,7 @@ public class TeleopV4 extends LinearOpMode {
follower.update(); follower.update();
Pose currentPose = follower.getPose(); Pose currentPose = follower.getPose();
teleStart = currentPose;
if (gamepad1.dpadLeftWasPressed()){ if (gamepad1.dpadLeftWasPressed()){
shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
@@ -259,9 +260,9 @@ public class TeleopV4 extends LinearOpMode {
} }
if (gamepad2.dpadRightWasPressed()){ if (gamepad2.dpadRightWasPressed()){
flywheelOffset+=100; flywheelOffset+=50;
} else if (gamepad2.dpadLeftWasPressed()){ } else if (gamepad2.dpadLeftWasPressed()){
flywheelOffset-=100; flywheelOffset-=50;
} }
if (gamepad2.crossWasPressed()){ if (gamepad2.crossWasPressed()){
@@ -279,6 +280,10 @@ public class TeleopV4 extends LinearOpMode {
// TELE.addData("Transfer Power", robot.transfer.getPower()); // TELE.addData("Transfer Power", robot.transfer.getPower());
TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM()); TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity()); TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
TELE.addData("Velocity 1", flywheel.getVelo1());
TELE.addData("Velocity 2", flywheel.getVelo2());
TELE.addData("Flywheel Offset", flywheelOffset);
TELE.addData("Hood Offset", hoodOffset);
// //
// TELE.addData("Current Position", currentPose); // TELE.addData("Current Position", currentPose);
// //

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(); NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.CM)); TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.CM));
TELE.addData("REV Green", revColor.blue / (revColor.green + revColor.blue + revColor.red)); TELE.addData("REV Green", revColor.green / (revColor.green + revColor.blue + revColor.red));
TELE.addData("Voltage Sensor", robot.voltage.getVoltage()); TELE.addData("Voltage Sensor", robot.voltage.getVoltage());

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

@@ -36,7 +36,7 @@ public class NewShooterTest extends LinearOpMode {
private boolean shooting = false; private boolean shooting = false;
public static int flywheelVelo = 0; public static int flywheelVelo = 0;
public static double hoodPos = 0.5; public static double hoodPos = 0.5;
public static double transferPower = -0.8; public static double transferPower = -1;
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {

44
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java
View File

@@ -83,11 +83,15 @@ public class Flywheel {
// pidf.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0); // pidf.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0);
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); if (velo1 != 0){
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
} else {
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
}
} }
private double prevF = 0; private double prevF = 0;
public static double voltagePIDFDifference = 1; public static double voltagePIDFDifference = 1;
double averageVoltage = 0; double averageVoltage = 0;
public void setF(double voltage){ public void setF(double voltage){
@@ -95,7 +99,11 @@ public class Flywheel {
double f = shooterPIDF_F / voltage; double f = shooterPIDF_F / voltage;
if (Math.abs(prevF - f) > voltagePIDFDifference && !steady) { if (Math.abs(prevF - f) > voltagePIDFDifference && !steady) {
shooterPIDF.f = f; shooterPIDF.f = f;
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); if (velo1 != 0){
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
} else {
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
}
} }
prevF = f; prevF = f;
} }
@@ -134,21 +142,43 @@ public class Flywheel {
double power; double power;
double prevTargetTime = 0; double prevTargetTime = 0;
double prevTargetVelocity = 0; double prevTargetVelocity = 0;
int veloMode = 0;
public void manageFlywheel(double commandedVelocity) { public void manageFlywheel(double commandedVelocity) {
if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) { if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
targetVelocity = commandedVelocity; targetVelocity = commandedVelocity;
} }
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); if (velo1 < 100){
power = robot.shooter1.getPower();
robot.shooter2.setPower(power);
}
velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
velo = velo1; if (velo1 < 100){
velo1 = 0;
velo = velo2;
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
if (veloMode != 2){
robot.shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
veloMode = 2;
}
power = robot.shooter2.getPower();
robot.shooter1.setPower(power);
} else {
velo2 = 0;
velo = velo1;
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
if (veloMode != 1){
robot.shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
veloMode = 1;
}
power = robot.shooter1.getPower();
robot.shooter2.setPower(power);
}
updateVelocityAverage(velo); updateVelocityAverage(velo);

19
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}; int[] shootOrder = {0, 1, 2};
final double sensorDistanceThreshold = 5.3; public static final double sensorDistanceThreshold = 5.35;
final long pulseTime = 100; // ms final long pulseTime = 70; // ms
private DesiredPattern desiredPattern = DesiredPattern.GPP; private DesiredPattern desiredPattern = DesiredPattern.GPP;
@@ -325,7 +325,7 @@ public class SpindexerTransferIntake {
robot.setSpinPos( robot.setSpinPos(
ServoPositions.spindexer_A2 ServoPositions.spindexer_A2
); );
robot.setTransferPower(-0.7); robot.setTransferPower(-1);
robot.setTransferServoPos( robot.setTransferServoPos(
ServoPositions.transferServo_out ServoPositions.transferServo_out
); );
@@ -335,7 +335,7 @@ public class SpindexerTransferIntake {
holdBallsTicker++; holdBallsTicker++;
} }
if (holdBallsTicker > 10){ if (holdBallsTicker > 20){
setRapidMode(RapidMode.TRANSFER_OFF); setRapidMode(RapidMode.TRANSFER_OFF);
holdBallsTicker = 0; holdBallsTicker = 0;
} }
@@ -355,7 +355,7 @@ public class SpindexerTransferIntake {
robot.setIntakePower(1.0); robot.setIntakePower(1.0);
if (stateTime() >= 300) { if (stateTime() >= 700) {
setRapidMode(RapidMode.PULSE_OUT); setRapidMode(RapidMode.PULSE_OUT);
} }
@@ -375,25 +375,23 @@ public class SpindexerTransferIntake {
robot.setIntakePower(1.0); robot.setIntakePower(1.0);
if (stateTime() >= 200) { if (stateTime() >= 400) {
setRapidMode(RapidMode.HOLD_BALLS); setRapidMode(RapidMode.HOLD_BALLS);
} }
break; break;
case HOLD_BALLS: case HOLD_BALLS:
if (robot.insideBeam.isPressed() if (robot.insideBeam.isPressed()
&& robot.outsideBeam.isPressed()) { && robot.outsideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
robot.setTransferPower(0); robot.setTransferPower(0);
robot.setIntakePower(0.1); robot.setIntakePower(0.1);
robot.setTransferServoPos(ServoPositions.transferServo_in); robot.setTransferServoPos(ServoPositions.transferServo_in);
} else { } else {
holdBallsTicker++; setRapidMode(RapidMode.INTAKE);
robot.setIntakePower(1);
} }
break; break;
@@ -679,7 +677,6 @@ public class SpindexerTransferIntake {
robot.setTransferPower(-0.8); robot.setTransferPower(-0.8);
} }
switch (shootState) { switch (shootState) {
case IDLE: case IDLE:
shootOrder = buildShootOrder( shootOrder = buildShootOrder(

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java
View File

@@ -17,8 +17,8 @@ public class Turret {
private final double servoTicksPer180 = 0.58; private final double servoTicksPer180 = 0.58;
public static double neutralPosition = 0.51; public static double neutralPosition = 0.51;
private final double turretMin = 0.05; private final double turretMin = 0.08;
private final double turretMax = 0.95; private final double turretMax = 0.91;
public static boolean limelightUsed = true; public static boolean limelightUsed = true;
public static double B_PID_P = 0.0001, B_PID_I = 0.0, B_PID_D = 0.000005; public static double B_PID_P = 0.0001, B_PID_I = 0.0, B_PID_D = 0.000005;
LLResult result; LLResult result;

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java
View File

@@ -67,6 +67,10 @@ public class VelocityCommander {
hoodC*Math.pow(dist, 1) + hoodC*Math.pow(dist, 1) +
hoodD; hoodD;
} }
if (lockBack){
hoodPos-=0.04;
}
hoodPos = Math.max(0.48, Math.min(0.88, hoodPos)); hoodPos = Math.max(0.48, Math.min(0.88, hoodPos));
} }
public double getHoodPredicted(){ public double getHoodPredicted(){
@@ -75,7 +79,7 @@ public class VelocityCommander {
private void distToTransferPow(double dist, double voltage){ private void distToTransferPow(double dist, double voltage){
if (dist < 140){ if (dist < 140){
transferPow = -0.75; transferPow = -1;
} else { } else {
transferPow = -0.6; transferPow = -0.6;
} }