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

back gate auto is like 90% done and changed some things to reduce warnings

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-17 15:45:28 -06:00
parent 0f556a193f
commit 7161933d06
12 changed files with 75 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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;
@@ -35,22 +41,10 @@ public class Auto_LT_Close extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93;
public static double spindexerSpeedIncrease = 0.014;
// 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;
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))

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

@@ -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;
@@ -70,12 +76,6 @@ public class Auto_LT_Far extends LinearOpMode {
public static double colorSenseTime = 1;
public static double intakeStackTime = 2.5;
public static double intakeGateTime = 2;
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;
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0;

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

@@ -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;
@@ -51,10 +49,8 @@ public class AutoActions{
private int mostGreenSlot = 0;
private double firstSpindexShootPos = spindexer_outtakeBall1;
private boolean shootForward = true;
public static double firstShootTime = 0.3;
public int motif = 0;
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){
this.robot = rob;
@@ -248,7 +244,7 @@ public class AutoActions{
end = prevSpinPos < spinEndPos;
}
if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < waitFirstBallTicks+1)) {
if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks+1)) {
if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) {
servos.setTransferPos(transferServo_out);
@@ -256,10 +252,10 @@ public class AutoActions{
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
if (shootForward && shooterTicker > waitFirstBallTicks) {
Spindexer.whileShooting = true;
if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) {
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else if (shooterTicker > waitFirstBallTicks){
} else if (shooterTicker > Spindexer.waitFirstBallTicks){
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
@@ -269,7 +265,7 @@ public class AutoActions{
} else {
servos.setTransferPos(transferServo_out);
Spindexer.whileShooting = false;
spindexer.resetSpindexer();
spindexer.processIntake();
@@ -416,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);

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

@@ -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 {

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

@@ -5,11 +5,11 @@ 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;
@@ -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;
}

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

@@ -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);
@@ -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();

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

@@ -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,7 +60,7 @@ 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;
@@ -78,7 +77,6 @@ public class Flywheel {
// really should be a running average of the last 5
steady = (Math.abs(commandedVelocity - velo) < 50);
return powPID;
}
public void update()

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

@@ -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;

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

@@ -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;
@@ -35,7 +33,7 @@ public class Robot {
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 double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
public DcMotorEx shooter1;
public DcMotorEx shooter2;
public Servo hood;

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
View File

@@ -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;
}
}

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
View File

@@ -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;

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

@@ -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;
@@ -32,13 +30,11 @@ public class Turret {
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.08, B_PID_I = 0.0, B_PID_D = 0.007;
Robot robot;
@@ -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 ---------------- */