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

Merge branch 'SpindexerUpgradesInWork' into auto9Ball

Browse Source 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
#	build.dependencies.gradle
This commit is contained in:
DanTheMan-byte
2026-01-28 19:43:20 -06:00
parent 66bb5c747f 159b130b5f
commit 7ae7574703
6 changed files with 252 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,17 +5,17 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class ServoPositions {
public static double spindexer_intakePos1 = 0;
public static double spindexer_intakePos1 = 0.05; //0.13;
public static double spindexer_intakePos2 = 0.19;//0.5;
public static double spindexer_intakePos2 = 0.24; //0.33;//0.5;
public static double spindexer_intakePos3 = 0.38;//0.66;
public static double spindexer_intakePos3 = 0.43; //0.53;//0.66;
public static double spindexer_outtakeBall3 = 0.65;
public static double spindexer_outtakeBall3 = 0.70; //0.65; //0.24;
public static double spindexer_outtakeBall2 = 0.46;
public static double spindexer_outtakeBall1 = 0.27;
public static double spinStartPos = spindexer_outtakeBall1 - 0.08;
public static double spindexer_outtakeBall2 = 0.51; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.32; //0.27; //0.4;
public static double spinStartPos = spindexer_outtakeBall3 - 0.1;
public static double transferServo_out = 0.15;

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

@@ -3,6 +3,10 @@ package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.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_intakePos2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos3;
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;
@@ -122,7 +126,8 @@ public class TeleopV3 extends LinearOpMode {
private int tickerA = 1;
private boolean transferIn = false;
boolean turretInterpolate = false;
public static double spinSpeedIncrease = 0.04;
public static double spinSpeedIncrease = 0.03;
public static int resetSpinTicks = 4;
public static double velPrediction(double distance) {
if (distance < 30) {
@@ -582,16 +587,14 @@ public class TeleopV3 extends LinearOpMode {
// }
if (enableSpindexerManager) {
if (!shootAll) {
spindexer.processIntake();
}
// RIGHT_BUMPER
if (gamepad1.right_bumper) {
if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
robot.intake.setPower(1);
} else {
robot.intake.setPower(0);
}
// LEFT_BUMPER
@@ -603,37 +606,52 @@ public class TeleopV3 extends LinearOpMode {
shootAll = true;
shooterTicker = 0;
spindexPos = spinStartPos;// TODO: Change starting position based on desired order to shoot green ball
}
intakeTicker++;
if (shootAll) {
intakeTicker = 0;
intake = false;
reject = false;
if (getRuntime() - shootStamp < 3.5) {
if (shooterTicker == 0 && !servo.spinEqual(spindexPos)){
robot.spin1.setPosition(spindexPos);
robot.spin2.setPosition(1-spindexPos);
} else {
// if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) {
//
// if (shooterTicker == 0){
// robot.transferServo.setPosition(transferServo_out);
// robot.spin1.setPosition(spinStartPos);
// robot.spin2.setPosition(1-spinStartPos);
// if (servo.spinEqual(spinStartPos)){
// shooterTicker++;
// }
// TELE.addLine("starting to shoot");
// } else {
// robot.transferServo.setPosition(transferServo_in);
// shooterTicker++;
// double prevSpinPos = servo.getSpinPos();
// robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease);
// robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
// TELE.addLine("shooting");
// }
if (shooterTicker == 0) {
spindexer.prepareShootAll();
TELE.addLine("preparing to shoot");
} else if (shooterTicker == 2) {
robot.transferServo.setPosition(transferServo_in);
shooterTicker++;
double prevSpinPos = robot.spin1.getPosition();
robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease);
robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
}
spindexer.shootAll();
TELE.addLine("starting to shoot");
} else if (!spindexer.shootAllComplete()) {
robot.transferServo.setPosition(transferServo_in);
TELE.addLine("shoot");
} else {
robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1;
shootAll = false;
spindexer.resetSpindexer();
spindexer.processIntake();
//spindexer.processIntake();
TELE.addLine("stop shooting");
}
shooterTicker++;
//spindexer.processIntake();
}
if (gamepad1.left_stick_button){
@@ -818,42 +836,40 @@ public class TeleopV3 extends LinearOpMode {
hub.clearBulkCache();
}
//
TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
TELE.addData("distanceToGoal", distanceToGoal);
TELE.addData("hood", robot.hood.getPosition());
TELE.addData("targetVel", vel);
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Velo1", flywheel.velo1);
TELE.addData("Velo2", flywheel.velo2);
TELE.addData("shootOrder", shootOrder);
TELE.addData("oddColor", oddBallColor);
// Spindexer Debug
// TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
// TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
// TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
//
// TELE.addData("pose", drive.localizer.getPose());
// TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
// TELE.addData("distanceToGoal", distanceToGoal);
// TELE.addData("hood", robot.hood.getPosition());
// TELE.addData("targetVel", vel);
// TELE.addData("Velocity", flywheel.getVelo());
// TELE.addData("Velo1", flywheel.velo1);
// TELE.addData("Velo2", flywheel.velo2);
// TELE.addData("shootOrder", shootOrder);
// TELE.addData("oddColor", oddBallColor);
//
// // Spindexer Debug
TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1));
TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition);
TELE.addData("spinIntakeState", spindexer.currentIntakeState);
TELE.addData("spinTestCounter", spindexer.counter);
TELE.addData("autoSpintake", autoSpintake);
//TELE.addData("distanceRearCenter", spindexer.distanceRearCenter);
//TELE.addData("distanceFrontDriver", spindexer.distanceFrontDriver);
//TELE.addData("distanceFrontPassenger", spindexer.distanceFrontPassenger);
TELE.addData("shootall commanded", shootAll);
// Targeting Debug
TELE.addData("robotX", robotX);
TELE.addData("robotY", robotY);
TELE.addData("robotInchesX", targeting.robotInchesX);
TELE.addData( "robotInchesY", targeting.robotInchesY);
TELE.addData("Targeting Interpolate", turretInterpolate);
TELE.addData("Targeting GridX", targeting.robotGridX);
TELE.addData("Targeting GridY", targeting.robotGridY);
TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
//
// TELE.addData("shootall commanded", shootAll);
// // Targeting Debug
// TELE.addData("robotX", robotX);
// TELE.addData("robotY", robotY);
// TELE.addData("robotInchesX", targeting.robotInchesX);
// TELE.addData( "robotInchesY", targeting.robotInchesY);
// TELE.addData("Targeting Interpolate", turretInterpolate);
// TELE.addData("Targeting GridX", targeting.robotGridX);
// TELE.addData("Targeting GridY", targeting.robotGridY);
// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.update();

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

@@ -39,6 +39,8 @@ public class Spindexer {
public double distanceFrontDriver = 0.0;
public double distanceFrontPassenger = 0.0;
private double prevPos = 0.0;
public Types.Motif desiredMotif = Types.Motif.NONE;
// For Use
enum RotatedBallPositionNames {
@@ -70,7 +72,10 @@ public class Spindexer {
SHOOT_ALL_READY
}
int shootWaitCount = 0;
public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
public int unknownColorDetect = 0;
enum BallColor {
UNKNOWN,
@@ -246,10 +251,21 @@ public class Spindexer {
return newPos1Detection;
}
public void moveSpindexerToPos(double pos) {
// Has code to unjam spindexer
private void moveSpindexerToPos(double pos) {
robot.spin1.setPosition(pos);
robot.spin2.setPosition(1-pos);
// double currentPos = servos.getSpinPos();
// if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
// if (currentPos > pos){
// robot.spin1.setPosition(servos.getSpinPos() + 0.05);
// robot.spin2.setPosition(1 - servos.getSpinPos() - 0.05);
// } else {
// robot.spin1.setPosition(servos.getSpinPos() - 0.05);
// robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05);
// }
// }
// prevPos = currentPos;
}
public void stopSpindexer() {
@@ -278,6 +294,10 @@ public class Spindexer {
}
}
public boolean slotIsEmpty(int slot){
return !ballPositions[slot].isEmpty;
}
public boolean isFull () {
return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty);
}
@@ -287,7 +307,7 @@ public class Spindexer {
case UNKNOWN_START:
// For now just set position ONE if UNKNOWN
commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[0]);
moveSpindexerToPos(intakePositions[0]);
currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE;
break;
case UNKNOWN_MOVE:
@@ -323,27 +343,24 @@ public class Spindexer {
// Find Next Open Position and start movement
double currentSpindexerPos = servos.getSpinPos();
double commandedtravelDistance = 2.0;
//double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos);
double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos);
//if (ballPositions[0].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
if (ballPositions[0].isEmpty) {
// Position 1
commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING;
}
//proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
//if (ballPositions[1].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
if (ballPositions[1].isEmpty) {
// Position 2
commandedIntakePosition = 1;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING;
}
//proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
if (ballPositions[2].isEmpty) {
// Position 3
commandedIntakePosition = 2;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING;
}
if (currentIntakeState != Spindexer.IntakeState.MOVING) {
@@ -380,47 +397,42 @@ public class Spindexer {
case SHOOT_ALL_PREP:
// We get here with function call to prepareToShootMotif
// Stopping when we get to the new position
if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_READY;
} else {
if (!servos.spinEqual(outakePositions[commandedIntakePosition])) {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
}
break;
case SHOOT_ALL_READY:
case SHOOT_ALL_READY: // Not used
// Double Check Colors
//detectBalls(false, false); // Minimize hardware calls
if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) {
// All ball shot move to intake state
currentIntakeState = Spindexer.IntakeState.FINDNEXT;
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
}
// Maintain Position
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
break;
case SHOOTNEXT:
// Find Next Open Position and start movement
if (!ballPositions[0].isEmpty) {
if (!ballPositions[1].isEmpty) {
// Position 1
commandedIntakePosition = 0;
servos.setSpinPos(outakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else if (ballPositions[1].isEmpty) { // Possible error: should it be !ballPosition[1].isEmpty?
// Position 2
commandedIntakePosition = 1;
servos.setSpinPos(outakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else if (ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty?
// Position 3
} else if (!ballPositions[2].isEmpty) {
// Position 2
commandedIntakePosition = 2;
servos.setSpinPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else if (!ballPositions[0].isEmpty) {
// Position 3
commandedIntakePosition = 0;
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else {
// Empty return to intake state
currentIntakeState = IntakeState.FINDNEXT;
}
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
break;
case SHOOTMOVING:
@@ -429,25 +441,34 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
}
break;
case SHOOTWAIT:
// Stopping when we get to the new position
if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
currentIntakeState = Spindexer.IntakeState.INTAKE;
stopSpindexer();
//detectBalls(true, false);
if (prevIntakeState != currentIntakeState) {
shootWaitCount = 0;
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
shootWaitCount++;
}
// wait 10 cycles
if (shootWaitCount > 2) {
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
ballPositions[commandedIntakePosition].isEmpty = true;
shootWaitCount = 0;
//stopSpindexer();
//detectBalls(true, false);
}
// Keep moving the spindexer
moveSpindexerToPos(outakePositions[commandedIntakePosition]+(shootWaitCount*0.01));
break;
default:
// Statements to execute if no case matches
}
prevIntakeState = currentIntakeState;
//TELE.addData("commandedIntakePosition", commandedIntakePosition);
//TELE.update();
// Signal a successful intake
@@ -499,6 +520,22 @@ public class Spindexer {
commandedIntakePosition = bestFitMotif();
}
public void prepareShootAll(){
currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP;
}
public void shootAll () {
currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
}
public boolean shootAllComplete ()
{
return ((currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_PREP) &&
(currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) &&
(currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) &&
(currentIntakeState != Spindexer.IntakeState.SHOOTNEXT) &&
(currentIntakeState != Spindexer.IntakeState.SHOOTWAIT));
}
void shootAllToIntake () {
currentIntakeState = Spindexer.IntakeState.FINDNEXT;
}

174
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java
View File

@@ -67,19 +67,19 @@ public class Targeting {
KNOWNTARGETING[3][4] = new Settings (3100.0, 0.47);
KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47);
// ROW 4
KNOWNTARGETING[4][0] = new Settings (4540.0, 0.1);
KNOWNTARGETING[4][1] = new Settings (4541.0, 0.1);
KNOWNTARGETING[4][2] = new Settings (4542.0, 0.1);
KNOWNTARGETING[4][3] = new Settings (4543.0, 0.1);
KNOWNTARGETING[4][4] = new Settings (4544.0, 0.1);
KNOWNTARGETING[4][5] = new Settings (4545.0, 0.1);
// ROW 1
KNOWNTARGETING[5][0] = new Settings (4550.0, 0.1);
KNOWNTARGETING[5][1] = new Settings (4551.0, 0.1);
KNOWNTARGETING[5][2] = new Settings (4552.0, 0.1);
KNOWNTARGETING[5][3] = new Settings (4553.0, 0.1);
KNOWNTARGETING[5][4] = new Settings (4554.0, 0.1);
KNOWNTARGETING[5][5] = new Settings (4555.0, 0.1);
KNOWNTARGETING[4][0] = new Settings (3100, 0.49);
KNOWNTARGETING[4][1] = new Settings (3100, 0.49);
KNOWNTARGETING[4][2] = new Settings (3100, 0.5);
KNOWNTARGETING[4][3] = new Settings (3200, 0.5);
KNOWNTARGETING[4][4] = new Settings (3250, 0.49);
KNOWNTARGETING[4][5] = new Settings (3300, 0.49);
// ROW 5
KNOWNTARGETING[5][0] = new Settings (3200, 0.48);
KNOWNTARGETING[5][1] = new Settings (3200, 0.48);
KNOWNTARGETING[5][2] = new Settings (3300, 0.48);
KNOWNTARGETING[5][3] = new Settings (3350, 0.48);
KNOWNTARGETING[5][4] = new Settings (3350, 0.48);
KNOWNTARGETING[5][5] = new Settings (3350, 0.48);
}
public Targeting()
@@ -107,61 +107,81 @@ public class Targeting {
// Determine if we need to interpolate based on tile position.
// if near upper or lower quarter or tile interpolate with next tile.
int x0 = 0;
int y0 = 0;
int x1 = 0;
int y1 = 0;
// interpolate = false;
// if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
// (robotGridX < 5) && (robotGridY <5)) {
// // +X, +Y
// interpolate = true;
// x1 = robotGridX + 1;
// y1 = robotGridY + 1;
// } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
// (robotGridX > 0) && (robotGridY > 0)) {
// // -X, -Y
// interpolate = true;
// x1 = robotGridX - 1;
// y1 = robotGridY - 1;
// } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
// (robotGridX < 5) && (robotGridY > 0)) {
// // +X, -Y
// interpolate = true;
// x1 = robotGridX + 1;
// y1 = robotGridY - 1;
// } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
// (robotGridX > 0) && (robotGridY < 5)) {
// // -X, +Y
// interpolate = true;
// x1 = robotGridX - 1;
// y1 = robotGridY + 1;
// } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) {
// // -X, Y
// interpolate = true;
// x1 = robotGridX - 1;
// y1 = robotGridY;
// } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) {
// // X, -Y
// interpolate = true;
// x1 = robotGridX;
// y1 = robotGridY - 1;
// } else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) {
// // +X, Y
// interpolate = true;
// x1 = robotGridX + 1;
// y1 = robotGridY;
// } else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) {
// // X, +Y
// interpolate = true;
// x1 = robotGridX;
// y1 = robotGridY + 1;
// }
interpolate = false;
if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
(robotGridX < 5) && (robotGridY <5)) {
// +X, +Y
interpolate = true;
x0 = robotGridX;
x1 = robotGridX + 1;
y0 = robotGridY;
y1 = robotGridY + 1;
} else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
(robotGridX > 0) && (robotGridY > 0)) {
// -X, -Y
interpolate = true;
x0 = robotGridX - 1;
x1 = robotGridX;
y0 = robotGridY - 1;
y1 = robotGridY;
} else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
(robotGridX < 5) && (robotGridY > 0)) {
// +X, -Y
interpolate = true;
x0 = robotGridX;
x1 = robotGridX + 1;
y0 = robotGridY - 1;
y1 = robotGridY;
} else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
(robotGridX > 0) && (robotGridY < 5)) {
// -X, +Y
interpolate = true;
x0 = robotGridX - 1;
x1 = robotGridX;
y0 = robotGridY;
y1 = robotGridY + 1;
} else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) {
// -X, Y
interpolate = true;
x0 = robotGridX - 1;
x1 = robotGridX;
y0 = robotGridY;
y1 = robotGridY;
} else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) {
// X, -Y
interpolate = true;
x0 = robotGridX;
x1 = robotGridX;
y0 = robotGridY - 1;
y1 = robotGridY;
} else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) {
// +X, Y
interpolate = true;
x0 = robotGridX;
x1 = robotGridX + 1;
y0 = robotGridY;
y1 = robotGridY;
} else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) {
// X, +Y
interpolate = true;
x0 = robotGridX;
x1 = robotGridX;
y0 = robotGridY;
y1 = robotGridY + 1;
} else {
interpolate = false;
}
//clamp
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
// basic search
if(!interpolate) {
if(true) { //!interpolate) {
if ((robotGridY < 6) && (robotGridX <6)) {
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
@@ -170,27 +190,29 @@ public class Targeting {
} else {
// bilinear interpolation
int x0 = robotGridX;
//int x0 = robotGridX;
//int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1);
int y0 = robotGridY;
//int y0 = robotGridY;
//int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1);
double x = (robotInchesX - (x0 * tileSize)) / tileSize;
double y = (robotInchesY - (y0 * tileSize)) / tileSize;
// double x = (robotInchesX - (x0 * tileSize)) / tileSize;
// double y = (robotInchesY - (y0 * tileSize)) / tileSize;
double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM;
double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM;
double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM;
double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM;
double angle00 = KNOWNTARGETING[y0][x0].hoodAngle;
double angle10 = KNOWNTARGETING[y0][x1].hoodAngle;
double angle01 = KNOWNTARGETING[y1][x0].hoodAngle;
double angle11 = KNOWNTARGETING[y1][x1].hoodAngle;
recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11;
recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11;
// double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM;
// double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM;
// double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM;
// double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM;
//
// double angle00 = KNOWNTARGETING[y0][x0].hoodAngle;
// double angle10 = KNOWNTARGETING[y0][x1].hoodAngle;
// double angle01 = KNOWNTARGETING[y1][x0].hoodAngle;
// double angle11 = KNOWNTARGETING[y1][x1].hoodAngle;
// recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11;
// recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11;
// Average target tiles
recommendedSettings.flywheelRPM = (KNOWNTARGETING[x0][y0].flywheelRPM + KNOWNTARGETING[x1][y1].flywheelRPM)/2.0;
recommendedSettings.hoodAngle = (KNOWNTARGETING[x0][y0].hoodAngle + KNOWNTARGETING[x1][y1].hoodAngle)/2.0;
return recommendedSettings;
}
}

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

@@ -119,17 +119,14 @@ public class Turret {
}
public double getTy() {
limelightRead();
return ty;
}
public double getLimelightX() {
limelightRead();
return limelightPosX;
}
public double getLimelightY() {
limelightRead();
return limelightPosY;
}

8
build.dependencies.gradle
View File

@@ -26,10 +26,10 @@ dependencies {
implementation 'com.bylazar:fullpanels:1.0.2' //Panels
implementation "com.acmerobotics.roadrunner:ftc:0.1.25"
implementation "com.acmerobotics.roadrunner:core:1.0.1"
implementation "com.acmerobotics.roadrunner:actions:1.0.1"
implementation "com.acmerobotics.dashboard:dashboard:0.5.1"
implementation "com.acmerobotics.roadrunner:ftc:0.1.25" //RR
implementation "com.acmerobotics.roadrunner:core:1.0.1" //RR
implementation "com.acmerobotics.roadrunner:actions:1.0.1" //RR
implementation "com.acmerobotics.dashboard:dashboard:0.5.1" //FTC Dash
implementation 'org.ftclib.ftclib:core:2.1.1' // FTC LIB