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

back gate auto is like 80% done

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-16 18:03:52 -06:00
parent 85989d54b9
commit 0f556a193f
11 changed files with 115 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
View File

@@ -33,7 +33,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
@Autonomous(preselectTeleOp = "TeleopV3") @Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close extends LinearOpMode { public class Auto_LT_Close extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93; public static double shoot0Vel = 2300, shoot0Hood = 0.93;
public static double spindexerSpeedIncrease = 0.008; public static double spindexerSpeedIncrease = 0.014;
// These values are ADDED to turrDefault // These values are ADDED to turrDefault
public static double redObeliskTurrPos1 = 0.12; public static double redObeliskTurrPos1 = 0.12;
@@ -355,15 +355,6 @@ public class Auto_LT_Close extends LinearOpMode {
void shoot(){ void shoot(){
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
autoActions.manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501,
0.501,
false
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
) )
@@ -380,12 +371,8 @@ public class Auto_LT_Close extends LinearOpMode {
flywheel0Time, flywheel0Time,
x1, x1,
y1, y1,
posXTolerance, h1
posYTolerance,
h1,
false
) )
) )
); );
} }
@@ -394,16 +381,12 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
pickup1.build(), pickup1.build(),
autoActions.manageShooterAuto( autoActions.intake(
intake1Time, intake1Time,
x2b, x2b,
y2b, y2b,
posXTolerance, h2b
posYTolerance,
h2b,
true
), ),
autoActions.intake(intake1Time),
autoActions.detectObelisk( autoActions.detectObelisk(
intake1Time, intake1Time,
x2b, x2b,
@@ -436,17 +419,15 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
autoActions.manageShooterAuto( shoot1.build(),
autoActions.prepareShootAll(
colorSenseTime,
shoot1Time, shoot1Time,
motif,
posX, posX,
posY, posY,
posXTolerance, posH
posYTolerance, )
posH,
false
),
shoot1.build(),
autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif)
) )
); );
} }
@@ -455,16 +436,12 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
pickup2.build(), pickup2.build(),
autoActions.manageShooterAuto( autoActions.intake(
intake2Time, intake2Time,
x3b, x3b,
y3b, y3b,
posXTolerance, h3b
posYTolerance,
h3b,
true
), ),
autoActions.intake(intake2Time),
autoActions.detectObelisk( autoActions.detectObelisk(
intake2Time, intake2Time,
x3b, x3b,
@@ -497,17 +474,14 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
autoActions.manageShooterAuto( shoot2.build(),
autoActions.prepareShootAll(
colorSenseTime,
shoot2Time, shoot2Time,
motif,
posX, posX,
posY, posY,
posXTolerance, posH)
posYTolerance,
posH,
false
),
shoot2.build(),
autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif)
) )
); );
} }
@@ -516,16 +490,12 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
pickup3.build(), pickup3.build(),
autoActions.manageShooterAuto( autoActions.intake(
intake3Time, intake3Time,
x4b, x4b,
y4b, y4b,
posXTolerance, h4b
posYTolerance,
h4b,
true
), ),
autoActions.intake(intake3Time),
autoActions.detectObelisk( autoActions.detectObelisk(
intake3Time, intake3Time,
x4b, x4b,
@@ -545,17 +515,15 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
autoActions.manageShooterAuto( shoot3.build(),
autoActions.prepareShootAll(
colorSenseTime,
shoot3Time, shoot3Time,
motif,
xLeave, xLeave,
yLeave, yLeave,
posXTolerance, hLeave
posYTolerance, )
hLeave,
false
),
shoot3.build(),
autoActions.prepareShootAll(colorSenseTime, shoot3Time, motif)
) )
); );
} }

82
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java
View File

