diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index f030251..db8c8c6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -53,8 +53,9 @@ public class TeleopV4 extends LinearOpMode { parkTilter = new ParkTilter(robot); - shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel); + shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander); shooter.setState(Shooter.ShooterState.TRACK_GOAL); + shooter.setRedAlliance(Color.redAlliance); spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander); spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); @@ -64,9 +65,7 @@ public class TeleopV4 extends LinearOpMode { if (isStopRequested()) return; while (opModeIsActive()) { - //Drivetrain - drivetrain.drive( -gamepad1.right_stick_y, gamepad1.right_stick_x, @@ -75,7 +74,8 @@ public class TeleopV4 extends LinearOpMode { follower.update(); - shooter.update(); + + shooter.update(robot.voltage.getVoltage()); spindexerTransferIntake.update(); SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); @@ -113,6 +113,12 @@ public class TeleopV4 extends LinearOpMode { parkTilter.unpark(); } + TELE.addData("Distance From Goal", commander.getDistance()); + TELE.addData("Hood Position", commander.getHoodPredicted()); + TELE.addData("Transfer Power", robot.transfer.getPower()); + TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM()); + TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity()); + TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index a9e6519..b5e381f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -1,51 +1,39 @@ package org.firstinspires.ftc.teamcode.tests; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; + 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; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.constants.ServoPositions; +import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; -import org.firstinspires.ftc.teamcode.utilsv2.Flywheel; -import org.firstinspires.ftc.teamcode.utilsv2.Robot; -import org.firstinspires.ftc.teamcode.utilsv2.Shooter; -import org.firstinspires.ftc.teamcode.utilsv2.Turret; +import org.firstinspires.ftc.teamcode.utilsv2.*; -@Config @TeleOp +@Config public class NewShooterTest extends LinearOpMode { - Robot robot; - Flywheel flywheel; - Turret turret; + Drivetrain drivetrain; Shooter shooter; MultipleTelemetry TELE; Follower follower; + SpindexerTransferIntake spindexerTransferIntake; + Turret turret; + Flywheel flywheel; + VelocityCommander commander; + ParkTilter parkTilter; - - public static boolean intake = true; - public static boolean shoot = false; - public static double intakePower = 1.0; - public static double transferShootPower = -1; - public static double transferIntakePower = -1; - public static double turretPos = 0.51; - public static double hoodPos = 0.51; - public static double flywheel_velo = 0; - - public static double shooterP = 500, shooterI = 1, shooterD = 0, shooterF = 93; - - private enum ShootState { - IDLE, - WAIT_GATE, - WAIT_PUSH - } - - private ShootState shootState = ShootState.IDLE; - private long timestamp = 0; + public static int flywheelVelo = 0; + public static double hoodPos = 0.5; + public static double transferPower = -0.7; +// public static double turretPos = 0.51; @Override public void runOpMode() throws InterruptedException { @@ -54,103 +42,90 @@ public class NewShooterTest extends LinearOpMode { robot = Robot.getInstance(hardwareMap); - TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + TELE = new MultipleTelemetry( + FtcDashboard.getInstance().getTelemetry(), telemetry + ); + commander = new VelocityCommander(); + drivetrain = new Drivetrain(robot, TELE); follower = Constants.createFollower(hardwareMap); - follower.setStartingPose(new Pose(72, 72, 0)); + Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH)); + follower.setStartingPose(start); flywheel = new Flywheel(robot); turret = new Turret(robot); - shooter = new Shooter( - robot, - TELE, - follower, - true, - turret, - flywheel - ); + parkTilter = new ParkTilter(robot); - shooter.setState(Shooter.ShooterState.MANUAL); + shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander); + shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); + shooter.setRedAlliance(Color.redAlliance); + spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander); + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); waitForStart(); if (isStopRequested()) return; while (opModeIsActive()) { + //Drivetrain + drivetrain.drive( + -gamepad1.right_stick_y, + gamepad1.right_stick_x, + gamepad1.left_stick_x + ); follower.update(); + shooter.setFlywheelVelocity(flywheelVelo); robot.setHoodPos(hoodPos); - shooter.setTurretPosition(turretPos); - shooter.setFlywheelVelocity(flywheel_velo); - double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage); +// shooter.setTurretPosition(turretPos); + shooter.update(robot.voltage.getVoltage()); + spindexerTransferIntake.update(); - robot.setSpinPos(ServoPositions.spindexer_A2); + SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); - if (intake && !shoot) { + if (gamepad1.leftBumperWasPressed() && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF || + state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_IN || + state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) { - shootState = ShootState.IDLE; - - robot.setRapidFireBlockerPos( - ServoPositions.rapidFireBlocker_Closed); - - robot.setTransferPower(transferIntakePower); - robot.setIntakePower(intakePower); - robot.setTransferServoPos( - ServoPositions.transferServo_out); - } else if (shoot) { - robot.setIntakePower(intakePower); - - - switch (shootState) { - - case IDLE: - - robot.setTransferPower(transferShootPower); - - timestamp = System.currentTimeMillis(); - shootState = ShootState.WAIT_GATE; - - break; - - case WAIT_GATE: - - if (System.currentTimeMillis() - timestamp >= 300) { - - robot.setRapidFireBlockerPos( - ServoPositions.rapidFireBlocker_Open); - - timestamp = System.currentTimeMillis(); - shootState = ShootState.WAIT_PUSH; - } - - break; - - case WAIT_PUSH: - - if (System.currentTimeMillis() - timestamp >= 100) { - - robot.setTransferServoPos( - ServoPositions.transferServo_in); - - shootState = ShootState.IDLE; - } - - break; - } + spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); } - TELE.addData("Flywheel Velocity1", (robot.shooter1.getVelocity() * 60) / 28); - TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28); - 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(); + if (gamepad1.right_trigger > 0.5 && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) { - shooter.update(); + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.HOLD_BALLS + ); + } + if (gamepad1.rightBumperWasPressed() + && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) { + + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.INTAKE + ); + } + + if (gamepad1.dpad_down){ + parkTilter.park(); + } else if (gamepad1.dpad_up) { + parkTilter.unpark(); + } + + TELE.addData("Distance From Goal", commander.getDistance()); + TELE.addData("Hood Position", commander.getHoodPredicted()); + TELE.addData("Transfer Power", commander.getTransferPow()); + TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM()); + TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity()); + + TELE.update(); } + } -} \ No newline at end of file +} 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 a8e8d64..c4ef6b7 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 @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.utilsv2; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -9,6 +10,7 @@ import org.firstinspires.ftc.teamcode.utilsv2.Robot; import java.util.LinkedList; +@Config public class Flywheel { Robot robot; @@ -70,8 +72,9 @@ public class Flywheel { private double prevF = 0; - public static double voltagePIDFDifference = 0.8; - public void setF(double f){ + public static double voltagePIDFDifference = 1; + public void setF(double voltage){ + double f = shooterPIDF_F / voltage; if (Math.abs(prevF - f) > voltagePIDFDifference) { shooterPIDF.f = f; robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); 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 6849d11..cde9526 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,9 +1,12 @@ package org.firstinspires.ftc.teamcode.utilsv2; +import static org.firstinspires.ftc.teamcode.utilsv2.Flywheel.*; + +import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.pedropathing.follower.Follower; - +@Config public class Shooter { Robot robot; @@ -15,18 +18,21 @@ public class Shooter { double goalY = 0.0; double obeliskX = 72; double obeliskY = 144; + double turretGoalX = 0; + double turretGoalY = 0; private boolean red = true; + public static boolean manualFlywheel = false; Follower follow; - public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance, Turret turret, Flywheel flywheel) { + public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance, Turret turret, Flywheel flywheel, VelocityCommander com) { this.robot = rob; this.fly = flywheel; this.turr = turret; this.follow = follower; - this.commander = new VelocityCommander(); + this.commander = com; setRedAlliance(redAlliance); @@ -37,10 +43,13 @@ public class Shooter { if (this.red) { goalX = 144; + turretGoalX = 136; } else { goalX = 0; + turretGoalX = 8; } goalY = 144; + turretGoalY = 136; } private double flywheelVelocity = 0.0; @@ -75,28 +84,32 @@ public class Shooter { } private final double shooterDistFromCenter = 1.545; - public void update() { + public void update(double voltage) { switch (state) { case NOTHING: break; case MANUAL: + manualFlywheel = true; 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() + follow.getAcceleration().getYComponent(), + voltage ); fly.manageFlywheel(flywheelVelocity); + fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage); turr.manual(turretPosition); break; case TRACK_GOAL: + manualFlywheel = false; turr.trackGoal( - (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), - (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), + (turretGoalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), + (turretGoalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getHeading(), follow.getAngularVelocity(), follow.getVelocity().getXComponent(), @@ -105,56 +118,70 @@ public class Shooter { follow.getAcceleration().getYComponent() ); - flywheelVelocity = commander.getVeloPredictive( + 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() + follow.getAcceleration().getYComponent(), + voltage ); + flywheelVelocity = commander.getPredictedRPM(); + robot.setHoodPos(commander.getHoodPredicted()); fly.manageFlywheel(flywheelVelocity); + fly.setF(voltage); break; case READ_OBELISK: + manualFlywheel = false; turr.trackObelisk( (obeliskX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), (obeliskY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getHeading() ); - flywheelVelocity = commander.getVeloPredictive( + 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() + follow.getAcceleration().getYComponent(), + voltage ); + flywheelVelocity = commander.getPredictedRPM(); + fly.manageFlywheel(flywheelVelocity); + fly.setF(voltage); break; case MANUAL_TURRET_TRACK_FLY: + manualFlywheel = false; turr.manual(turretPosition); - flywheelVelocity = commander.getVeloPredictive( + 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() + follow.getAcceleration().getYComponent(), + voltage ); + + flywheelVelocity = commander.getPredictedRPM(); robot.setHoodPos(commander.getHoodPredicted()); fly.manageFlywheel(flywheelVelocity); break; case MANUAL_FLYWHEEL_TRACK_TURR: + manualFlywheel = true; turr.trackGoal( - (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), - (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), + (turretGoalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), + (turretGoalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getHeading(), follow.getAngularVelocity(), follow.getVelocity().getXComponent(), @@ -162,7 +189,18 @@ public class Shooter { follow.getVelocity().getYComponent(), follow.getAcceleration().getYComponent() ); + 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(), + voltage + ); fly.manageFlywheel(flywheelVelocity); + fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage); + fly.setF(voltage); break; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java index 96db551..521ab3e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java @@ -1,11 +1,16 @@ package org.firstinspires.ftc.teamcode.utilsv2; +import android.health.connect.datatypes.units.Velocity; + +import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.hardware.NormalizedRGBA; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.constants.ServoPositions; +import org.firstinspires.ftc.teamcode.tests.NewShooterTest; +@Config public class SpindexerTransferIntake { private final Robot robot; @@ -236,10 +241,6 @@ public class SpindexerTransferIntake { public void update() { - if (mode == SpindexerMode.RAPID && rapidMode == RapidMode.INTAKE){ - robot.setTransferPower(-0.7); - } - switch (mode) { case RAPID: @@ -259,6 +260,7 @@ public class SpindexerTransferIntake { robot.setSpinPos( ServoPositions.spindexer_A2 ); + robot.setTransferPower(-0.7); robot.setTransferServoPos( ServoPositions.transferServo_out ); @@ -276,6 +278,7 @@ public class SpindexerTransferIntake { if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) { setRapidMode(RapidMode.BEFORE_PULSE_OUT); } + robot.setTransferPower(-0.3); break; @@ -333,6 +336,12 @@ public class SpindexerTransferIntake { setRapidMode(RapidMode.SHOOT); } + if (Shooter.manualFlywheel){ + robot.setTransferPower(NewShooterTest.transferPower); + } else { + robot.setTransferPower(commander.getTransferPow()); + } + break; case SHOOT: @@ -343,6 +352,13 @@ public class SpindexerTransferIntake { if (stateTime() >= 400) { setRapidMode(RapidMode.INTAKE); } + + if (Shooter.manualFlywheel){ + robot.setTransferPower(NewShooterTest.transferPower); + } else { + robot.setTransferPower(commander.getTransferPow()); + } + break; } break; 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 index c2484c8..790c3c2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java @@ -1,13 +1,16 @@ package org.firstinspires.ftc.teamcode.utilsv2; +import com.acmerobotics.dashboard.config.Config; + +@Config 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 static double xVelK = 0.05; // TODO: Tune + public static double xAccK = 0.025; // TODO: Tune + public static double yVelK = 0.05; // TODO: Tune + public static double yAccK = 0.025; // TODO: Tune private double hoodPos = 0.88; private double transferPow = -1; + private double velo = 0; public VelocityCommander() {} @@ -23,7 +26,6 @@ public class VelocityCommander { private final double veloJ = 272005.7124; private final double veloK = -2474581.713; private double distToRPM (double dist){ - double velo = 0; if (dist < 49) { velo = 2000; } else if (dist > 165){ @@ -46,7 +48,7 @@ public class VelocityCommander { } private final double hoodA = -4.3276177*Math.pow(10, -13); - private final double hoodB = 2.68062979*Math.pow(10, -11); + private final double hoodB = 2.68062979*Math.pow(10, -10); private final double hoodC = -7.12859632*Math.pow(10, -8); private final double hoodD = 0.0000106010785; private final double hoodE = -0.000960693973; @@ -73,8 +75,11 @@ public class VelocityCommander { hoodPos = Math.max(0.35, Math.min(0.88, hoodPos)); } } + public double getHoodPredicted(){ + return hoodPos; + } - private void distToTransferPow(double dist){ + private void distToTransferPow(double dist, double voltage){ if (dist < 118){ transferPow = -1; } else if (dist < 125){ @@ -82,32 +87,31 @@ public class VelocityCommander { } else { transferPow = -0.5; } - } +// transferPow = Math.max(-0.5, Math.min(-1, transferPow * (14/voltage))); + } public double getTransferPow(){return transferPow;} - public double getHoodPredicted(){ - return hoodPos; - } - // 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) { + public void getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc, double voltage) { 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 - predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight); + double goalHeight = 28; + predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight * goalHeight); distToHood(predictedDist); - - return distToRPM(predictedDist); + distToTransferPow(predictedDist, voltage); + distToRPM(predictedDist); } + public double getPredictedRPM(){return velo;} + public double getDistance(){return predictedDist;} }