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

Added shooter class to manager flywheel turret and targetter

Browse Source
This commit is contained in:
Keshav Anand
2026-06-01 17:12:17 -05:00
parent f32f31a224
commit 184ec893a4
4 changed files with 200 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,9 +26,9 @@ public class Flywheel {
private final LinkedList<Double> velocityHistory = new LinkedList<>();
public Flywheel(HardwareMap hardwareMap) {
public Flywheel(Robot rob) {
robot = new Robot(hardwareMap);
robot = rob;
shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
shooterPIDF2 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
}

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

@@ -1,5 +1,162 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.pedropathing.follower.Follower;
import org.firstinspires.ftc.teamcode.utils.Robot;
public class Shooter {
Robot robot;
Flywheel fly;
Turret turr;
VelocityCommander commander;
double goalX = 0.0;
double goalY = 0.0;
double obeliskX = 72;
double obeliskY = 144;
private boolean red = true;
Follower follow;
public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance) {
this.robot = rob;
this.fly = new Flywheel(rob);
this.turr = new Turret(rob);
this.follow = follower;
this.commander = new VelocityCommander();
setRedAlliance(redAlliance);
if (redAlliance) {
goalX = 144;
goalY = 144;
} else {
goalX = 0;
goalY = 144;
}
}
public void setRedAlliance(boolean input) {
this.red = input;
}
private double flywheelVelocity = 0.0;
private double turretPosition = 0.5;
enum ShooterState {
READ_OBELISK,
TRACK_GOAL,
MANUAL_FLYWHEEL_TRACK_TURR,
MANUAL_TURRET_TRACK_FLY,
MANUAL,
NOTHING
}
private ShooterState state = ShooterState.NOTHING;
public void setState(ShooterState shooterState) {
this.state = shooterState;
}
public void setTurretPosition(double input) {
this.turretPosition = input;
}
public void setFlywheelVelocity(double input) {
this.flywheelVelocity = input;
}
public int getObeliskID() {
return turr.getObeliskID();
}
public void update() {
switch (state) {
case NOTHING:
break;
case MANUAL:
fly.manageFlywheel(flywheelVelocity);
turr.manual(turretPosition);
break;
case TRACK_GOAL:
turr.trackGoal(
(follow.getPose().getX() - goalX),
(follow.getPose().getY() - goalY),
follow.getHeading(),
follow.getAngularVelocity(),
follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent()
);
flywheelVelocity = commander.getVeloPredictive(
(follow.getPose().getX() - goalX),
(follow.getPose().getY() - goalY),
follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent()
);
fly.manageFlywheel(flywheelVelocity);
break;
case READ_OBELISK:
turr.trackObelisk(
(follow.getPose().getX() - goalX),
(follow.getPose().getY() - goalY),
follow.getHeading()
);
flywheelVelocity = commander.getVeloPredictive(
(follow.getPose().getX() - goalX),
(follow.getPose().getY() - goalY),
follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent()
);
fly.manageFlywheel(flywheelVelocity);
break;
case MANUAL_TURRET_TRACK_FLY:
turr.manual(turretPosition);
flywheelVelocity = commander.getVeloPredictive(
(follow.getPose().getX() - goalX),
(follow.getPose().getY() - goalY),
follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent()
);
fly.manageFlywheel(flywheelVelocity);
break;
case MANUAL_FLYWHEEL_TRACK_TURR:
turr.trackGoal(
(follow.getPose().getX() - goalX),
(follow.getPose().getY() - goalY),
follow.getHeading(),
follow.getAngularVelocity(),
follow.getVelocity().getXComponent(),
follow.getAcceleration().getXComponent(),
follow.getVelocity().getYComponent(),
follow.getAcceleration().getYComponent()
);
fly.manageFlywheel(flywheelVelocity);
break;
}
}
}

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

@@ -18,7 +18,6 @@ public class Turret {
private final double turretMin = 0.04; //TODO: Tune
private final double turretMax = 0.94; //TODO: Tune
private final double hVelK = 0; // TODO: Tune
private final double hAccK = 0; // TODO: Tune
private final double xVelK = 0; // TODO: Tune
private final double xAccK = 0; // TODO: Tune
private final double yVelK = 0; // TODO: Tune
@@ -81,14 +80,19 @@ public class Turret {
return obeliskID;
}
public void manual (double pos) {
robot.turr1.setPosition(pos);
robot.turr2.setPosition(pos);
}
public void trackGoal(double dx, double dy, double h, double hVel, double hAcc, 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
// h is the raw heading where 0 degrees is positive x in the system of x, y
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) + (0.5 * hAcc * hAccK); // Positive bc h = robot heading
double predictedH = h + (hVel * hVelK); // Positive bc h = robot heading
predictedH = wrapAngle(predictedH);

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java Normal file
View File

@@ -0,0 +1,34 @@
package org.firstinspires.ftc.teamcode.utilsv2;
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 VelocityCommander() {
}
private double distToRPM (double dist){
return Math.sqrt(dist*dist + goalH*goalH);
//TODO: Add regression here using goalH
}
public double getVeloStationary (double distance){
return distToRPM(distance);
}
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);
return distToRPM(predictedDist);
}
}