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

shooting is ok but NOT PERFECT

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-04 17:29:14 -05:00
parent c15b9d58d4
commit e7056812b4
6 changed files with 152 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -55,6 +55,7 @@ public class TeleopV4 extends LinearOpMode {
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);
@@ -73,7 +74,8 @@ public class TeleopV4 extends LinearOpMode {
follower.update();
shooter.update();
shooter.update(robot.voltage.getVoltage());
spindexerTransferIntake.update();
SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
@@ -114,7 +116,8 @@ public class TeleopV4 extends LinearOpMode {
TELE.addData("Distance From Goal", commander.getDistance());
TELE.addData("Hood Position", commander.getHoodPredicted());
TELE.addData("Transfer Power", robot.transfer.getPower());
TELE.addData("Velocity RPM", commander.getPredictedRPM());
TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
TELE.update();
}

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

@@ -1,53 +1,38 @@
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.VelocityCommander;
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 turretPos = 0.51;
@Override
public void runOpMode() throws InterruptedException {
@@ -56,105 +41,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);
commander = new VelocityCommander();
shooter = new Shooter(
robot,
TELE,
follower,
true,
turret,
flywheel,
commander
);
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", robot.transfer.getPower());
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);

58
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,6 +18,8 @@ public class Shooter {
double goalY = 0.0;
double obeliskX = 72;
double obeliskY = 144;
double turretGoalX = 0;
double turretGoalY = 0;
private boolean red = true;
@@ -37,10 +42,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,7 +83,7 @@ public class Shooter {
}
private final double shooterDistFromCenter = 1.545;
public void update() {
public void update(double voltage) {
switch (state) {
case NOTHING:
@@ -87,16 +95,18 @@ public class Shooter {
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:
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,17 +115,21 @@ 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:
turr.trackObelisk(
@@ -124,28 +138,35 @@ public class Shooter {
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:
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);
@@ -153,8 +174,8 @@ public class Shooter {
case MANUAL_FLYWHEEL_TRACK_TURR:
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 +183,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;
}

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

@@ -2,11 +2,13 @@ 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 org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
@Config
public class SpindexerTransferIntake {
private final Robot robot;

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

@@ -1,14 +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 veloRPM = 2000;
private double velo = 0;
public VelocityCommander() {}
@@ -24,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){
@@ -78,7 +79,7 @@ public class VelocityCommander {
return hoodPos;
}
private void distToTransferPow(double dist){
private void distToTransferPow(double dist, double voltage){
if (dist < 118){
transferPow = -1;
} else if (dist < 125){
@@ -86,6 +87,8 @@ public class VelocityCommander {
} else {
transferPow = -0.5;
}
// transferPow = Math.max(-0.5, Math.min(-1, transferPow * (14/voltage)));
}
public double getTransferPow(){return transferPow;}
@@ -94,22 +97,21 @@ public class VelocityCommander {
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);
distToTransferPow(predictedDist);
veloRPM = distToRPM(predictedDist);
return veloRPM;
distToTransferPow(predictedDist, voltage);
distToRPM(predictedDist);
}
public double getPredictedRPM(){return veloRPM;}
public double getPredictedRPM(){return velo;}
public double getDistance(){return predictedDist;}
}