diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java index 0e9f773..123d3dc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java @@ -26,9 +26,9 @@ public class Flywheel { private final LinkedList 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); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index 74a39b6..901981a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -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; + + } + + } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java index 5c0e638..ec15235 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java @@ -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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java new file mode 100644 index 0000000..297b723 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java @@ -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); + } + + +}