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

Yay

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-03 15:51:03 -05:00
parent a89535830b
commit 05f59d1820
5 changed files with 46 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.tests;
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;
@@ -20,7 +22,9 @@ public class NewShooterTest extends LinearOpMode {
Robot robot;
Flywheel flywheel;
Turret turret;
Shooter shooter;
MultipleTelemetry TELE;
Follower follower;
public static boolean intake = true;
@@ -52,14 +56,16 @@ public class NewShooterTest extends LinearOpMode {
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
follower = Constants.createFollower(hardwareMap);
follower.setStartingPose(new Pose(72, 72, 0));
flywheel = new Flywheel(robot);
turret = new Turret(robot);
Shooter shooter = new Shooter(
shooter = new Shooter(
robot,
TELE,
Constants.createFollower(hardwareMap),
follower,
true,
turret,
flywheel
@@ -73,6 +79,8 @@ public class NewShooterTest extends LinearOpMode {
while (opModeIsActive()) {
follower.update();
robot.setHoodPos(hoodPos);
shooter.setTurretPosition(turretPos);
shooter.setFlywheelVelocity(flywheel_velo);
@@ -136,9 +144,10 @@ public class NewShooterTest extends LinearOpMode {
TELE.addData("Flywheel Velocity1", (robot.shooter1.getVelocity() * 60) / 28);
TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28);
TELE.addData("Flywheel Averag Velocity", flywheel.getAverageVelocity());
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();
shooter.update();

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

@@ -128,5 +128,7 @@ public class Flywheel {
steady = (Math.abs(commandedVelocity - averageVelocity) < 50);
}
public double getShooterPower(){return power;}
}

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

@@ -74,20 +74,30 @@ public class Shooter {
return turr.getObeliskID();
}
private final double shooterDistFromCenter = 1.545;
public void update() {
switch (state) {
case NOTHING:
break;
case MANUAL:
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()
);
fly.manageFlywheel(flywheelVelocity);
turr.manual(turretPosition);
break;
case TRACK_GOAL:
turr.trackGoal(
(goalX - follow.getPose().getX()),
(goalY - follow.getPose().getY()),
(goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getHeading(),
follow.getAngularVelocity(),
follow.getVelocity().getXComponent(),
@@ -97,8 +107,8 @@ public class Shooter {
);
flywheelVelocity = commander.getVeloPredictive(
(goalX - follow.getPose().getX()),
(goalY - follow.getPose().getY()),
(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(),
@@ -109,14 +119,14 @@ public class Shooter {
break;
case READ_OBELISK:
turr.trackObelisk(
(goalX - follow.getPose().getX()),
(goalY - follow.getPose().getY()),
(obeliskX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(obeliskY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getHeading()
);
flywheelVelocity = commander.getVeloPredictive(
(goalX - follow.getPose().getX()),
(goalY - follow.getPose().getY()),
(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(),
@@ -129,8 +139,8 @@ public class Shooter {
case MANUAL_TURRET_TRACK_FLY:
turr.manual(turretPosition);
flywheelVelocity = commander.getVeloPredictive(
(goalX - follow.getPose().getX()),
(goalY - follow.getPose().getY()),
(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(),
@@ -142,8 +152,8 @@ public class Shooter {
case MANUAL_FLYWHEEL_TRACK_TURR:
turr.trackGoal(
(goalX - follow.getPose().getX()),
(goalY - follow.getPose().getY()),
(goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getHeading(),
follow.getAngularVelocity(),
follow.getVelocity().getXComponent(),
@@ -158,4 +168,7 @@ public class Shooter {
}
public double getDistance(){return commander.getDistance();}
}

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

@@ -83,7 +83,7 @@ public class Turret {
robot.setTurretPos(pos);
}
// 1.545
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

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

@@ -15,20 +15,22 @@ public class VelocityCommander {
return Math.sqrt(dist*dist + goalH*goalH) * 20;
//TODO: Add regression here using goalH
}
// 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) {
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 predictedDist = Math.sqrt(dx*dx + dy*dy);
predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight);
return distToRPM(predictedDist);
}
public double getDistance(){return predictedDist;}
}