@@ -60,16 +60,16 @@ public class Auto_LT_Far extends LinearOpMode {
public static int pickupStackSpeed = 12; public static int pickupStackSpeed = 12;
public static int pickupGateSpeed = 25; public static int pickupGateSpeed = 25;
int prevMotif = 0; int prevMotif = 0;
public static double spindexerSpeedIncrease = 0.008; public static double spindexerSpeedIncrease = 0.014;
public static double shootAllTime = 2; public static double shootAllTime = 2;
// ---- POSITION TOLERANCES ---- // ---- POSITION TOLERANCES ----
public static double posXTolerance = 5.0; public static double posXTolerance = 5.0;
public static double posYTolerance = 5.0; public static double posYTolerance = 5.0;
public static double shootStackTime = 2; public static double shootStackTime = 2;
public static double shootGateTime = 2; public static double shootGateTime = 2.5;
public static double colorSenseTime = 1; public static double colorSenseTime = 1;
public static double intakeStackTime = 3; public static double intakeStackTime = 2.5;
public static double intakeGateTime = 2.5; public static double intakeGateTime = 2;
public static double redObeliskTurrPos1 = 0.12; public static double redObeliskTurrPos1 = 0.12;
public static double redObeliskTurrPos2 = 0.13; public static double redObeliskTurrPos2 = 0.13;
public static double redObeliskTurrPos3 = 0.14; public static double redObeliskTurrPos3 = 0.14;
@@ -79,7 +79,7 @@ public class Auto_LT_Far extends LinearOpMode {
double obeliskTurrPos1 = 0.0; double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0; double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0; double obeliskTurrPos3 = 0.0;
public static double endAutoTime = 25; public static double endAutoTime = 26;
// initialize path variables here // initialize path variables here
TrajectoryActionBuilder leave3Ball = null; TrajectoryActionBuilder leave3Ball = null;
@@ -276,7 +276,14 @@ public class Auto_LT_Far extends LinearOpMode {
} }
while (gatePickup && getRuntime() - stamp < endAutoTime){ while (gatePickup && getRuntime() - stamp < endAutoTime){
cycleGatePickup(); cycleGatePickupBalls();
if (getRuntime() - stamp > endAutoTime){
break;
}
cycleGatePrepareShoot();
if (getRuntime() - stamp > endAutoTime + shootAllTime + 1){
break;
}
shoot(); shoot();
} }
@@ -307,15 +314,6 @@ public class Auto_LT_Far extends LinearOpMode {
void shoot(){ void shoot(){
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
autoActions.manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501,
0.501,
false
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
) )
@@ -329,10 +327,7 @@ public class Auto_LT_Far extends LinearOpMode {
flywheel0Time, flywheel0Time,
0.501, 0.501,
0.501, 0.501,
0.501, 0.501
0.501,
0.501,
false
) )
) )
@@ -353,16 +348,12 @@ public class Auto_LT_Far extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
pickup3.build(), pickup3.build(),
autoActions.manageShooterAuto( autoActions.intake(
intakeStackTime, intakeStackTime,
xStackPickupB, xStackPickupB,
yStackPickupB, yStackPickupB,
posXTolerance, hStackPickupB
posYTolerance,
hStackPickupB,
true
), ),
autoActions.intake(intakeStackTime),
autoActions.detectObelisk( autoActions.detectObelisk(
intakeStackTime, intakeStackTime,
xStackPickupB, xStackPickupB,
@@ -382,35 +373,28 @@ public class Auto_LT_Far extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
autoActions.manageShooterAuto( shoot3.build(),
autoActions.prepareShootAll(
colorSenseTime,
shootStackTime, shootStackTime,
motif,
xShoot, xShoot,
yShoot, yShoot,
posXTolerance, hShoot)
posYTolerance,
hShoot,
false
),
shoot3.build(),
autoActions.prepareShootAll(colorSenseTime, shootStackTime, motif)
) )
); );
} }
void cycleGatePickup(){ void cycleGatePickupBalls(){
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
pickupGate.build(), pickupGate.build(),
autoActions.manageShooterAuto( autoActions.intake(
intakeGateTime, intakeStackTime,
pickupGateX, pickupGateX,
pickupGateY, pickupGateY,
posXTolerance, pickupGateH
posYTolerance,
pickupGateH,
true
), ),
autoActions.intake(intakeStackTime),
autoActions.detectObelisk( autoActions.detectObelisk(
intakeGateTime, intakeGateTime,
pickupGateX, pickupGateX,
@@ -426,20 +410,20 @@ public class Auto_LT_Far extends LinearOpMode {
if (motif == 0) motif = prevMotif; if (motif == 0) motif = prevMotif;
prevMotif = motif; prevMotif = motif;
}
void cycleGatePrepareShoot(){
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
autoActions.manageShooterAuto( shootGate.build(),
autoActions.prepareShootAll(
colorSenseTime,
shootGateTime, shootGateTime,
motif,
xShoot, xShoot,
yShoot, yShoot,
posXTolerance, hShoot
posYTolerance, )
hShoot,
false
),
shootGate.build(),
autoActions.prepareShootAll(colorSenseTime, shootGateTime, motif)
) )
); );
} }

