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

Merge remote-tracking branch 'origin/cowtown-work' into add-sorted-spindexer

Browse Source 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
This commit is contained in:
Keshav Anand
2026-06-04 18:12:04 -05:00
parent 9a4aca90ba a3479d8816
commit 8c2a655c5c
6 changed files with 190 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
View File

@@ -53,8 +53,9 @@ public class TeleopV4 extends LinearOpMode {
parkTilter = new ParkTilter(robot);
shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel);
shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
shooter.setState(Shooter.ShooterState.TRACK_GOAL);
shooter.setRedAlliance(Color.redAlliance);
spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
@@ -64,9 +65,7 @@ public class TeleopV4 extends LinearOpMode {
if (isStopRequested()) return;
while (opModeIsActive()) {
//Drivetrain
drivetrain.drive(
-gamepad1.right_stick_y,
gamepad1.right_stick_x,
@@ -75,7 +74,8 @@ public class TeleopV4 extends LinearOpMode {
follower.update();
shooter.update();
shooter.update(robot.voltage.getVoltage());
spindexerTransferIntake.update();
SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
@@ -113,6 +113,12 @@ public class TeleopV4 extends LinearOpMode {
parkTilter.unpark();
}
TELE.addData("Distance From Goal", commander.getDistance());
TELE.addData("Hood Position", commander.getHoodPredicted());
TELE.addData("Transfer Power", robot.transfer.getPower());
TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
TELE.update();
}

183
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java
View File

@@ -1,51 +1,39 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.Pose;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
import org.firstinspires.ftc.teamcode.utilsv2.Robot;
import org.firstinspires.ftc.teamcode.utilsv2.Shooter;
import org.firstinspires.ftc.teamcode.utilsv2.Turret;
import org.firstinspires.ftc.teamcode.utilsv2.*;
@Config
@TeleOp
@Config
public class NewShooterTest extends LinearOpMode {
Robot robot;
Flywheel flywheel;
Turret turret;
Drivetrain drivetrain;
Shooter shooter;
MultipleTelemetry TELE;
Follower follower;
SpindexerTransferIntake spindexerTransferIntake;
Turret turret;
Flywheel flywheel;
VelocityCommander commander;
ParkTilter parkTilter;
public static boolean intake = true;
public static boolean shoot = false;
public static double intakePower = 1.0;
public static double transferShootPower = -1;
public static double transferIntakePower = -1;
public static double turretPos = 0.51;
public static double hoodPos = 0.51;
public static double flywheel_velo = 0;
public static double shooterP = 500, shooterI = 1, shooterD = 0, shooterF = 93;
private enum ShootState {
IDLE,
WAIT_GATE,
WAIT_PUSH
}
private ShootState shootState = ShootState.IDLE;
private long timestamp = 0;
public static int flywheelVelo = 0;
public static double hoodPos = 0.5;
public static double transferPower = -0.7;
// public static double turretPos = 0.51;
@Override
public void runOpMode() throws InterruptedException {
@@ -54,103 +42,90 @@ public class NewShooterTest extends LinearOpMode {
robot = Robot.getInstance(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
TELE = new MultipleTelemetry(
FtcDashboard.getInstance().getTelemetry(), telemetry
);
commander = new VelocityCommander();
drivetrain = new Drivetrain(robot, TELE);
follower = Constants.createFollower(hardwareMap);
follower.setStartingPose(new Pose(72, 72, 0));
Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
follower.setStartingPose(start);
flywheel = new Flywheel(robot);
turret = new Turret(robot);
shooter = new Shooter(
robot,
TELE,
follower,
true,
turret,
flywheel
);
parkTilter = new ParkTilter(robot);
shooter.setState(Shooter.ShooterState.MANUAL);
shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
shooter.setRedAlliance(Color.redAlliance);
spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
//Drivetrain
drivetrain.drive(
-gamepad1.right_stick_y,
gamepad1.right_stick_x,
gamepad1.left_stick_x
);
follower.update();
shooter.setFlywheelVelocity(flywheelVelo);
robot.setHoodPos(hoodPos);
shooter.setTurretPosition(turretPos);
shooter.setFlywheelVelocity(flywheel_velo);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage);
// shooter.setTurretPosition(turretPos);
shooter.update(robot.voltage.getVoltage());
spindexerTransferIntake.update();
robot.setSpinPos(ServoPositions.spindexer_A2);
SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
if (intake && !shoot) {
if (gamepad1.leftBumperWasPressed() &&
(state == SpindexerTransferIntake.RapidMode.INTAKE ||
state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
shootState = ShootState.IDLE;
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Closed);
robot.setTransferPower(transferIntakePower);
robot.setIntakePower(intakePower);
robot.setTransferServoPos(
ServoPositions.transferServo_out);
} else if (shoot) {
robot.setIntakePower(intakePower);
switch (shootState) {
case IDLE:
robot.setTransferPower(transferShootPower);
timestamp = System.currentTimeMillis();
shootState = ShootState.WAIT_GATE;
break;
case WAIT_GATE:
if (System.currentTimeMillis() - timestamp >= 300) {
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Open);
timestamp = System.currentTimeMillis();
shootState = ShootState.WAIT_PUSH;
}
break;
case WAIT_PUSH:
if (System.currentTimeMillis() - timestamp >= 100) {
robot.setTransferServoPos(
ServoPositions.transferServo_in);
shootState = ShootState.IDLE;
}
break;
}
spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
}
TELE.addData("Flywheel Velocity1", (robot.shooter1.getVelocity() * 60) / 28);
TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28);
TELE.addData("Flywheel Average Velocity", flywheel.getAverageVelocity());
TELE.addData("PIDF Coefficients", Flywheel.shooterPIDF);
TELE.addData("Power", flywheel.getShooterPower());
TELE.addData("Distance", shooter.getDistance());
TELE.update();
if (gamepad1.right_trigger > 0.5 &&
(state == SpindexerTransferIntake.RapidMode.INTAKE ||
state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
shooter.update();
spindexerTransferIntake.setRapidMode(
SpindexerTransferIntake.RapidMode.HOLD_BALLS
);
}
if (gamepad1.rightBumperWasPressed()
&& state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
spindexerTransferIntake.setRapidMode(
SpindexerTransferIntake.RapidMode.INTAKE
);
}
if (gamepad1.dpad_down){
parkTilter.park();
} else if (gamepad1.dpad_up) {
parkTilter.unpark();
}
TELE.addData("Distance From Goal", commander.getDistance());
TELE.addData("Hood Position", commander.getHoodPredicted());
TELE.addData("Transfer Power", commander.getTransferPow());
TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
TELE.update();
}
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java
View File

@@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -9,6 +10,7 @@ import org.firstinspires.ftc.teamcode.utilsv2.Robot;
import java.util.LinkedList;
@Config
public class Flywheel {
Robot robot;
@@ -70,8 +72,9 @@ public class Flywheel {
private double prevF = 0;
public static double voltagePIDFDifference = 0.8;
public void setF(double f){
public static double voltagePIDFDifference = 1;
public void setF(double voltage){
double f = shooterPIDF_F / voltage;
if (Math.abs(prevF - f) > voltagePIDFDifference) {
shooterPIDF.f = f;
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);

68
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java
View File

@@ -1,9 +1,12 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import static org.firstinspires.ftc.teamcode.utilsv2.Flywheel.*;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.pedropathing.follower.Follower;
@Config
public class Shooter {
Robot robot;
@@ -15,18 +18,21 @@ public class Shooter {
double goalY = 0.0;
double obeliskX = 72;
double obeliskY = 144;
double turretGoalX = 0;
double turretGoalY = 0;
private boolean red = true;
public static boolean manualFlywheel = false;
Follower follow;
public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance, Turret turret, Flywheel flywheel) {
public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance, Turret turret, Flywheel flywheel, VelocityCommander com) {
this.robot = rob;
this.fly = flywheel;
this.turr = turret;
this.follow = follower;
this.commander = new VelocityCommander();
this.commander = com;
setRedAlliance(redAlliance);
@@ -37,10 +43,13 @@ public class Shooter {
if (this.red) {
goalX = 144;
turretGoalX = 136;
} else {
goalX = 0;
turretGoalX = 8;
}
goalY = 144;
turretGoalY = 136;
}
private double flywheelVelocity = 0.0;
@@ -75,28 +84,32 @@ public class Shooter {
}
private final double shooterDistFromCenter = 1.545;
public void update() {
public void update(double voltage) {
switch (state) {
case NOTHING:
break;
case MANUAL:
manualFlywheel = true;
commander.getVeloPredictive(
(goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent()
follow.getAcceleration().getYComponent(),
voltage
);
fly.manageFlywheel(flywheelVelocity);
fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage);
turr.manual(turretPosition);
break;
case TRACK_GOAL:
manualFlywheel = false;
turr.trackGoal(
(goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
(turretGoalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(turretGoalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getHeading(),
follow.getAngularVelocity(),
follow.getVelocity().getXComponent(),
@@ -105,56 +118,70 @@ public class Shooter {
follow.getAcceleration().getYComponent()
);
flywheelVelocity = commander.getVeloPredictive(
commander.getVeloPredictive(
(goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent()
follow.getAcceleration().getYComponent(),
voltage
);
flywheelVelocity = commander.getPredictedRPM();
robot.setHoodPos(commander.getHoodPredicted());
fly.manageFlywheel(flywheelVelocity);
fly.setF(voltage);
break;
case READ_OBELISK:
manualFlywheel = false;
turr.trackObelisk(
(obeliskX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(obeliskY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getHeading()
);
flywheelVelocity = commander.getVeloPredictive(
commander.getVeloPredictive(
(goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent()
follow.getAcceleration().getYComponent(),
voltage
);
flywheelVelocity = commander.getPredictedRPM();
fly.manageFlywheel(flywheelVelocity);
fly.setF(voltage);
break;
case MANUAL_TURRET_TRACK_FLY:
manualFlywheel = false;
turr.manual(turretPosition);
flywheelVelocity = commander.getVeloPredictive(
commander.getVeloPredictive(
(goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent()
follow.getAcceleration().getYComponent(),
voltage
);
flywheelVelocity = commander.getPredictedRPM();
robot.setHoodPos(commander.getHoodPredicted());
fly.manageFlywheel(flywheelVelocity);
break;
case MANUAL_FLYWHEEL_TRACK_TURR:
manualFlywheel = true;
turr.trackGoal(
(goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
(turretGoalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(turretGoalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getHeading(),
follow.getAngularVelocity(),
follow.getVelocity().getXComponent(),
@@ -162,7 +189,18 @@ public class Shooter {
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent()
);
commander.getVeloPredictive(
(goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent(),
voltage
);
fly.manageFlywheel(flywheelVelocity);
fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage);
fly.setF(voltage);
break;
}

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
View File

@@ -1,11 +1,16 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import android.health.connect.datatypes.units.Velocity;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.tests.NewShooterTest;
@Config
public class SpindexerTransferIntake {
private final Robot robot;
@@ -236,10 +241,6 @@ public class SpindexerTransferIntake {
public void update() {
if (mode == SpindexerMode.RAPID && rapidMode == RapidMode.INTAKE){
robot.setTransferPower(-0.7);
}
switch (mode) {
case RAPID:
@@ -259,6 +260,7 @@ public class SpindexerTransferIntake {
robot.setSpinPos(
ServoPositions.spindexer_A2
);
robot.setTransferPower(-0.7);
robot.setTransferServoPos(
ServoPositions.transferServo_out
);
@@ -276,6 +278,7 @@ public class SpindexerTransferIntake {
if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
setRapidMode(RapidMode.BEFORE_PULSE_OUT);
}
robot.setTransferPower(-0.3);
break;
@@ -333,6 +336,12 @@ public class SpindexerTransferIntake {
setRapidMode(RapidMode.SHOOT);
}
if (Shooter.manualFlywheel){
robot.setTransferPower(NewShooterTest.transferPower);
} else {
robot.setTransferPower(commander.getTransferPow());
}
break;
case SHOOT:
@@ -343,6 +352,13 @@ public class SpindexerTransferIntake {
if (stateTime() >= 400) {
setRapidMode(RapidMode.INTAKE);
}
if (Shooter.manualFlywheel){
robot.setTransferPower(NewShooterTest.transferPower);
} else {
robot.setTransferPower(commander.getTransferPow());
}
break;
}
break;

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java
View File

@@ -1,13 +1,16 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import com.acmerobotics.dashboard.config.Config;
@Config
public class VelocityCommander {
private final double goalH = 20.0; //TODO: Tune
private final double xVelK = 0; // TODO: Tune
private final double xAccK = 0; // TODO: Tune
private final double yVelK = 0; // TODO: Tune
private final double yAccK = 0; // TODO: Tune
public static double xVelK = 0.05; // TODO: Tune
public static double xAccK = 0.025; // TODO: Tune
public static double yVelK = 0.05; // TODO: Tune
public static double yAccK = 0.025; // TODO: Tune
private double hoodPos = 0.88;
private double transferPow = -1;
private double velo = 0;
public VelocityCommander() {}
@@ -23,7 +26,6 @@ public class VelocityCommander {
private final double veloJ = 272005.7124;
private final double veloK = -2474581.713;
private double distToRPM (double dist){
double velo = 0;
if (dist < 49) {
velo = 2000;
} else if (dist > 165){
@@ -46,7 +48,7 @@ public class VelocityCommander {
}
private final double hoodA = -4.3276177*Math.pow(10, -13);
private final double hoodB = 2.68062979*Math.pow(10, -11);
private final double hoodB = 2.68062979*Math.pow(10, -10);
private final double hoodC = -7.12859632*Math.pow(10, -8);
private final double hoodD = 0.0000106010785;
private final double hoodE = -0.000960693973;
@@ -73,8 +75,11 @@ public class VelocityCommander {
hoodPos = Math.max(0.35, Math.min(0.88, hoodPos));
}
}
public double getHoodPredicted(){
return hoodPos;
}
private void distToTransferPow(double dist){
private void distToTransferPow(double dist, double voltage){
if (dist < 118){
transferPow = -1;
} else if (dist < 125){
@@ -82,32 +87,31 @@ public class VelocityCommander {
} else {
transferPow = -0.5;
}
}
// transferPow = Math.max(-0.5, Math.min(-1, transferPow * (14/voltage)));
}
public double getTransferPow(){return transferPow;}
public double getHoodPredicted(){
return hoodPos;
}
// 27
public double getVeloStationary (double distance){
return distToRPM(distance);
}
private final double goalHeight = 28;
double predictedDist = 0;
public double getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc) {
public void getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc, double voltage) {
double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK); // Negative bc dy = target - robot
predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight);
double goalHeight = 28;
predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight * goalHeight);
distToHood(predictedDist);
return distToRPM(predictedDist);
distToTransferPow(predictedDist, voltage);
distToRPM(predictedDist);
}
public double getPredictedRPM(){return velo;}
public double getDistance(){return predictedDist;}
}