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

tweaks were made and hardware changes are needed

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-04 22:13:49 -05:00
parent e25b372eca
commit 9d29e0b56c
4 changed files with 57 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -55,6 +55,7 @@ public class TeleopV4 extends LinearOpMode {
turret = new Turret(robot);
parkTilter = new ParkTilter(robot);
parkTilter.unpark();
shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
shooter.setState(Shooter.ShooterState.TRACK_GOAL);
@@ -65,7 +66,7 @@ public class TeleopV4 extends LinearOpMode {
turret.switchPipeline(Turret.PipelineMode.TRACKING);
robot.limelight.start();
limelightUsed = false;
limelightUsed = true;
waitForStart();
@@ -86,6 +87,20 @@ public class TeleopV4 extends LinearOpMode {
follower.update();
if (gamepad1.dpadLeftWasPressed()){
shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
}
if (gamepad1.dpadRightWasPressed()){
shooter.setState(Shooter.ShooterState.TRACK_GOAL);
}
if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
shooter.setFlywheelVelocity(2500);
robot.setHoodPos(0.6);
robot.setTransferPower(-0.8);
}
shooter.update(robot.voltage.getVoltage());
spindexerTransferIntake.update();
@@ -110,12 +125,9 @@ public class TeleopV4 extends LinearOpMode {
SpindexerTransferIntake.RapidMode.HOLD_BALLS
);
}
if (gamepad1.rightBumperWasPressed()
&& state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
spindexerTransferIntake.setRapidMode(
SpindexerTransferIntake.RapidMode.INTAKE
);
if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
robot.setIntakePower(1);
}
if (gamepad2.leftBumperWasPressed()){

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

@@ -15,6 +15,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.teleop.TeleopV4;
import org.firstinspires.ftc.teamcode.utilsv2.*;
@TeleOp
@@ -57,6 +58,7 @@ public class NewShooterTest extends LinearOpMode {
turret = new Turret(robot);
parkTilter = new ParkTilter(robot);
parkTilter.unpark();
shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
@@ -81,10 +83,28 @@ public class NewShooterTest extends LinearOpMode {
gamepad1.left_stick_x
);
if (gamepad1.crossWasPressed()){
follower.setPose(TeleopV4.relocalizePose);
gamepad1.rumble(100);
}
follower.update();
shooter.setFlywheelVelocity(flywheelVelo);
robot.setHoodPos(hoodPos);
if (gamepad1.dpadLeftWasPressed()){
shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
}
if (gamepad1.dpadRightWasPressed()){
shooter.setState(Shooter.ShooterState.TRACK_GOAL);
}
if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
shooter.setFlywheelVelocity(flywheelVelo);
robot.setHoodPos(hoodPos);
robot.setTransferPower(transferPower);
}
// shooter.setTurretPosition(turretPos);
shooter.update(robot.voltage.getVoltage());
spindexerTransferIntake.update();
@@ -116,12 +136,8 @@ public class NewShooterTest extends LinearOpMode {
SpindexerTransferIntake.RapidMode.HOLD_BALLS
);
}
if (gamepad1.rightBumperWasPressed()
&& state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
spindexerTransferIntake.setRapidMode(
SpindexerTransferIntake.RapidMode.INTAKE
);
if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
robot.setIntakePower(1);
}
if (gamepad1.dpad_down){
@@ -135,6 +151,7 @@ public class NewShooterTest extends LinearOpMode {
TELE.addData("Transfer Power", commander.getTransferPow());
TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
TELE.addData("TX:", turret.getTX());
TELE.update();
}

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

@@ -71,6 +71,10 @@ public class Shooter {
this.state = shooterState;
}
public ShooterState getState(){
return state;
}
public void setTurretPosition(double input) {
this.turretPosition = input;
}

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java
View File

@@ -1,12 +1,10 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import static java.lang.Math.abs;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDController;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.util.Range;
@@ -23,18 +21,18 @@ public class Turret {
private final double turretMin = 0.05;
private final double turretMax = 0.95;
public static boolean limelightUsed = true;
public static double B_PID_P = 0.08, B_PID_I = 0.0, B_PID_D = 0.007;
public static double B_PID_P = 0.0003, B_PID_I = 0.0, B_PID_D = 0.00003;
LLResult result;
PIDController bearingPID;
boolean bearingAligned = false;
private boolean lockOffset = false;
public int LL_COAST_TICKS = 60;
public static double TARGET_POSITION_TOLERANCE = 0.5;
public static double alphaTX = 0.5;
private double targetTx = 0;
private double bearingOffset = 0;
private double currentTrackOffset = 0;
private double llCoast = 0;
private double servoAngle = 0.51;
double tx = 0.0;
double ty = 0.0;
private final double hVelK = 0; // TODO: Tune
private final double xVelK = 0; // TODO: Tune
private final double xAccK = 0; // TODO: Tune
@@ -67,6 +65,8 @@ public class Turret {
}
}
public double getTX(){return tx;}
public enum PipelineMode{
OBELISK,
TRACKING
@@ -137,13 +137,12 @@ public class Turret {
robot.setTurretPos(pos);
}
private double currentTrackOffset = 0;
private double llCoast = 0;
private double servoAngle = 0.51;
public void trackGoal(double dx, double dy, double h, double hVel, double xVel, double xAcc, double yVel, double yAcc) {
// dx, dy, dz is target - robot
// h is the raw heading where 0 degrees is positive x in the system of x, y
bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D); // Keep when debugging/tuning, comment out when doing teleop
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
double predictedH = h + (hVel * hVelK); // Positive bc h = robot heading
@@ -162,7 +161,7 @@ public class Turret {
targetTx = (tx*alphaTX)+(targetTx*(1-alphaTX));
bearingAligned = Math.abs(targetTx) < TARGET_POSITION_TOLERANCE;
if (!bearingAligned){
bearingOffset = -(bearingPID.calculate(targetTx, 0.0));
bearingOffset = (bearingPID.calculate(targetTx, 0.0));
}
} else {
targetTx = 0;
@@ -178,8 +177,6 @@ public class Turret {
}
}
currentTrackOffset += bearingOffset;
double servoTicksFromNeutral = (angleDelta+currentTrackOffset) * (2.0 * servoTicksPer180);
servoAngle = neutralPosition + servoTicksFromNeutral;