79
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java
View File

@@ -49,11 +49,12 @@ public class AutoActions{
private int passengerSlotGreen = 0; private int passengerSlotGreen = 0;
private int rearSlotGreen = 0; private int rearSlotGreen = 0;
private int mostGreenSlot = 0; private int mostGreenSlot = 0;
private double firstSpindexShootPos = spinStartPos; private double firstSpindexShootPos = spindexer_outtakeBall1;
private boolean shootForward = true; private boolean shootForward = true;
public static double firstShootTime = 0.3; public static double firstShootTime = 0.3;
public int motif = 0; public int motif = 0;
double spinEndPos = ServoPositions.spinEndPos; double spinEndPos = ServoPositions.spinEndPos;
int waitFirstBallTicks = 4;
public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig){ public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig){
this.robot = rob; this.robot = rob;
@@ -68,7 +69,14 @@ public class AutoActions{
this.light = lig; this.light = lig;
} }
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { public Action prepareShootAll(
double colorSenseTime,
double time,
int motif_id,
double posX,
double posY,
double posH
) {
return new Action() { return new Action() {
double stamp = 0.0; double stamp = 0.0;
int ticker = 0; int ticker = 0;
@@ -107,10 +115,12 @@ public class AutoActions{
spinEndPos = spindexer_outtakeBall2 + 0.1; spinEndPos = spindexer_outtakeBall2 + 0.1;
} }
Action manageShooter = null;
@Override @Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) { public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) { if (ticker == 0) {
stamp = System.currentTimeMillis(); stamp = System.currentTimeMillis();
manageShooter = manageShooterAuto(time, posX, posY, posH);
} }
ticker++; ticker++;
servos.setTransferPos(transferServo_out); servos.setTransferPos(transferServo_out);
@@ -120,6 +130,8 @@ public class AutoActions{
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
manageShooter.run(telemetryPacket);
if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) { if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
spindexerWiggle *= -1.0; spindexerWiggle *= -1.0;
@@ -199,22 +211,16 @@ public class AutoActions{
}; };
} }
private boolean doneShooting = false;
public Action shootAllAuto(double shootTime, double spindexSpeed) { public Action shootAllAuto(double shootTime, double spindexSpeed) {
return new Action() { return new Action() {
int ticker = 1; int ticker = 1;
double stamp = 0.0; double stamp = 0.0;
double velo = 0.0;
int shooterTicker = 0; int shooterTicker = 0;
Action manageShooter = null;
@Override @Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) { public boolean run(@NonNull TelemetryPacket telemetryPacket) {
velo = flywheel.getVelo();
drive.updatePoseEstimate(); drive.updatePoseEstimate();
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
@@ -226,9 +232,13 @@ public class AutoActions{
if (ticker == 1) { if (ticker == 1) {
stamp = System.currentTimeMillis(); stamp = System.currentTimeMillis();
manageShooter = manageShooterAuto(shootTime, 0.501, 0.501, 0.501);
} }
ticker++; ticker++;
manageShooter.run(telemetryPacket);
double prevSpinPos = servos.getSpinCmdPos(); double prevSpinPos = servos.getSpinCmdPos();
boolean end; boolean end;
@@ -238,18 +248,18 @@ public class AutoActions{
end = prevSpinPos < spinEndPos; end = prevSpinPos < spinEndPos;
} }
if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < 2)) { if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < waitFirstBallTicks+1)) {
if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 3) { if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) {
servos.setTransferPos(transferServo_out); servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos); servos.setSpinPos(firstSpindexShootPos);
} else { } else {
servos.setTransferPos(transferServo_in); servos.setTransferPos(transferServo_in);
shooterTicker++; shooterTicker++;
if (shootForward) { if (shootForward && shooterTicker > waitFirstBallTicks) {
servos.setSpinPos(prevSpinPos + spindexSpeed); servos.setSpinPos(prevSpinPos + spindexSpeed);
} else { } else if (shooterTicker > waitFirstBallTicks){
servos.setSpinPos(prevSpinPos - spindexSpeed); servos.setSpinPos(prevSpinPos - spindexSpeed);
} }
@@ -262,7 +272,6 @@ public class AutoActions{
spindexer.resetSpindexer(); spindexer.resetSpindexer();
spindexer.processIntake(); spindexer.processIntake();
doneShooting = true;
return false; return false;
@@ -271,15 +280,21 @@ public class AutoActions{
}; };
} }
public Action intake(double intakeTime) { public Action intake(
double time,
double posX,
double posY,
double posH
) {
return new Action() { return new Action() {
double stamp = 0.0; double stamp = 0.0;
int ticker = 0; int ticker = 0;
Action manageShooter = null;
@Override @Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) { public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) { if (ticker == 0) {
stamp = System.currentTimeMillis(); stamp = System.currentTimeMillis();
manageShooter = manageShooterAuto(time, posX, posY, posH);
} }
ticker++; ticker++;
@@ -292,7 +307,9 @@ public class AutoActions{
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); manageShooter.run(telemetryPacket);
return ((System.currentTimeMillis() - stamp) < (time * 1000)) && !spindexer.isFull();
} }
}; };
} }
@@ -360,22 +377,15 @@ public class AutoActions{
double time, double time,
double posX, double posX,
double posY, double posY,
double posXTolerance, double posH
double posYTolerance,
double posH,
boolean whileIntaking
) { ) {
return new Action() { return new Action() {
double stamp = 0.0; double stamp = 0.0;
int ticker = 0; int ticker = 0;
int shootingTicker = 0;
double shootingStamp = 0;
final boolean timeFallback = (time != 0.501); final boolean timeFallback = (time != 0.501);
final boolean posXFallback = (posX != 0.501);
final boolean posYFallback = (posY != 0.501);
@Override @Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) { public boolean run(@NonNull TelemetryPacket telemetryPacket) {
@@ -418,31 +428,18 @@ public class AutoActions{
servos.setHoodPos(targetingSettings.hoodAngle); servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage(); double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(targetingSettings.flywheelRPM); flywheel.manageFlywheel(targetingSettings.flywheelRPM);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(robotX - posX) < posXTolerance; boolean shouldFinish = timeDone || flywheel.getSteady();
boolean yDone = posYFallback && Math.abs(robotY - posY) < posYTolerance;
boolean shouldFinish;
if (whileIntaking) {
shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
} else {
shouldFinish = timeDone || (xDone && yDone) || doneShooting;
}
teleStart = currentPose; teleStart = currentPose;
TELE.addData("Steady?", flywheel.getSteady()); TELE.addData("Steady?", flywheel.getSteady());
TELE.update(); TELE.update();
if (shouldFinish) { return !shouldFinish;
doneShooting = false;
return false;
} else {
return true;
}
} }
}; };
} }

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java
View File

