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 @Config
public class ServoPositions { 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_outtakeBall2 = 0.51; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.27; public static double spindexer_outtakeBall1 = 0.32; //0.27; //0.4;
public static double spinStartPos = spindexer_outtakeBall1 - 0.08; public static double spinStartPos = spindexer_outtakeBall3 - 0.1;
public static double transferServo_out = 0.15; public static double transferServo_out = 0.15;

128
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.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; 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_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_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; 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.spinD;
@@ -122,7 +126,8 @@ public class TeleopV3 extends LinearOpMode {
private int tickerA = 1; private int tickerA = 1;
private boolean transferIn = false; private boolean transferIn = false;
boolean turretInterpolate = 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) { public static double velPrediction(double distance) {
if (distance < 30) { if (distance < 30) {
@@ -582,16 +587,14 @@ public class TeleopV3 extends LinearOpMode {
// } // }
if (enableSpindexerManager) { if (enableSpindexerManager) {
if (!shootAll) { spindexer.processIntake();
spindexer.processIntake();
}
// RIGHT_BUMPER // RIGHT_BUMPER
if (gamepad1.right_bumper) { if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
robot.intake.setPower(1); robot.intake.setPower(1);
} else { } else {
robot.intake.setPower(0); robot.intake.setPower(0);
} }
// LEFT_BUMPER // LEFT_BUMPER
@@ -603,37 +606,52 @@ public class TeleopV3 extends LinearOpMode {
shootAll = true; shootAll = true;
shooterTicker = 0; shooterTicker = 0;
spindexPos = spinStartPos;// TODO: Change starting position based on desired order to shoot green ball
} }
intakeTicker++;
if (shootAll) { if (shootAll) {
intakeTicker = 0;
intake = false; intake = false;
reject = false; reject = false;
if (getRuntime() - shootStamp < 3.5) { // if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) {
//
if (shooterTicker == 0 && !servo.spinEqual(spindexPos)){ // if (shooterTicker == 0){
robot.spin1.setPosition(spindexPos); // robot.transferServo.setPosition(transferServo_out);
robot.spin2.setPosition(1-spindexPos); // robot.spin1.setPosition(spinStartPos);
} else { // robot.spin2.setPosition(1-spinStartPos);
robot.transferServo.setPosition(transferServo_in); // if (servo.spinEqual(spinStartPos)){
shooterTicker++; // shooterTicker++;
double prevSpinPos = robot.spin1.getPosition(); // }
robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); // TELE.addLine("starting to shoot");
robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); // } 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);
spindexer.shootAll();
TELE.addLine("starting to shoot");
} else if (!spindexer.shootAllComplete()) {
robot.transferServo.setPosition(transferServo_in);
TELE.addLine("shoot");
} else { } else {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1; //spindexPos = spindexer_intakePos1;
shootAll = false; shootAll = false;
spindexer.resetSpindexer(); spindexer.resetSpindexer();
spindexer.processIntake(); //spindexer.processIntake();
TELE.addLine("stop shooting");
} }
shooterTicker++;
//spindexer.processIntake();
} }
if (gamepad1.left_stick_button){ if (gamepad1.left_stick_button){
@@ -818,42 +836,40 @@ public class TeleopV3 extends LinearOpMode {
hub.clearBulkCache(); hub.clearBulkCache();
} }
// //
TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); // TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); // TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
TELE.addData("Spin3Green", green3 + ": " + ballIn(3)); // TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
//
TELE.addData("pose", drive.localizer.getPose()); // TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); // TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
TELE.addData("distanceToGoal", distanceToGoal); // TELE.addData("distanceToGoal", distanceToGoal);
TELE.addData("hood", robot.hood.getPosition()); // TELE.addData("hood", robot.hood.getPosition());
TELE.addData("targetVel", vel); // TELE.addData("targetVel", vel);
TELE.addData("Velocity", flywheel.getVelo()); // TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Velo1", flywheel.velo1); // TELE.addData("Velo1", flywheel.velo1);
TELE.addData("Velo2", flywheel.velo2); // TELE.addData("Velo2", flywheel.velo2);
TELE.addData("shootOrder", shootOrder); // TELE.addData("shootOrder", shootOrder);
TELE.addData("oddColor", oddBallColor); // TELE.addData("oddColor", oddBallColor);
//
// Spindexer Debug // // Spindexer Debug
TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1)); TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1));
TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition); TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition);
TELE.addData("spinIntakeState", spindexer.currentIntakeState); TELE.addData("spinIntakeState", spindexer.currentIntakeState);
TELE.addData("spinTestCounter", spindexer.counter); TELE.addData("spinTestCounter", spindexer.counter);
TELE.addData("autoSpintake", autoSpintake); TELE.addData("autoSpintake", autoSpintake);
//TELE.addData("distanceRearCenter", spindexer.distanceRearCenter); //
//TELE.addData("distanceFrontDriver", spindexer.distanceFrontDriver); // TELE.addData("shootall commanded", shootAll);
//TELE.addData("distanceFrontPassenger", spindexer.distanceFrontPassenger); // // Targeting Debug
TELE.addData("shootall commanded", shootAll); // TELE.addData("robotX", robotX);
// Targeting Debug // TELE.addData("robotY", robotY);
TELE.addData("robotX", robotX); // TELE.addData("robotInchesX", targeting.robotInchesX);
TELE.addData("robotY", robotY); // TELE.addData( "robotInchesY", targeting.robotInchesY);
TELE.addData("robotInchesX", targeting.robotInchesX); // TELE.addData("Targeting Interpolate", turretInterpolate);
TELE.addData( "robotInchesY", targeting.robotInchesY); // TELE.addData("Targeting GridX", targeting.robotGridX);
TELE.addData("Targeting Interpolate", turretInterpolate); // TELE.addData("Targeting GridY", targeting.robotGridY);
TELE.addData("Targeting GridX", targeting.robotGridX); // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
TELE.addData("Targeting GridY", targeting.robotGridY); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.update(); 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 distanceFrontDriver = 0.0;
public double distanceFrontPassenger = 0.0; public double distanceFrontPassenger = 0.0;
private double prevPos = 0.0;
public Types.Motif desiredMotif = Types.Motif.NONE; public Types.Motif desiredMotif = Types.Motif.NONE;
// For Use // For Use
enum RotatedBallPositionNames { enum RotatedBallPositionNames {
@@ -70,7 +72,10 @@ public class Spindexer {
SHOOT_ALL_READY SHOOT_ALL_READY
} }
int shootWaitCount = 0;
public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
public int unknownColorDetect = 0; public int unknownColorDetect = 0;
enum BallColor { enum BallColor {
UNKNOWN, UNKNOWN,
@@ -246,10 +251,21 @@ public class Spindexer {
return newPos1Detection; return newPos1Detection;
} }
// Has code to unjam spindexer
public void moveSpindexerToPos(double pos) { private void moveSpindexerToPos(double pos) {
robot.spin1.setPosition(pos); robot.spin1.setPosition(pos);
robot.spin2.setPosition(1-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() { public void stopSpindexer() {
@@ -278,6 +294,10 @@ public class Spindexer {
} }
} }
public boolean slotIsEmpty(int slot){
return !ballPositions[slot].isEmpty;
}
public boolean isFull () { public boolean isFull () {
return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty); return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty);
} }
@@ -287,7 +307,7 @@ public class Spindexer {
case UNKNOWN_START: case UNKNOWN_START:
// For now just set position ONE if UNKNOWN // For now just set position ONE if UNKNOWN
commandedIntakePosition = 0; commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[0]); moveSpindexerToPos(intakePositions[0]);
currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE; currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE;
break; break;
case UNKNOWN_MOVE: case UNKNOWN_MOVE:
@@ -323,27 +343,24 @@ public class Spindexer {
// Find Next Open Position and start movement // Find Next Open Position and start movement
double currentSpindexerPos = servos.getSpinPos(); double currentSpindexerPos = servos.getSpinPos();
double commandedtravelDistance = 2.0; 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 && (proposedTravelDistance < commandedtravelDistance)) {
if (ballPositions[0].isEmpty) { if (ballPositions[0].isEmpty) {
// Position 1 // Position 1
commandedIntakePosition = 0; commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING; 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 && (proposedTravelDistance < commandedtravelDistance)) {
if (ballPositions[1].isEmpty) { if (ballPositions[1].isEmpty) {
// Position 2 // Position 2
commandedIntakePosition = 1; commandedIntakePosition = 1;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING; currentIntakeState = Spindexer.IntakeState.MOVING;
} }
//proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos); proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
if (ballPositions[2].isEmpty) { if (ballPositions[2].isEmpty) {
// Position 3 // Position 3
commandedIntakePosition = 2; commandedIntakePosition = 2;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING; currentIntakeState = Spindexer.IntakeState.MOVING;
} }
if (currentIntakeState != Spindexer.IntakeState.MOVING) { if (currentIntakeState != Spindexer.IntakeState.MOVING) {
@@ -380,47 +397,42 @@ public class Spindexer {
case SHOOT_ALL_PREP: case SHOOT_ALL_PREP:
// We get here with function call to prepareToShootMotif // We get here with function call to prepareToShootMotif
// Stopping when we get to the new position // Stopping when we get to the new position
if (servos.spinEqual(intakePositions[commandedIntakePosition])) { if (!servos.spinEqual(outakePositions[commandedIntakePosition])) {
currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_READY;
} else {
// Keep moving the spindexer // 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; break;
case SHOOT_ALL_READY: case SHOOT_ALL_READY: // Not used
// Double Check Colors // Double Check Colors
//detectBalls(false, false); // Minimize hardware calls //detectBalls(false, false); // Minimize hardware calls
if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) { if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) {
// All ball shot move to intake state // All ball shot move to intake state
currentIntakeState = Spindexer.IntakeState.FINDNEXT; currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
} }
// Maintain Position // Maintain Position
moveSpindexerToPos(intakePositions[commandedIntakePosition]); moveSpindexerToPos(outakePositions[commandedIntakePosition]);
break; break;
case SHOOTNEXT: case SHOOTNEXT:
// Find Next Open Position and start movement // Find Next Open Position and start movement
if (!ballPositions[0].isEmpty) { if (!ballPositions[1].isEmpty) {
// Position 1 // 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; commandedIntakePosition = 1;
servos.setSpinPos(outakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else if (ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty? } else if (!ballPositions[2].isEmpty) {
// Position 3 // Position 2
commandedIntakePosition = 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; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else { } else {
// Empty return to intake state // Empty return to intake state
currentIntakeState = IntakeState.FINDNEXT; currentIntakeState = IntakeState.FINDNEXT;
} }
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" moveSpindexerToPos(outakePositions[commandedIntakePosition]);
break; break;
case SHOOTMOVING: case SHOOTMOVING:
@@ -429,25 +441,34 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
} else { } else {
// Keep moving the spindexer // Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" moveSpindexerToPos(outakePositions[commandedIntakePosition]);
} }
break; break;
case SHOOTWAIT: case SHOOTWAIT:
// Stopping when we get to the new position // Stopping when we get to the new position
if (servos.spinEqual(intakePositions[commandedIntakePosition])) { if (prevIntakeState != currentIntakeState) {
currentIntakeState = Spindexer.IntakeState.INTAKE; shootWaitCount = 0;
stopSpindexer();
//detectBalls(true, false);
} else { } else {
// Keep moving the spindexer shootWaitCount++;
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
} }
// 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; break;
default: default:
// Statements to execute if no case matches // Statements to execute if no case matches
} }
prevIntakeState = currentIntakeState;
//TELE.addData("commandedIntakePosition", commandedIntakePosition); //TELE.addData("commandedIntakePosition", commandedIntakePosition);
//TELE.update(); //TELE.update();
// Signal a successful intake // Signal a successful intake
@@ -499,6 +520,22 @@ public class Spindexer {
commandedIntakePosition = bestFitMotif(); 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 () { void shootAllToIntake () {
currentIntakeState = Spindexer.IntakeState.FINDNEXT; 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][4] = new Settings (3100.0, 0.47);
KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47); KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47);
// ROW 4 // ROW 4
KNOWNTARGETING[4][0] = new Settings (4540.0, 0.1); KNOWNTARGETING[4][0] = new Settings (3100, 0.49);
KNOWNTARGETING[4][1] = new Settings (4541.0, 0.1); KNOWNTARGETING[4][1] = new Settings (3100, 0.49);
KNOWNTARGETING[4][2] = new Settings (4542.0, 0.1); KNOWNTARGETING[4][2] = new Settings (3100, 0.5);
KNOWNTARGETING[4][3] = new Settings (4543.0, 0.1); KNOWNTARGETING[4][3] = new Settings (3200, 0.5);
KNOWNTARGETING[4][4] = new Settings (4544.0, 0.1); KNOWNTARGETING[4][4] = new Settings (3250, 0.49);
KNOWNTARGETING[4][5] = new Settings (4545.0, 0.1); KNOWNTARGETING[4][5] = new Settings (3300, 0.49);
// ROW 1 // ROW 5
KNOWNTARGETING[5][0] = new Settings (4550.0, 0.1); KNOWNTARGETING[5][0] = new Settings (3200, 0.48);
KNOWNTARGETING[5][1] = new Settings (4551.0, 0.1); KNOWNTARGETING[5][1] = new Settings (3200, 0.48);
KNOWNTARGETING[5][2] = new Settings (4552.0, 0.1); KNOWNTARGETING[5][2] = new Settings (3300, 0.48);
KNOWNTARGETING[5][3] = new Settings (4553.0, 0.1); KNOWNTARGETING[5][3] = new Settings (3350, 0.48);
KNOWNTARGETING[5][4] = new Settings (4554.0, 0.1); KNOWNTARGETING[5][4] = new Settings (3350, 0.48);
KNOWNTARGETING[5][5] = new Settings (4555.0, 0.1); KNOWNTARGETING[5][5] = new Settings (3350, 0.48);
} }
public Targeting() public Targeting()
@@ -107,61 +107,81 @@ public class Targeting {
// Determine if we need to interpolate based on tile position. // Determine if we need to interpolate based on tile position.
// if near upper or lower quarter or tile interpolate with next tile. // if near upper or lower quarter or tile interpolate with next tile.
int x0 = 0;
int y0 = 0;
int x1 = 0; int x1 = 0;
int y1 = 0; int y1 = 0;
// interpolate = false; interpolate = false;
// if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
// (robotGridX < 5) && (robotGridY <5)) { (robotGridX < 5) && (robotGridY <5)) {
// // +X, +Y // +X, +Y
// interpolate = true; interpolate = true;
// x1 = robotGridX + 1; x0 = robotGridX;
// y1 = robotGridY + 1; x1 = robotGridX + 1;
// } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && y0 = robotGridY;
// (robotGridX > 0) && (robotGridY > 0)) { y1 = robotGridY + 1;
// // -X, -Y } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
// interpolate = true; (robotGridX > 0) && (robotGridY > 0)) {
// x1 = robotGridX - 1; // -X, -Y
// y1 = robotGridY - 1; interpolate = true;
// } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && x0 = robotGridX - 1;
// (robotGridX < 5) && (robotGridY > 0)) { x1 = robotGridX;
// // +X, -Y y0 = robotGridY - 1;
// interpolate = true; y1 = robotGridY;
// x1 = robotGridX + 1; } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
// y1 = robotGridY - 1; (robotGridX < 5) && (robotGridY > 0)) {
// } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && // +X, -Y
// (robotGridX > 0) && (robotGridY < 5)) { interpolate = true;
// // -X, +Y x0 = robotGridX;
// interpolate = true; x1 = robotGridX + 1;
// x1 = robotGridX - 1; y0 = robotGridY - 1;
// y1 = robotGridY + 1; y1 = robotGridY;
// } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) { } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
// // -X, Y (robotGridX > 0) && (robotGridY < 5)) {
// interpolate = true; // -X, +Y
// x1 = robotGridX - 1; interpolate = true;
// y1 = robotGridY; x0 = robotGridX - 1;
// } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) { x1 = robotGridX;
// // X, -Y y0 = robotGridY;
// interpolate = true; y1 = robotGridY + 1;
// x1 = robotGridX; } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) {
// y1 = robotGridY - 1; // -X, Y
// } else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) { interpolate = true;
// // +X, Y x0 = robotGridX - 1;
// interpolate = true; x1 = robotGridX;
// x1 = robotGridX + 1; y0 = robotGridY;
// y1 = robotGridY; y1 = robotGridY;
// } else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) { } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) {
// // X, +Y // X, -Y
// interpolate = true; interpolate = true;
// x1 = robotGridX; x0 = robotGridX;
// y1 = robotGridY + 1; 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 //clamp
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
// basic search // basic search
if(!interpolate) { if(true) { //!interpolate) {
if ((robotGridY < 6) && (robotGridX <6)) { if ((robotGridY < 6) && (robotGridX <6)) {
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM; recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle; recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
@@ -170,27 +190,29 @@ public class Targeting {
} else { } else {
// bilinear interpolation // bilinear interpolation
int x0 = robotGridX; //int x0 = robotGridX;
//int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1); //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); //int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1);
double x = (robotInchesX - (x0 * tileSize)) / tileSize; // double x = (robotInchesX - (x0 * tileSize)) / tileSize;
double y = (robotInchesY - (y0 * tileSize)) / tileSize; // double y = (robotInchesY - (y0 * tileSize)) / tileSize;
double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM; // double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM;
double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM; // double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM;
double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM; // double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM;
double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM; // double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM;
//
double angle00 = KNOWNTARGETING[y0][x0].hoodAngle; // double angle00 = KNOWNTARGETING[y0][x0].hoodAngle;
double angle10 = KNOWNTARGETING[y0][x1].hoodAngle; // double angle10 = KNOWNTARGETING[y0][x1].hoodAngle;
double angle01 = KNOWNTARGETING[y1][x0].hoodAngle; // double angle01 = KNOWNTARGETING[y1][x0].hoodAngle;
double angle11 = KNOWNTARGETING[y1][x1].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;
// 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; 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() { public double getTy() {
limelightRead();
return ty; return ty;
} }
public double getLimelightX() { public double getLimelightX() {
limelightRead();
return limelightPosX; return limelightPosX;
} }
public double getLimelightY() { public double getLimelightY() {
limelightRead();
return limelightPosY; return limelightPosY;
} }

8
build.dependencies.gradle
View File

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