Compare commits
2 Commits
85989d54b9
...
7161933d06
| Author | SHA1 | Date | |
|---|---|---|---|
| 7161933d06 | |||
| 0f556a193f |
@@ -2,6 +2,12 @@ package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos2;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
|
||||
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
|
||||
@@ -33,24 +39,12 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
|
||||
@Autonomous(preselectTeleOp = "TeleopV3")
|
||||
public class Auto_LT_Close extends LinearOpMode {
|
||||
public static double shoot0Vel = 2300, shoot0Hood = 0.93;
|
||||
public static double spindexerSpeedIncrease = 0.008;
|
||||
|
||||
// These values are ADDED to turrDefault
|
||||
public static double redObeliskTurrPos1 = 0.12;
|
||||
public static double redObeliskTurrPos2 = 0.13;
|
||||
public static double redObeliskTurrPos3 = 0.14;
|
||||
public static double blueObeliskTurrPos1 = -0.12;
|
||||
public static double blueObeliskTurrPos2 = -0.13;
|
||||
public static double blueObeliskTurrPos3 = -0.14;
|
||||
public static double redTurretShootPos = 0.1;
|
||||
public static double blueTurretShootPos = -0.14;
|
||||
public static double spindexerSpeedIncrease = 0.014;
|
||||
|
||||
double obeliskTurrPos1 = 0.0;
|
||||
double obeliskTurrPos2 = 0.0;
|
||||
double obeliskTurrPos3 = 0.0;
|
||||
|
||||
double turretShootPos = 0.0;
|
||||
|
||||
public static double shootAllTime = 2;
|
||||
public static double intake1Time = 3.3;
|
||||
public static double intake2Time = 3.8;
|
||||
@@ -97,8 +91,6 @@ public class Auto_LT_Close extends LinearOpMode {
|
||||
double xPrep, yPrep, hPrep;
|
||||
double xLeave, yLeave, hLeave;
|
||||
|
||||
private double shoot1Tangent;
|
||||
|
||||
int ballCycles = 3;
|
||||
int prevMotif = 0;
|
||||
|
||||
@@ -218,8 +210,6 @@ public class Auto_LT_Close extends LinearOpMode {
|
||||
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
|
||||
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
|
||||
obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
|
||||
turretShootPos = turrDefault + redTurretShootPos;
|
||||
|
||||
} else {
|
||||
robot.light.setPosition(0.6);
|
||||
|
||||
@@ -261,8 +251,6 @@ public class Auto_LT_Close extends LinearOpMode {
|
||||
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
|
||||
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
|
||||
obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
|
||||
turretShootPos = turrDefault + blueTurretShootPos;
|
||||
|
||||
}
|
||||
|
||||
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
|
||||
@@ -355,15 +343,6 @@ public class Auto_LT_Close extends LinearOpMode {
|
||||
void shoot(){
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
autoActions.manageShooterAuto(
|
||||
shootAllTime,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
false
|
||||
),
|
||||
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
|
||||
)
|
||||
|
||||
@@ -380,12 +359,8 @@ public class Auto_LT_Close extends LinearOpMode {
|
||||
flywheel0Time,
|
||||
x1,
|
||||
y1,
|
||||
posXTolerance,
|
||||
posYTolerance,
|
||||
h1,
|
||||
false
|
||||
h1
|
||||
)
|
||||
|
||||
)
|
||||
);
|
||||
}
|
||||
@@ -394,16 +369,12 @@ public class Auto_LT_Close extends LinearOpMode {
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
pickup1.build(),
|
||||
autoActions.manageShooterAuto(
|
||||
autoActions.intake(
|
||||
intake1Time,
|
||||
x2b,
|
||||
y2b,
|
||||
posXTolerance,
|
||||
posYTolerance,
|
||||
h2b,
|
||||
true
|
||||
h2b
|
||||
),
|
||||
autoActions.intake(intake1Time),
|
||||
autoActions.detectObelisk(
|
||||
intake1Time,
|
||||
x2b,
|
||||
@@ -436,17 +407,15 @@ public class Auto_LT_Close extends LinearOpMode {
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
autoActions.manageShooterAuto(
|
||||
shoot1.build(),
|
||||
autoActions.prepareShootAll(
|
||||
colorSenseTime,
|
||||
shoot1Time,
|
||||
motif,
|
||||
posX,
|
||||
posY,
|
||||
posXTolerance,
|
||||
posYTolerance,
|
||||
posH,
|
||||
false
|
||||
),
|
||||
shoot1.build(),
|
||||
autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif)
|
||||
posH
|
||||
)
|
||||
)
|
||||
);
|
||||
}
|
||||
@@ -455,16 +424,12 @@ public class Auto_LT_Close extends LinearOpMode {
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
pickup2.build(),
|
||||
autoActions.manageShooterAuto(
|
||||
autoActions.intake(
|
||||
intake2Time,
|
||||
x3b,
|
||||
y3b,
|
||||
posXTolerance,
|
||||
posYTolerance,
|
||||
h3b,
|
||||
true
|
||||
h3b
|
||||
),
|
||||
autoActions.intake(intake2Time),
|
||||
autoActions.detectObelisk(
|
||||
intake2Time,
|
||||
x3b,
|
||||
@@ -497,17 +462,14 @@ public class Auto_LT_Close extends LinearOpMode {
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
autoActions.manageShooterAuto(
|
||||
shoot2.build(),
|
||||
autoActions.prepareShootAll(
|
||||
colorSenseTime,
|
||||
shoot2Time,
|
||||
motif,
|
||||
posX,
|
||||
posY,
|
||||
posXTolerance,
|
||||
posYTolerance,
|
||||
posH,
|
||||
false
|
||||
),
|
||||
shoot2.build(),
|
||||
autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif)
|
||||
posH)
|
||||
)
|
||||
);
|
||||
}
|
||||
@@ -516,16 +478,12 @@ public class Auto_LT_Close extends LinearOpMode {
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
pickup3.build(),
|
||||
autoActions.manageShooterAuto(
|
||||
autoActions.intake(
|
||||
intake3Time,
|
||||
x4b,
|
||||
y4b,
|
||||
posXTolerance,
|
||||
posYTolerance,
|
||||
h4b,
|
||||
true
|
||||
h4b
|
||||
),
|
||||
autoActions.intake(intake3Time),
|
||||
autoActions.detectObelisk(
|
||||
intake3Time,
|
||||
x4b,
|
||||
@@ -545,17 +503,15 @@ public class Auto_LT_Close extends LinearOpMode {
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
autoActions.manageShooterAuto(
|
||||
shoot3.build(),
|
||||
autoActions.prepareShootAll(
|
||||
colorSenseTime,
|
||||
shoot3Time,
|
||||
motif,
|
||||
xLeave,
|
||||
yLeave,
|
||||
posXTolerance,
|
||||
posYTolerance,
|
||||
hLeave,
|
||||
false
|
||||
),
|
||||
shoot3.build(),
|
||||
autoActions.prepareShootAll(colorSenseTime, shoot3Time, motif)
|
||||
hLeave
|
||||
)
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
@@ -3,6 +3,14 @@ package org.firstinspires.ftc.teamcode.autonomous;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueTurretShootPos;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos2;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redTurretShootPos;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
|
||||
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
|
||||
@@ -34,8 +42,6 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
|
||||
@Autonomous(preselectTeleOp = "TeleopV3")
|
||||
public class Auto_LT_Far extends LinearOpMode {
|
||||
public static double shoot0Vel = 3300, shoot0Hood = 0.48;
|
||||
public static double redTurretShootPos = 0.05;
|
||||
public static double blueTurretShootPos = -0.05;
|
||||
double xLeave, yLeave, hLeave;
|
||||
public int motif = 0;
|
||||
double turretShootPos = 0.0;
|
||||
@@ -60,26 +66,20 @@ public class Auto_LT_Far extends LinearOpMode {
|
||||
public static int pickupStackSpeed = 12;
|
||||
public static int pickupGateSpeed = 25;
|
||||
int prevMotif = 0;
|
||||
public static double spindexerSpeedIncrease = 0.008;
|
||||
public static double spindexerSpeedIncrease = 0.014;
|
||||
public static double shootAllTime = 2;
|
||||
// ---- POSITION TOLERANCES ----
|
||||
public static double posXTolerance = 5.0;
|
||||
public static double posYTolerance = 5.0;
|
||||
public static double shootStackTime = 2;
|
||||
public static double shootGateTime = 2;
|
||||
public static double shootGateTime = 2.5;
|
||||
public static double colorSenseTime = 1;
|
||||
public static double intakeStackTime = 3;
|
||||
public static double intakeGateTime = 2.5;
|
||||
public static double redObeliskTurrPos1 = 0.12;
|
||||
public static double redObeliskTurrPos2 = 0.13;
|
||||
public static double redObeliskTurrPos3 = 0.14;
|
||||
public static double blueObeliskTurrPos1 = -0.12;
|
||||
public static double blueObeliskTurrPos2 = -0.13;
|
||||
public static double blueObeliskTurrPos3 = -0.14;
|
||||
public static double intakeStackTime = 2.5;
|
||||
public static double intakeGateTime = 2;
|
||||
double obeliskTurrPos1 = 0.0;
|
||||
double obeliskTurrPos2 = 0.0;
|
||||
double obeliskTurrPos3 = 0.0;
|
||||
public static double endAutoTime = 25;
|
||||
public static double endAutoTime = 26;
|
||||
|
||||
// initialize path variables here
|
||||
TrajectoryActionBuilder leave3Ball = null;
|
||||
@@ -276,7 +276,14 @@ public class Auto_LT_Far extends LinearOpMode {
|
||||
}
|
||||
|
||||
while (gatePickup && getRuntime() - stamp < endAutoTime){
|
||||
cycleGatePickup();
|
||||
cycleGatePickupBalls();
|
||||
if (getRuntime() - stamp > endAutoTime){
|
||||
break;
|
||||
}
|
||||
cycleGatePrepareShoot();
|
||||
if (getRuntime() - stamp > endAutoTime + shootAllTime + 1){
|
||||
break;
|
||||
}
|
||||
shoot();
|
||||
}
|
||||
|
||||
@@ -307,15 +314,6 @@ public class Auto_LT_Far extends LinearOpMode {
|
||||
void shoot(){
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
autoActions.manageShooterAuto(
|
||||
shootAllTime,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
false
|
||||
),
|
||||
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
|
||||
)
|
||||
|
||||
@@ -329,10 +327,7 @@ public class Auto_LT_Far extends LinearOpMode {
|
||||
flywheel0Time,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
false
|
||||
0.501
|
||||
)
|
||||
|
||||
)
|
||||
@@ -353,16 +348,12 @@ public class Auto_LT_Far extends LinearOpMode {
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
pickup3.build(),
|
||||
autoActions.manageShooterAuto(
|
||||
autoActions.intake(
|
||||
intakeStackTime,
|
||||
xStackPickupB,
|
||||
yStackPickupB,
|
||||
posXTolerance,
|
||||
posYTolerance,
|
||||
hStackPickupB,
|
||||
true
|
||||
hStackPickupB
|
||||
),
|
||||
autoActions.intake(intakeStackTime),
|
||||
autoActions.detectObelisk(
|
||||
intakeStackTime,
|
||||
xStackPickupB,
|
||||
@@ -382,35 +373,28 @@ public class Auto_LT_Far extends LinearOpMode {
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
autoActions.manageShooterAuto(
|
||||
shoot3.build(),
|
||||
autoActions.prepareShootAll(
|
||||
colorSenseTime,
|
||||
shootStackTime,
|
||||
motif,
|
||||
xShoot,
|
||||
yShoot,
|
||||
posXTolerance,
|
||||
posYTolerance,
|
||||
hShoot,
|
||||
false
|
||||
),
|
||||
shoot3.build(),
|
||||
autoActions.prepareShootAll(colorSenseTime, shootStackTime, motif)
|
||||
hShoot)
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
void cycleGatePickup(){
|
||||
void cycleGatePickupBalls(){
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
pickupGate.build(),
|
||||
autoActions.manageShooterAuto(
|
||||
intakeGateTime,
|
||||
autoActions.intake(
|
||||
intakeStackTime,
|
||||
pickupGateX,
|
||||
pickupGateY,
|
||||
posXTolerance,
|
||||
posYTolerance,
|
||||
pickupGateH,
|
||||
true
|
||||
pickupGateH
|
||||
),
|
||||
autoActions.intake(intakeStackTime),
|
||||
autoActions.detectObelisk(
|
||||
intakeGateTime,
|
||||
pickupGateX,
|
||||
@@ -426,20 +410,20 @@ public class Auto_LT_Far extends LinearOpMode {
|
||||
|
||||
if (motif == 0) motif = prevMotif;
|
||||
prevMotif = motif;
|
||||
}
|
||||
|
||||
void cycleGatePrepareShoot(){
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
autoActions.manageShooterAuto(
|
||||
shootGate.build(),
|
||||
autoActions.prepareShootAll(
|
||||
colorSenseTime,
|
||||
shootGateTime,
|
||||
motif,
|
||||
xShoot,
|
||||
yShoot,
|
||||
posXTolerance,
|
||||
posYTolerance,
|
||||
hShoot,
|
||||
false
|
||||
),
|
||||
shootGate.build(),
|
||||
autoActions.prepareShootAll(colorSenseTime, shootGateTime, motif)
|
||||
hShoot
|
||||
)
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
@@ -2,7 +2,6 @@ package org.firstinspires.ftc.teamcode.autonomous.actions;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
|
||||
@@ -10,7 +9,6 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
|
||||
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.utils.Targeting.turretInterpolate;
|
||||
|
||||
import androidx.annotation.NonNull;
|
||||
|
||||
@@ -49,9 +47,8 @@ public class AutoActions{
|
||||
private int passengerSlotGreen = 0;
|
||||
private int rearSlotGreen = 0;
|
||||
private int mostGreenSlot = 0;
|
||||
private double firstSpindexShootPos = spinStartPos;
|
||||
private double firstSpindexShootPos = spindexer_outtakeBall1;
|
||||
private boolean shootForward = true;
|
||||
public static double firstShootTime = 0.3;
|
||||
public int motif = 0;
|
||||
double spinEndPos = ServoPositions.spinEndPos;
|
||||
|
||||
@@ -68,7 +65,14 @@ public class AutoActions{
|
||||
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() {
|
||||
double stamp = 0.0;
|
||||
int ticker = 0;
|
||||
@@ -107,10 +111,12 @@ public class AutoActions{
|
||||
spinEndPos = spindexer_outtakeBall2 + 0.1;
|
||||
}
|
||||
|
||||
Action manageShooter = null;
|
||||
@Override
|
||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||
if (ticker == 0) {
|
||||
stamp = System.currentTimeMillis();
|
||||
manageShooter = manageShooterAuto(time, posX, posY, posH);
|
||||
}
|
||||
ticker++;
|
||||
servos.setTransferPos(transferServo_out);
|
||||
@@ -120,6 +126,8 @@ public class AutoActions{
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
manageShooter.run(telemetryPacket);
|
||||
|
||||
if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
|
||||
|
||||
spindexerWiggle *= -1.0;
|
||||
@@ -199,22 +207,16 @@ public class AutoActions{
|
||||
};
|
||||
}
|
||||
|
||||
private boolean doneShooting = false;
|
||||
public Action shootAllAuto(double shootTime, double spindexSpeed) {
|
||||
return new Action() {
|
||||
int ticker = 1;
|
||||
|
||||
double stamp = 0.0;
|
||||
|
||||
double velo = 0.0;
|
||||
|
||||
int shooterTicker = 0;
|
||||
|
||||
Action manageShooter = null;
|
||||
@Override
|
||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||
|
||||
velo = flywheel.getVelo();
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
@@ -226,9 +228,13 @@ public class AutoActions{
|
||||
|
||||
if (ticker == 1) {
|
||||
stamp = System.currentTimeMillis();
|
||||
manageShooter = manageShooterAuto(shootTime, 0.501, 0.501, 0.501);
|
||||
|
||||
}
|
||||
ticker++;
|
||||
|
||||
manageShooter.run(telemetryPacket);
|
||||
|
||||
double prevSpinPos = servos.getSpinCmdPos();
|
||||
|
||||
boolean end;
|
||||
@@ -238,18 +244,18 @@ public class AutoActions{
|
||||
end = prevSpinPos < spinEndPos;
|
||||
}
|
||||
|
||||
if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < 2)) {
|
||||
if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks+1)) {
|
||||
|
||||
if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 3) {
|
||||
if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) {
|
||||
servos.setTransferPos(transferServo_out);
|
||||
servos.setSpinPos(firstSpindexShootPos);
|
||||
} else {
|
||||
servos.setTransferPos(transferServo_in);
|
||||
shooterTicker++;
|
||||
|
||||
if (shootForward) {
|
||||
Spindexer.whileShooting = true;
|
||||
if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) {
|
||||
servos.setSpinPos(prevSpinPos + spindexSpeed);
|
||||
} else {
|
||||
} else if (shooterTicker > Spindexer.waitFirstBallTicks){
|
||||
servos.setSpinPos(prevSpinPos - spindexSpeed);
|
||||
}
|
||||
|
||||
@@ -259,10 +265,9 @@ public class AutoActions{
|
||||
|
||||
} else {
|
||||
servos.setTransferPos(transferServo_out);
|
||||
|
||||
Spindexer.whileShooting = false;
|
||||
spindexer.resetSpindexer();
|
||||
spindexer.processIntake();
|
||||
doneShooting = true;
|
||||
|
||||
return false;
|
||||
|
||||
@@ -271,15 +276,21 @@ public class AutoActions{
|
||||
};
|
||||
}
|
||||
|
||||
public Action intake(double intakeTime) {
|
||||
public Action intake(
|
||||
double time,
|
||||
double posX,
|
||||
double posY,
|
||||
double posH
|
||||
) {
|
||||
return new Action() {
|
||||
double stamp = 0.0;
|
||||
int ticker = 0;
|
||||
|
||||
Action manageShooter = null;
|
||||
@Override
|
||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||
if (ticker == 0) {
|
||||
stamp = System.currentTimeMillis();
|
||||
manageShooter = manageShooterAuto(time, posX, posY, posH);
|
||||
}
|
||||
ticker++;
|
||||
|
||||
@@ -292,7 +303,9 @@ public class AutoActions{
|
||||
|
||||
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 +373,15 @@ public class AutoActions{
|
||||
double time,
|
||||
double posX,
|
||||
double posY,
|
||||
double posXTolerance,
|
||||
double posYTolerance,
|
||||
double posH,
|
||||
boolean whileIntaking
|
||||
double posH
|
||||
) {
|
||||
|
||||
return new Action() {
|
||||
|
||||
double stamp = 0.0;
|
||||
int ticker = 0;
|
||||
int shootingTicker = 0;
|
||||
double shootingStamp = 0;
|
||||
|
||||
final boolean timeFallback = (time != 0.501);
|
||||
final boolean posXFallback = (posX != 0.501);
|
||||
final boolean posYFallback = (posY != 0.501);
|
||||
|
||||
@Override
|
||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||
@@ -406,7 +412,7 @@ public class AutoActions{
|
||||
deltaPose = new Pose2d(robotX, robotY, robotHeading);
|
||||
}
|
||||
|
||||
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
|
||||
// double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
|
||||
|
||||
targetingSettings = targeting.calculateSettings
|
||||
(robotX, robotY, robotHeading, 0.0, false);
|
||||
@@ -418,31 +424,18 @@ public class AutoActions{
|
||||
servos.setHoodPos(targetingSettings.hoodAngle);
|
||||
|
||||
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);
|
||||
|
||||
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
|
||||
boolean xDone = posXFallback && Math.abs(robotX - posX) < posXTolerance;
|
||||
boolean yDone = posYFallback && Math.abs(robotY - posY) < posYTolerance;
|
||||
boolean shouldFinish;
|
||||
if (whileIntaking) {
|
||||
shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
|
||||
} else {
|
||||
shouldFinish = timeDone || (xDone && yDone) || doneShooting;
|
||||
}
|
||||
boolean shouldFinish = timeDone || flywheel.getSteady();
|
||||
|
||||
teleStart = currentPose;
|
||||
|
||||
TELE.addData("Steady?", flywheel.getSteady());
|
||||
TELE.update();
|
||||
|
||||
if (shouldFinish) {
|
||||
doneShooting = false;
|
||||
return false;
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
|
||||
return !shouldFinish;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
package org.firstinspires.ftc.teamcode.constants;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.Pose2d;
|
||||
|
||||
@Config
|
||||
public class Back_Poses {
|
||||
@@ -14,10 +13,10 @@ public class Back_Poses {
|
||||
public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 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 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 autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50;
|
||||
|
||||
@@ -39,8 +39,8 @@ public class Front_Poses {
|
||||
public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
|
||||
public static double bxPrep = 45, byPrep = -10, bhPrep = -50;
|
||||
|
||||
public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50;
|
||||
public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50;
|
||||
public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 55;
|
||||
public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -55;
|
||||
|
||||
public static Pose2d teleStart = new Pose2d(0, 0, 0);
|
||||
}
|
||||
|
||||
@@ -5,18 +5,18 @@ import com.acmerobotics.dashboard.config.Config;
|
||||
@Config
|
||||
public class ServoPositions {
|
||||
|
||||
public static double spindexer_intakePos1 = 0.07; //0.13;
|
||||
public static double spindexer_intakePos1 = 0.06; //0.13;
|
||||
|
||||
public static double spindexer_intakePos2 = 0.27; //0.33;//0.5;
|
||||
public static double spindexer_intakePos2 = 0.25; //0.33;//0.5;
|
||||
|
||||
public static double spindexer_intakePos3 = 0.46; //0.53;//0.66;
|
||||
public static double spindexer_intakePos3 = 0.43; //0.53;//0.66;
|
||||
|
||||
public static double spindexer_outtakeBall3 = 0.71; //0.65; //0.24;
|
||||
public static double spindexer_outtakeBall3b = 0.15; //0.65; //0.24;
|
||||
|
||||
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 spinStartPos = 0.22;
|
||||
public static double spinStartPos = 0.3;
|
||||
public static double spinEndPos = 0.85;
|
||||
|
||||
public static double shootAllSpindexerSpeedIncrease = 0.014;
|
||||
@@ -39,7 +39,7 @@ public class ServoPositions {
|
||||
public static double blueObeliskTurrPos1 = -0.12;
|
||||
public static double blueObeliskTurrPos2 = -0.13;
|
||||
public static double blueObeliskTurrPos3 = -0.14;
|
||||
public static double redTurretShootPos = 0.1;
|
||||
public static double blueTurretShootPos = -0.14;
|
||||
public static double redTurretShootPos = 0.05;
|
||||
public static double blueTurretShootPos = -0.05;
|
||||
|
||||
}
|
||||
|
||||
@@ -2,7 +2,6 @@ package org.firstinspires.ftc.teamcode.teleop;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
|
||||
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.utils.Targeting.turretInterpolate;
|
||||
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
|
||||
@@ -46,9 +45,9 @@ public class TeleopV3 extends LinearOpMode {
|
||||
public double vel = 3000;
|
||||
public boolean autoVel = true;
|
||||
public boolean targetingHood = true;
|
||||
public boolean autoHood = true;
|
||||
// public boolean autoHood = true;
|
||||
public double shootStamp = 0.0;
|
||||
boolean fixedTurret = false;
|
||||
// boolean fixedTurret = false;
|
||||
Robot robot;
|
||||
MultipleTelemetry TELE;
|
||||
Light light;
|
||||
@@ -66,13 +65,13 @@ public class TeleopV3 extends LinearOpMode {
|
||||
boolean reject = false;
|
||||
double xOffset = 0.0;
|
||||
double yOffset = 0.0;
|
||||
double headingOffset = 0.0;
|
||||
// double headingOffset = 0.0;
|
||||
int ticker = 0;
|
||||
|
||||
boolean autoSpintake = false;
|
||||
// boolean autoSpintake = false;
|
||||
boolean enableSpindexerManager = true;
|
||||
|
||||
boolean overrideTurr = false;
|
||||
// boolean overrideTurr = false;
|
||||
|
||||
int intakeTicker = 0;
|
||||
private boolean shootAll = false;
|
||||
@@ -178,7 +177,7 @@ public class TeleopV3 extends LinearOpMode {
|
||||
double dy = robotY - goalY; // delta y from robot to goal
|
||||
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
|
||||
|
||||
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
|
||||
// double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
|
||||
|
||||
targetingSettings = targeting.calculateSettings
|
||||
(robotX, robotY, robotHeading, 0.0, turretInterpolate);
|
||||
@@ -210,7 +209,7 @@ public class TeleopV3 extends LinearOpMode {
|
||||
|
||||
//SHOOTER:
|
||||
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);
|
||||
|
||||
//HOOD:
|
||||
@@ -262,7 +261,6 @@ public class TeleopV3 extends LinearOpMode {
|
||||
spindexer.setIntakePower(1);
|
||||
} else if (gamepad1.cross) {
|
||||
spindexer.setIntakePower(-1);
|
||||
|
||||
} else {
|
||||
spindexer.setIntakePower(0);
|
||||
}
|
||||
@@ -288,11 +286,7 @@ public class TeleopV3 extends LinearOpMode {
|
||||
// //servo.setTransferPos(transferServo_in);
|
||||
// spindexer.shootAll();
|
||||
// TELE.addLine("starting to shoot");
|
||||
} else if (!spindexer.shootAllComplete()) {
|
||||
servo.setTransferPos(transferServo_in);
|
||||
//TELE.addLine("shoot");
|
||||
} else {
|
||||
servo.setTransferPos(transferServo_out);
|
||||
} else if (spindexer.shootAllComplete()) {
|
||||
//spindexPos = spindexer_intakePos1;
|
||||
shootAll = false;
|
||||
spindexer.resetSpindexer();
|
||||
|
||||
@@ -37,7 +37,7 @@ public class ShooterTest extends LinearOpMode {
|
||||
public static double P = 255.0;
|
||||
public static double I = 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 hoodPos = 0.501;
|
||||
public static double turretPos = 0.501;
|
||||
|
||||
@@ -12,15 +12,13 @@ public class Flywheel {
|
||||
public double velo1 = 0.0;
|
||||
public double velo2 = 0.0;
|
||||
double targetVelocity = 0.0;
|
||||
double previousTargetVelocity = 0.0;
|
||||
double powPID = 0.0;
|
||||
boolean steady = false;
|
||||
public Flywheel (HardwareMap hardwareMap) {
|
||||
robot = new Robot(hardwareMap);
|
||||
shooterPIDF1 = new PIDFCoefficients
|
||||
(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F);
|
||||
(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
|
||||
shooterPIDF2 = new PIDFCoefficients
|
||||
(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F);
|
||||
(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
|
||||
}
|
||||
|
||||
public double getVelo () {
|
||||
@@ -52,6 +50,7 @@ public class Flywheel {
|
||||
if (Math.abs(prevF - f) > voltagePIDFDifference){
|
||||
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
|
||||
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
|
||||
prevF = f;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -61,26 +60,23 @@ public class Flywheel {
|
||||
// Convert from Ticks per Second to RPM
|
||||
private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;}
|
||||
|
||||
public double manageFlywheel(double commandedVelocity) {
|
||||
public void manageFlywheel(double commandedVelocity) {
|
||||
|
||||
if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
|
||||
targetVelocity = commandedVelocity;
|
||||
// Add code here to set PIDF based on desired RPM
|
||||
//robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
|
||||
//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
|
||||
steady = (Math.abs(commandedVelocity - velo) < 200.0);
|
||||
steady = (Math.abs(commandedVelocity - velo) < 50);
|
||||
|
||||
return powPID;
|
||||
}
|
||||
|
||||
public void update()
|
||||
|
||||
@@ -7,10 +7,10 @@ public class MeasuringLoopTimes {
|
||||
private double minLoopTime = 999999999999.0;
|
||||
|
||||
private double maxLoopTime = 0.0;
|
||||
private double mainLoopTime = 0.0;
|
||||
double mainLoopTime = 0.0;
|
||||
|
||||
private double MeasurementStart = 0.0;
|
||||
private double currentTime = 0.0;
|
||||
double currentTime = 0.0;
|
||||
|
||||
private double avgLoopTime = 0.0;
|
||||
private int avgLoopTimeTicker = 0;
|
||||
|
||||
@@ -1,11 +1,9 @@
|
||||
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;
|
||||
import com.qualcomm.robotcore.hardware.CRServo;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
@@ -31,11 +29,11 @@ public class Robot {
|
||||
public DcMotorEx intake;
|
||||
public DcMotorEx transfer;
|
||||
public PIDFCoefficients shooterPIDF;
|
||||
public double shooterPIDF_P = 255.0;
|
||||
public double shooterPIDF_I = 0.0;
|
||||
public double shooterPIDF_D = 0.0;
|
||||
public double shooterPIDF_F = 90;
|
||||
public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
|
||||
public static double shooterPIDF_P = 255;
|
||||
public static double shooterPIDF_I = 0.0;
|
||||
public static double shooterPIDF_D = 0.0;
|
||||
public static double shooterPIDF_F = 75;
|
||||
// public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
|
||||
public DcMotorEx shooter1;
|
||||
public DcMotorEx shooter2;
|
||||
public Servo hood;
|
||||
|
||||
@@ -14,8 +14,6 @@ public class Servos {
|
||||
public static double turrP = 1.1, turrI = 0.25, turrD = 0.0625, turrF = 0;
|
||||
public static double spin_scalar = 1.112;
|
||||
public static double spin_restPos = 0.155;
|
||||
public static double turret_scalar = 1.009;
|
||||
public static double turret_restPos = 0.0;
|
||||
Robot robot;
|
||||
PIDFController spinPID;
|
||||
PIDFController turretPID;
|
||||
@@ -51,14 +49,13 @@ public class Servos {
|
||||
return (Math.abs(pos1 - pos2) < 0.005);
|
||||
}
|
||||
|
||||
public double setTransferPos(double pos) {
|
||||
public void setTransferPos(double pos) {
|
||||
if (firstTransferPos || !servoPosEqual(pos, prevTransferPos)) {
|
||||
robot.transferServo.setPosition(pos);
|
||||
firstTransferPos = false;
|
||||
}
|
||||
|
||||
prevTransferPos = pos;
|
||||
return pos;
|
||||
}
|
||||
|
||||
public double setSpinPos(double pos) {
|
||||
@@ -72,29 +69,16 @@ public class Servos {
|
||||
return pos;
|
||||
}
|
||||
|
||||
public double setHoodPos(double pos){
|
||||
public void setHoodPos(double pos){
|
||||
if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) {
|
||||
robot.hood.setPosition(pos + hoodOffset);
|
||||
firstHoodPos = false;
|
||||
}
|
||||
|
||||
prevHoodPos = pos;
|
||||
return pos;
|
||||
}
|
||||
|
||||
public boolean spinEqual(double pos) {
|
||||
return Math.abs(pos - this.getSpinPos()) < 0.03;
|
||||
}
|
||||
|
||||
public double getTurrPos() {
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
public double setTurrPos(double pos) {
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
public boolean turretEqual(double pos) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -16,6 +16,8 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
|
||||
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.utils.Servos.spinD;
|
||||
import static org.firstinspires.ftc.teamcode.utils.Servos.spinF;
|
||||
import static org.firstinspires.ftc.teamcode.utils.Servos.spinI;
|
||||
@@ -52,6 +54,9 @@ public class Spindexer {
|
||||
private double prevPos = 0.0;
|
||||
public double spindexerPosOffset = 0.00;
|
||||
public static int shootWaitMax = 4;
|
||||
public static boolean whileShooting = false;
|
||||
public static int waitFirstBallTicks = 4;
|
||||
private int shootTicks = 0;
|
||||
public StateEnums.Motif desiredMotif = StateEnums.Motif.NONE;
|
||||
// For Use
|
||||
enum RotatedBallPositionNames {
|
||||
@@ -530,18 +535,26 @@ public class Spindexer {
|
||||
break;
|
||||
|
||||
case SHOOT_PREP_CONTINOUS:
|
||||
if (servos.spinEqual(spinStartPos)){
|
||||
if (shootTicks > waitFirstBallTicks){
|
||||
currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
|
||||
shootTicks++;
|
||||
} else if (servos.spinEqual(spinStartPos)){
|
||||
shootTicks++;
|
||||
servos.setTransferPos(transferServo_in);
|
||||
} else {
|
||||
servos.setSpinPos(spinStartPos);
|
||||
}
|
||||
break;
|
||||
|
||||
case SHOOT_CONTINOUS:
|
||||
whileShooting = true;
|
||||
ballPositions[0].isEmpty = false;
|
||||
ballPositions[1].isEmpty = false;
|
||||
ballPositions[2].isEmpty = false;
|
||||
if (servos.getSpinPos() > spinEndPos){
|
||||
whileShooting = false;
|
||||
servos.setTransferPos(transferServo_out);
|
||||
shootTicks = 0;
|
||||
currentIntakeState = IntakeState.FINDNEXT;
|
||||
} else {
|
||||
double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease;
|
||||
|
||||
@@ -10,11 +10,9 @@ import com.qualcomm.hardware.limelightvision.LLResult;
|
||||
import com.qualcomm.hardware.limelightvision.LLResultTypes;
|
||||
import com.qualcomm.hardware.limelightvision.Limelight3A;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
|
||||
import org.firstinspires.ftc.teamcode.constants.Color;
|
||||
import org.firstinspires.ftc.teamcode.constants.StateEnums;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
@@ -27,20 +25,18 @@ public class Turret {
|
||||
public static double turret180Range = 0.4;
|
||||
public static double turrDefault = 0.37;
|
||||
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 double manualOffset = 0.0;
|
||||
|
||||
public static double visionCorrectionGain = 0.08; // Single tunable gain
|
||||
public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
|
||||
public static double cameraBearingEqual = 0.5; // Deadband
|
||||
// public static double visionCorrectionGain = 0.08; // Single tunable gain
|
||||
// public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
|
||||
// public static double cameraBearingEqual = 0.5; // Deadband
|
||||
|
||||
// TODO: tune these values for limelight
|
||||
|
||||
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.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;
|
||||
MultipleTelemetry TELE;
|
||||
Limelight3A webcam;
|
||||
@@ -49,7 +45,8 @@ public class Turret {
|
||||
double limelightPosX = 0.0;
|
||||
double limelightPosY = 0.0;
|
||||
LLResult result;
|
||||
|
||||
public static double TARGET_POSITION_TOLERANCE = 0.5;
|
||||
public static double COLOR_OK_TOLERANCE = 2;
|
||||
boolean bearingAligned = false;
|
||||
private boolean lockOffset = false;
|
||||
private int obeliskID = 0;
|
||||
@@ -57,12 +54,9 @@ public class Turret {
|
||||
private double currentTrackOffset = 0.0;
|
||||
private double lightColor = Color.LightRed;
|
||||
private int currentTrackCount = 0;
|
||||
private double permanentOffset = 0.0;
|
||||
double permanentOffset = 0.0;
|
||||
private int prevPipeline = -1;
|
||||
private PIDController bearingPID;
|
||||
|
||||
private double prevTurretPos = 0.0;
|
||||
private boolean firstTurretPos = true;
|
||||
PIDController bearingPID;
|
||||
|
||||
public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
|
||||
this.TELE = tele;
|
||||
@@ -168,16 +162,16 @@ public class Turret {
|
||||
/*
|
||||
Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
|
||||
*/
|
||||
|
||||
private double targetTx = 0;
|
||||
public static double alphaTX = 0.5;
|
||||
private double bearingAlign(LLResult llResult) {
|
||||
double bearingOffset = 0.0;
|
||||
double targetTx = llResult.getTx(); // How far left or right the target is (degrees)
|
||||
final double MIN_OFFSET_POWER = 0.15;
|
||||
final double TARGET_POSITION_TOLERANCE = 1.0;
|
||||
// LL has 54.5 degree total Horizontal FOV; very edges are not useful.
|
||||
final double HORIZONTAL_FOV_RANGE = 26.0; // Total usable horizontal degrees from center +/-
|
||||
final double DRIVE_POWER_REDUCTION = 2.0;
|
||||
final double COLOR_OK_TOLERANCE = 2.5;
|
||||
double tx = llResult.getTx(); // How far left or right the target is (degrees)
|
||||
targetTx = (tx*alphaTX)+(targetTx*(1-alphaTX));
|
||||
// final double MIN_OFFSET_POWER = 0.15;
|
||||
// // LL has 54.5 degree total Horizontal FOV; very edges are not useful.
|
||||
// final double HORIZONTAL_FOV_RANGE = 26.0; // Total usable horizontal degrees from center +/-
|
||||
// final double DRIVE_POWER_REDUCTION = 2.0;
|
||||
|
||||
if (abs(targetTx) < TARGET_POSITION_TOLERANCE) {
|
||||
bearingAligned = true;
|
||||
@@ -293,7 +287,7 @@ public class Turret {
|
||||
targetTurretPos = Math.max(turrMin, Math.min(targetTurretPos, turrMax));
|
||||
|
||||
// Interpolate towards target position
|
||||
double currentPos = getTurrPos();
|
||||
// double currentPos = getTurrPos();
|
||||
double turretPos = targetTurretPos;
|
||||
|
||||
if (targetTurretPos == turrMin) {
|
||||
@@ -303,7 +297,9 @@ public class Turret {
|
||||
}
|
||||
|
||||
// Set servo positions
|
||||
setTurret(turretPos + manualOffset);
|
||||
if (!Spindexer.whileShooting || abs(targetTx) > COLOR_OK_TOLERANCE){
|
||||
setTurret(turretPos + manualOffset);
|
||||
}
|
||||
|
||||
|
||||
/* ---------------- TELEMETRY ---------------- */
|
||||
|
||||
Reference in New Issue
Block a user