@@ -14,10 +14,10 @@ public class Back_Poses {
public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140; public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140;
public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140; public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
public static double rStackPickupBX = 55, rStackPickupBY = 83, rStackPickupBH = 140.1; public static double rStackPickupBX = 55, rStackPickupBY = 73, rStackPickupBH = 140.1;
public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1; public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1;
public static double rPickupGateX = 65, rPickupGateY = 80, rPickupGateH = 140; public static double rPickupGateX = 70, rPickupGateY = 90, rPickupGateH = 140;
public static double bPickupGateX = 60, bPickupGateY = -88, bPickupGateH = -140; public static double bPickupGateX = 60, bPickupGateY = -88, bPickupGateH = -140;
public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50; public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java
View File

@@ -39,8 +39,8 @@ public class Front_Poses {
public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
public static double bxPrep = 45, byPrep = -10, bhPrep = -50; public static double bxPrep = 45, byPrep = -10, bhPrep = -50;
public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50; public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 55;
public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50; public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -55;
public static Pose2d teleStart = new Pose2d(0, 0, 0); public static Pose2d teleStart = new Pose2d(0, 0, 0);
} }

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

@@ -16,7 +16,7 @@ public class ServoPositions {
public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6; public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4; public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4;
public static double spinStartPos = 0.22; public static double spinStartPos = 0.3;
public static double spinEndPos = 0.85; public static double spinEndPos = 0.85;
public static double shootAllSpindexerSpeedIncrease = 0.014; public static double shootAllSpindexerSpeedIncrease = 0.014;

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

@@ -210,7 +210,7 @@ public class TeleopV3 extends LinearOpMode {
//SHOOTER: //SHOOTER:
double voltage = robot.voltage.getVoltage(); double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel); flywheel.manageFlywheel(vel);
//HOOD: //HOOD:

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

@@ -37,7 +37,7 @@ public class ShooterTest extends LinearOpMode {
public static double P = 255.0; public static double P = 255.0;
public static double I = 0.0; public static double I = 0.0;
public static double D = 0.0; public static double D = 0.0;
public static double F = 90; public static double F = 75;
public static double transferPower = 1.0; public static double transferPower = 1.0;
public static double hoodPos = 0.501; public static double hoodPos = 0.501;
public static double turretPos = 0.501; public static double turretPos = 0.501;

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
View File

@@ -68,17 +68,15 @@ public class Flywheel {
// Add code here to set PIDF based on desired RPM // Add code here to set PIDF based on desired RPM
//robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); //robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
//robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); //robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
// Record Current Velocity
velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
velo = Math.max(velo1, velo2);
} }
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
velo = Math.max(velo1, velo2);
// really should be a running average of the last 5 // really should be a running average of the last 5
steady = (Math.abs(commandedVelocity - velo) < 200.0); steady = (Math.abs(commandedVelocity - velo) < 50);
return powPID; return powPID;
} }

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

