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.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; 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.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -20,7 +22,9 @@ public class NewShooterTest extends LinearOpMode {
Robot robot; Robot robot;
Flywheel flywheel; Flywheel flywheel;
Turret turret; Turret turret;
Shooter shooter;
MultipleTelemetry TELE; MultipleTelemetry TELE;
Follower follower;
public static boolean intake = true; public static boolean intake = true;
@@ -52,14 +56,16 @@ public class NewShooterTest extends LinearOpMode {
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
follower = Constants.createFollower(hardwareMap);
follower.setStartingPose(new Pose(72, 72, 0));
flywheel = new Flywheel(robot); flywheel = new Flywheel(robot);
turret = new Turret(robot); turret = new Turret(robot);
Shooter shooter = new Shooter( shooter = new Shooter(
robot, robot,
TELE, TELE,
Constants.createFollower(hardwareMap), follower,
true, true,
turret, turret,
flywheel flywheel
@@ -73,6 +79,8 @@ public class NewShooterTest extends LinearOpMode {
while (opModeIsActive()) { while (opModeIsActive()) {
follower.update();
robot.setHoodPos(hoodPos); robot.setHoodPos(hoodPos);
shooter.setTurretPosition(turretPos); shooter.setTurretPosition(turretPos);
shooter.setFlywheelVelocity(flywheel_velo); 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 Velocity1", (robot.shooter1.getVelocity() * 60) / 28);
TELE.addData("Flywheel Velocity2", (robot.shooter2.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("PIDF Coefficients", Flywheel.shooterPIDF);
TELE.addData("Power", flywheel.getShooterPower()); TELE.addData("Power", flywheel.getShooterPower());
TELE.addData("Distance", shooter.getDistance());
TELE.update(); TELE.update();
shooter.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); steady = (Math.abs(commandedVelocity - averageVelocity) < 50);
} }
public double getShooterPower(){return power;} 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(); return turr.getObeliskID();
} }
private final double shooterDistFromCenter = 1.545;
public void update() { public void update() {
switch (state) { switch (state) {
case NOTHING: case NOTHING:
break; break;
case MANUAL: 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); fly.manageFlywheel(flywheelVelocity);
turr.manual(turretPosition); turr.manual(turretPosition);
break; break;
case TRACK_GOAL: case TRACK_GOAL:
turr.trackGoal( turr.trackGoal(
(goalX - follow.getPose().getX()), (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY()), (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getHeading(), follow.getHeading(),
follow.getAngularVelocity(), follow.getAngularVelocity(),
follow.getVelocity().getXComponent(), follow.getVelocity().getXComponent(),
@@ -97,8 +107,8 @@ public class Shooter {
); );
flywheelVelocity = commander.getVeloPredictive( flywheelVelocity = commander.getVeloPredictive(
(goalX - follow.getPose().getX()), (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY()), (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getVelocity().getXComponent(), follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(), follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(), follow.getVelocity().getYComponent(),
@@ -109,14 +119,14 @@ public class Shooter {
break; break;
case READ_OBELISK: case READ_OBELISK:
turr.trackObelisk( turr.trackObelisk(
(goalX - follow.getPose().getX()), (obeliskX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY()), (obeliskY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getHeading() follow.getHeading()
); );
flywheelVelocity = commander.getVeloPredictive( flywheelVelocity = commander.getVeloPredictive(
(goalX - follow.getPose().getX()), (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY()), (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getVelocity().getXComponent(), follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(), follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(), follow.getVelocity().getYComponent(),
@@ -129,8 +139,8 @@ public class Shooter {
case MANUAL_TURRET_TRACK_FLY: case MANUAL_TURRET_TRACK_FLY:
turr.manual(turretPosition); turr.manual(turretPosition);
flywheelVelocity = commander.getVeloPredictive( flywheelVelocity = commander.getVeloPredictive(
(goalX - follow.getPose().getX()), (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY()), (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getVelocity().getXComponent(), follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(), follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(), follow.getVelocity().getYComponent(),
@@ -142,8 +152,8 @@ public class Shooter {
case MANUAL_FLYWHEEL_TRACK_TURR: case MANUAL_FLYWHEEL_TRACK_TURR:
turr.trackGoal( turr.trackGoal(
(goalX - follow.getPose().getX()), (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(goalY - follow.getPose().getY()), (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getHeading(), follow.getHeading(),
follow.getAngularVelocity(), follow.getAngularVelocity(),
follow.getVelocity().getXComponent(), 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); robot.setTurretPos(pos);
} }
// 1.545
public void trackGoal(double dx, double dy, double h, double hVel, double xVel, double xAcc, double yVel, double yAcc) { 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 // 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; return Math.sqrt(dist*dist + goalH*goalH) * 20;
//TODO: Add regression here using goalH //TODO: Add regression here using goalH
} }
// 27
public double getVeloStationary (double distance){ public double getVeloStationary (double distance){
return distToRPM(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 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 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 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); return distToRPM(predictedDist);
} }
public double getDistance(){return predictedDist;}
} }