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

Improve Spindexer shaking. Upgrade shooterTest to control the spindexer and fix flywheel real time pidf coef updates..

Browse Source
This commit is contained in:
Matt9150
2026-01-25 16:48:27 -06:00
parent 8dc03adfd3
commit d216ce78fc
6 changed files with 76 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
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.19;
public static double spindexer_intakePos1 = 0.18;
public static double spindexer_intakePos2 = 0.35;//0.5;
public static double spindexer_intakePos2 = 0.36;//0.5;
public static double spindexer_intakePos3 = 0.51;//0.66;
public static double spindexer_intakePos3 = 0.54;//0.66;
public static double spindexer_outtakeBall3 = 0.47;

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

@@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
@@ -12,12 +13,13 @@ import com.qualcomm.robotcore.hardware.DcMotorEx;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
@Config
@TeleOp
public class ShooterTest extends LinearOpMode {
public static int mode = 0;
public static int mode = 1;
public static double parameter = 0.0;
// --- CONSTANTS YOU TUNE ---
@@ -31,9 +33,18 @@ public class ShooterTest extends LinearOpMode {
public static double hoodPos = 0.501;
public static double turretPos = 0.501;
public static boolean shoot = false;
public static boolean intake = false;
Robot robot;
Flywheel flywheel;
double shootStamp = 0.0;
boolean shootAll = false;
public double spinPow = 0.09;
Spindexer spindexer ;
@Override
public void runOpMode() throws InterruptedException {
@@ -41,6 +52,7 @@ public class ShooterTest extends LinearOpMode {
DcMotorEx leftShooter = robot.shooter1;
DcMotorEx rightShooter = robot.shooter2;
flywheel = new Flywheel(hardwareMap);
spindexer = new Spindexer(hardwareMap);
MultipleTelemetry TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -64,12 +76,53 @@ public class ShooterTest extends LinearOpMode {
robot.hood.setPosition(hoodPos);
}
robot.transfer.setPower(transferPower);
if (intake) {
robot.intake.setPower(1);
} else {
robot.intake.setPower(0);
}
if (shoot) {
shootStamp = getRuntime();
shootAll = true;
shoot = false;
robot.transfer.setPower(transferPower);
}
if (shootAll) {
//intake = false;
//reject = false;
// TODO: Change starting position based on desired order to shoot green ball
//spindexPos = spindexer_intakePos1;
if (getRuntime() - shootStamp < 3.5) {
robot.transferServo.setPosition(transferServo_in);
robot.spin1.setPower(-spinPow);
robot.spin2.setPower(spinPow);
} else {
robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1;
shootAll = false;
robot.transferServo.setPosition(transferServo_out);
robot.transfer.setPower(0);
robot.spin1.setPower(0);
robot.spin2.setPower(0);
spindexer.resetSpindexer();
spindexer.processIntake();
}
} else {
spindexer.processIntake();
}
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Velocity 1", flywheel.getVelo1());
TELE.addData("Velocity 2", flywheel.getVelo2());

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

@@ -38,10 +38,14 @@ public class Flywheel {
// Set the robot PIDF for the next cycle.
public void setPIDF(double p, double i, double d, double f) {
robot.shooterPIDF.p = p;
robot.shooterPIDF.i = i;
robot.shooterPIDF.d = d;
robot.shooterPIDF.f = f;
shooterPIDF1.p = p;
shooterPIDF1.i = i;
shooterPIDF1.d = d;
shooterPIDF1.f = f;
shooterPIDF2.p = p;
shooterPIDF2.i = i;
shooterPIDF2.d = d;
shooterPIDF2.f = f;
}
// Convert from RPM to Ticks per Second

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

@@ -21,7 +21,7 @@ public class Robot {
//Initialize Public Components
public static boolean usingLimelight = true;
public static boolean usingCamera = true;
public static boolean usingCamera = false;
public DcMotorEx frontLeft;
public DcMotorEx frontRight;
public DcMotorEx backLeft;

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

@@ -8,7 +8,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
public class Servos {
//PID constants
// TODO: get PIDF constants
public static double spinP = 3.3, spinI = 0, spinD = 0.1, spinF = 0.02;
public static double spinP = 2.0, spinI = 0, spinD = 0.3, spinF = 0.02;
public static double turrP = 1.1, turrI = 0.25, turrD = 0.0625, turrF = 0;
public static double spin_scalar = 1.0086;
public static double spin_restPos = 0.0;
@@ -40,7 +40,7 @@ public class Servos {
}
public boolean spinEqual(double pos) {
return Math.abs(pos - this.getSpinPos()) < 0.02;
return Math.abs(pos - this.getSpinPos()) < 0.03;
}
public double getTurrPos() {

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

@@ -277,7 +277,6 @@ public class Spindexer {
if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
currentIntakeState = Spindexer.IntakeState.UNKNOWN_DETECT;
stopSpindexer();
detectBalls(true, true);
unknownColorDetect = 0;
} else {
// Keep moving the spindexer
@@ -288,7 +287,7 @@ public class Spindexer {
if (unknownColorDetect >5) {
currentIntakeState = Spindexer.IntakeState.FINDNEXT;
} else {
detectBalls(true, true);
//detectBalls(true, true);
unknownColorDetect++;
}
break;
@@ -343,7 +342,7 @@ public class Spindexer {
if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
currentIntakeState = Spindexer.IntakeState.INTAKE;
stopSpindexer();
detectBalls(false, false);
//detectBalls(false, false);
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
@@ -374,7 +373,7 @@ public class Spindexer {
case SHOOT_ALL_READY:
// 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) {
// All ball shot move to intake state
currentIntakeState = Spindexer.IntakeState.FINDNEXT;
@@ -422,7 +421,7 @@ public class Spindexer {
if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
currentIntakeState = Spindexer.IntakeState.INTAKE;
stopSpindexer();
detectBalls(true, false);
//detectBalls(true, false);
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);