@@ -31,10 +31,10 @@ public class Robot {
public DcMotorEx intake; public DcMotorEx intake;
public DcMotorEx transfer; public DcMotorEx transfer;
public PIDFCoefficients shooterPIDF; public PIDFCoefficients shooterPIDF;
public double shooterPIDF_P = 255.0; public static double shooterPIDF_P = 255;
public double shooterPIDF_I = 0.0; public static double shooterPIDF_I = 0.0;
public double shooterPIDF_D = 0.0; public static double shooterPIDF_D = 0.0;
public double shooterPIDF_F = 90; public static double shooterPIDF_F = 75;
public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
public DcMotorEx shooter1; public DcMotorEx shooter1;
public DcMotorEx shooter2; public DcMotorEx shooter2;

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

@@ -27,7 +27,7 @@ public class Turret {
public static double turret180Range = 0.4; public static double turret180Range = 0.4;
public static double turrDefault = 0.37; public static double turrDefault = 0.37;
public static double turrMin = 0.15; public static double turrMin = 0.15;
public static double turrMax = 0.85; public static double turrMax = 0.8;
public static boolean limelightUsed = true; public static boolean limelightUsed = true;
public static double manualOffset = 0.0; public static double manualOffset = 0.0;
@@ -40,7 +40,7 @@ public class Turret {
public static double clampTolerance = 0.03; public static double clampTolerance = 0.03;
//public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125; //public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
public static double B_PID_P = 0.066, B_PID_I = 0.0, B_PID_D = 0.007; public static double B_PID_P = 0.08, B_PID_I = 0.0, B_PID_D = 0.007;
Robot robot; Robot robot;
MultipleTelemetry TELE; MultipleTelemetry TELE;
Limelight3A webcam; Limelight3A webcam;