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 70f8f78..2f4756a 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 @@ -33,6 +33,8 @@ public class TeleopV4 extends LinearOpMode { public static Pose relocalizePose = new Pose(56, 11, 0); public static Pose teleStart = new Pose(0,0,0); + private boolean firstTickFull = true; + @Override public void runOpMode() throws InterruptedException { @@ -77,6 +79,25 @@ public class TeleopV4 extends LinearOpMode { TELE.addData("TELE START", teleStart); TELE.update(); + while (opModeInInit()){ + if (gamepad1.triangleWasPressed()){ + VelocityCommander.lockFront = true; + VelocityCommander.lockBack = false; + } else if (gamepad1.squareWasPressed()){ + VelocityCommander.lockBack = true; + VelocityCommander.lockFront = false; + } else if (gamepad1.circleWasPressed()){ + VelocityCommander.lockBack = false; + VelocityCommander.lockFront = false; + } + TELE.addLine("Initialization is done"); + TELE.addData("Starting Position", follower.getPose()); + TELE.addData("TELE START", teleStart); + TELE.addData("Front?:", VelocityCommander.lockFront); + TELE.addData("Back?:", VelocityCommander.lockBack); + TELE.update(); + } + waitForStart(); if (isStopRequested()) return; @@ -86,7 +107,8 @@ public class TeleopV4 extends LinearOpMode { drivetrain.drive( -gamepad1.right_stick_y, gamepad1.right_stick_x, - gamepad1.left_stick_x + gamepad1.left_stick_x, + gamepad1.left_trigger ); if (gamepad1.crossWasPressed()){ @@ -116,6 +138,23 @@ public class TeleopV4 extends LinearOpMode { robot.setHoodPos(0.6); } + if (gamepad1.triangleWasPressed()){ + VelocityCommander.lockFront = true; + VelocityCommander.lockBack = false; + TELE.addData("Front?:", VelocityCommander.lockFront); + TELE.addData("Back?:", VelocityCommander.lockBack); + } else if (gamepad1.squareWasPressed()){ + VelocityCommander.lockBack = true; + VelocityCommander.lockFront = false; + TELE.addData("Front?:", VelocityCommander.lockFront); + TELE.addData("Back?:", VelocityCommander.lockBack); + } else if (gamepad1.circleWasPressed()){ + VelocityCommander.lockBack = false; + VelocityCommander.lockFront = false; + TELE.addData("Front?:", VelocityCommander.lockFront); + TELE.addData("Back?:", VelocityCommander.lockBack); + } + shooter.update(robot.voltage.getVoltage()); spindexerTransferIntake.update(); @@ -130,6 +169,13 @@ public class TeleopV4 extends LinearOpMode { state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) { spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); + SpindexerTransferIntake.intakeFull = false; + firstTickFull = true; + } + + if (SpindexerTransferIntake.intakeFull && firstTickFull){ + gamepad1.rumble(100); + firstTickFull = false; } if (gamepad1.right_trigger > 0.5 && @@ -159,20 +205,20 @@ public class TeleopV4 extends LinearOpMode { } loopTimes.loop(); - TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime()); - TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin()); - TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin()); - +// TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime()); +// TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin()); +// TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin()); +// TELE.addData("Distance From Goal", commander.getDistance()); - TELE.addData("Hood Position", commander.getHoodPredicted()); - TELE.addData("Transfer Power", robot.transfer.getPower()); +// 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.addData("Current Position", currentPose); - - TELE.addData("Current LL Pipeline", turret.pipeline()); - +// +// TELE.addData("Current Position", currentPose); +// +// TELE.addData("Current LL Pipeline", turret.pipeline()); +// 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 b35ef9d..446584c 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 @@ -81,17 +81,20 @@ public class NewShooterTest extends LinearOpMode { drivetrain.drive( -gamepad1.right_stick_y, gamepad1.right_stick_x, - gamepad1.left_stick_x + gamepad1.left_stick_x, + gamepad1.left_trigger ); - if (Color.redAlliance){ - TeleopV4.relocalizePose = new Pose(57.5, 5, 0); - } else { - TeleopV4.relocalizePose = new Pose(-57.5, 5, Math.toRadians(180)); + if (gamepad1.crossWasPressed()){ + if (Color.redAlliance){ + TeleopV4.relocalizePose = new Pose(57.5, 5, 0); + } else { + TeleopV4.relocalizePose = new Pose(-57.5, 5, Math.toRadians(180)); + } + follower.setPose(TeleopV4.relocalizePose); + sleep(500); + gamepad1.rumble(100); } - follower.setPose(TeleopV4.relocalizePose); - sleep(500); - gamepad1.rumble(100); follower.update(); @@ -154,6 +157,7 @@ public class NewShooterTest extends LinearOpMode { TELE.addData("Hood Position", commander.getHoodPredicted()); TELE.addData("Transfer Power", commander.getTransferPow()); TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM()); + TELE.addData("Manuel Velocity RPM", flywheelVelo); TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity()); TELE.addData("TX:", turret.getTX()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java index f34b4fb..b9a65eb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java @@ -91,7 +91,8 @@ public class SortedSpindexerTest extends LinearOpMode { drivetrain.drive( -gamepad1.right_stick_y, gamepad1.right_stick_x, - gamepad1.left_stick_x + gamepad1.left_stick_x, + gamepad1.left_trigger ); follower.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java index 8623588..4d76bc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java @@ -37,7 +37,7 @@ public class Drivetrain { tele = input; } - public void drive(double y, double x, double rx) { + public void drive(double y, double x, double rx, double stop) { boolean snappedForward = false; boolean snappedStrafe = false; @@ -79,18 +79,30 @@ public class Drivetrain { robot.setFrontRightPower(frontRightPower); robot.setBackRightPower(backRightPower); - if (tele) { - - telemetry.addData("Forward Snap", snappedForward); - telemetry.addData("Strafe Snap", snappedStrafe); - - telemetry.addData("Correction RX", correctionRX); - - telemetry.addData("FL", frontLeftPower); - telemetry.addData("BL", backLeftPower); - telemetry.addData("FR", frontRightPower); - telemetry.addData("BR", backRightPower); + if (stop > 0.5){ + robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + robot.setFrontLeftPower(0); + robot.setBackLeftPower(0); + robot.setFrontRightPower(0); + robot.setBackRightPower(0); } + +// if (tele) { +// +// telemetry.addData("Forward Snap", snappedForward); +// telemetry.addData("Strafe Snap", snappedStrafe); +// +// telemetry.addData("Correction RX", correctionRX); +// +// telemetry.addData("FL", frontLeftPower); +// telemetry.addData("BL", backLeftPower); +// telemetry.addData("FR", frontRightPower); +// telemetry.addData("BR", backRightPower); +// +// } } } \ 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 c4ef6b7..4d64b52 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,6 +1,8 @@ package org.firstinspires.ftc.teamcode.utilsv2; import com.acmerobotics.dashboard.config.Config; +import com.arcrobotics.ftclib.controller.PIDFController; +import com.arcrobotics.ftclib.controller.wpilibcontroller.SimpleMotorFeedforward; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -16,10 +18,20 @@ public class Flywheel { // public PIDFCoefficients shooterPIDF1, shooterPIDF2; public static PIDFCoefficients shooterPIDF; + PIDFController pidf; + SimpleMotorFeedforward feedforward; + + public static double kS = 0.01; // Static feedforward + public static double kV = 0.0001935; // Velocity feedforward + public static double shooterPIDF_P = 500; public static double shooterPIDF_I = 1; public static double shooterPIDF_D = 0.0; public static double shooterPIDF_F = 93; +// public static double shooterPIDF_P = 0.0001; +// public static double shooterPIDF_I = 0; +// public static double shooterPIDF_D = 0.00001; +// public static double shooterPIDF_F = 0; private double velo = 0.0; private double velo1 = 0.0; @@ -36,6 +48,8 @@ public class Flywheel { public Flywheel(Robot rob) { robot = rob; shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12); +// pidf = new PIDFController(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0); +// feedforward = new SimpleMotorFeedforward(kS, kV); } public double getVelo() { @@ -67,19 +81,23 @@ public class Flywheel { shooterPIDF.d = d; shooterPIDF.f = f; +// pidf.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0); + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); } private double prevF = 0; public static double voltagePIDFDifference = 1; + double averageVoltage = 0; public void setF(double voltage){ + averageVoltage = ALPHA * voltage + (1 - ALPHA) * averageVoltage; double f = shooterPIDF_F / voltage; - if (Math.abs(prevF - f) > voltagePIDFDifference) { + if (Math.abs(prevF - f) > voltagePIDFDifference && !steady) { shooterPIDF.f = f; robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); - prevF = f; } + prevF = f; } // Convert from RPM to Ticks per Second @@ -92,25 +110,30 @@ public class Flywheel { return (TPS * 60.0) / 28.0; } + double ALPHA = 0.3; private void updateVelocityAverage(double newVelocity) { - velocityHistory.add(newVelocity); +// velocityHistory.add(newVelocity); +// +// int velocityHistorySize = 5; +// if (velocityHistory.size() > velocityHistorySize) { +// velocityHistory.removeFirst(); +// } +// +// double sum = 0.0; +// +// for (double v : velocityHistory) { +// sum += v; +// } +// +// averageVelocity = sum / velocityHistory.size(); - int velocityHistorySize = 5; - if (velocityHistory.size() > velocityHistorySize) { - velocityHistory.removeFirst(); - } - - double sum = 0.0; - - for (double v : velocityHistory) { - sum += v; - } - - averageVelocity = sum / velocityHistory.size(); + averageVelocity = ALPHA * newVelocity + (1 - ALPHA) * averageVelocity; } double power; + double prevTargetTime = 0; + double prevTargetVelocity = 0; public void manageFlywheel(double commandedVelocity) { if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) { @@ -125,13 +148,12 @@ public class Flywheel { velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); - velo = (velo1 + velo2) / 2.0; + velo = velo1; updateVelocityAverage(velo); steady = (Math.abs(commandedVelocity - averageVelocity) < 50); } - public double getShooterPower(){return power;} } \ No newline at end of file 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 6dea819..1eef705 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 @@ -104,7 +104,8 @@ public class Shooter { follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), follow.getAcceleration().getYComponent(), - voltage + voltage, + fly.getAverageVelocity() ); fly.manageFlywheel(flywheelVelocity); @@ -131,7 +132,8 @@ public class Shooter { follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), follow.getAcceleration().getYComponent(), - voltage + voltage, + fly.getAverageVelocity() ); flywheelVelocity = commander.getPredictedRPM(); @@ -153,7 +155,8 @@ public class Shooter { follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), follow.getAcceleration().getYComponent(), - voltage + voltage, + fly.getAverageVelocity() ); flywheelVelocity = commander.getPredictedRPM(); @@ -172,7 +175,8 @@ public class Shooter { follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), follow.getAcceleration().getYComponent(), - voltage + voltage, + fly.getAverageVelocity() ); flywheelVelocity = commander.getPredictedRPM(); @@ -200,7 +204,8 @@ public class Shooter { follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), follow.getAcceleration().getYComponent(), - voltage + voltage, + fly.getAverageVelocity() ); fly.manageFlywheel(flywheelVelocity); fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage); 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 4b4445d..c5667e3 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 @@ -274,22 +274,23 @@ public class SpindexerTransferIntake { private int greenTicks = 0; private int ballTicks = 0; private int holdBallsTicker = 0; + public static boolean intakeFull = true; public void update() { - TELE.addData("Sorted State", sortedIntakeStates); - TELE.addData("Ball0", ballColors[0]); - TELE.addData("Ball1", ballColors[1]); - TELE.addData("Ball2", ballColors[2]); - - TELE.addData("Shoot0", shootOrder[0]); - TELE.addData("Shoot1", shootOrder[1]); - TELE.addData("Shoot2", shootOrder[2]); - - TELE.addData("Color0", ballColors[0]); - TELE.addData("Color1", ballColors[1]); - TELE.addData("Color2", ballColors[2]); - - TELE.addData("Shoot State", shootState); +// TELE.addData("Sorted State", sortedIntakeStates); +// TELE.addData("Ball0", ballColors[0]); +// TELE.addData("Ball1", ballColors[1]); +// TELE.addData("Ball2", ballColors[2]); +// +// TELE.addData("Shoot0", shootOrder[0]); +// TELE.addData("Shoot1", shootOrder[1]); +// TELE.addData("Shoot2", shootOrder[2]); +// +// TELE.addData("Color0", ballColors[0]); +// TELE.addData("Color1", ballColors[1]); +// TELE.addData("Color2", ballColors[2]); +// +// TELE.addData("Shoot State", shootState); switch (mode) { @@ -317,9 +318,13 @@ public class SpindexerTransferIntake { if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) { - setRapidMode(RapidMode.TRANSFER_OFF); + holdBallsTicker++; } + if (holdBallsTicker > 10){ + setRapidMode(RapidMode.TRANSFER_OFF); + holdBallsTicker = 0; + } break; @@ -366,10 +371,12 @@ public class SpindexerTransferIntake { case HOLD_BALLS: if (robot.insideBeam.isPressed() - && robot.outsideBeam.isPressed() && holdBallsTicker > 10) { + && robot.outsideBeam.isPressed()) { - robot.setIntakePower(0.1); robot.setTransferPower(0); + robot.setIntakePower(0.1); + robot.setTransferServoPos(ServoPositions.transferServo_in); + intakeFull = true; } else { holdBallsTicker++; @@ -379,37 +386,42 @@ public class SpindexerTransferIntake { case OPEN_GATE: - robot.setRapidFireBlockerPos( - ServoPositions.rapidFireBlocker_Open - ); - if (stateTime() >= 100) { setRapidMode(RapidMode.SHOOT); } + robot.setTransferServoPos(ServoPositions.transferServo_in); + + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Open + ); + if (Shooter.manualFlywheel) { robot.setTransferPower(NewShooterTest.transferPower); + robot.setIntakePower(-NewShooterTest.transferPower); } else { robot.setTransferPower(commander.getTransferPow()); + robot.setIntakePower(-commander.getTransferPow()); } break; case SHOOT: - robot.setTransferServoPos( - ServoPositions.transferServo_in - ); - if (stateTime() >= 400) { + if (stateTime() >= 500) { setRapidMode(RapidMode.INTAKE); } if (Shooter.manualFlywheel) { robot.setTransferPower(NewShooterTest.transferPower); + robot.setIntakePower(-NewShooterTest.transferPower); } else { robot.setTransferPower(commander.getTransferPow()); + robot.setIntakePower(-commander.getTransferPow()); } + holdBallsTicker = 0; + break; } 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 c00914c..c2031ad 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 @@ -20,7 +20,7 @@ public class Turret { private final double turretMin = 0.05; private final double turretMax = 0.95; public static boolean limelightUsed = true; - public static double B_PID_P = 0.00015, B_PID_I = 0.0, B_PID_D = 0.00001; + public static double B_PID_P = 0.0001, B_PID_I = 0.0, B_PID_D = 0.000005; LLResult result; PIDController bearingPID; boolean bearingAligned = false; 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 e1a6b9c..09bc369 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 @@ -8,88 +8,71 @@ public class VelocityCommander { public static double xAccK = 0.025; // TODO: Tune public static double yVelK = 0.05; // TODO: Tune public static double yAccK = 0.025; // TODO: Tune + public static boolean lockFront = false; + public static boolean lockBack = false; + public static int farBound = 140; + public static int closeBound = 110; + public static double errorHoodAdjustment = 0.0005; private double hoodPos = 0.88; private double transferPow = -1; private int velo = 0; public VelocityCommander() {} - private final double veloA = -2.703087757*Math.pow(10, -14); - private final double veloB = 2.904756341*Math.pow(10, -11); - private final double veloC = -1.381814293*Math.pow(10, -8); - private final double veloD = 0.000003829224585; - private final double veloE = -0.000684090204; - private final double veloF = 0.0822754689; - private final double veloG = -6.743119277; - private final double veloH = 371.7359504; - private final double veloI = -13189.70958; - private final double veloJ = 272005.7124; - private final double veloK = -2474581.713; + final double veloA = -0.00000133612; + final double veloB = 0.000542733; + final double veloC = -0.0739531; + final double veloD = 5.16759; + final double veloE = 62.45781; private double distToRPM (double dist){ - double currentVelo = 0; - if (dist < 49) { - currentVelo = 2000; - } else if (dist > 165){ - velo = 3760; - } else { - currentVelo = veloA*Math.pow(dist, 10) + - veloB*Math.pow(dist, 9) + - veloC*Math.pow(dist, 8) + - veloD*Math.pow(dist, 7) + - veloE*Math.pow(dist, 6) + - veloF*Math.pow(dist, 5) + - veloG*Math.pow(dist, 4) + - veloH*Math.pow(dist, 3) + - veloI*Math.pow(dist, 2) + - veloJ*Math.pow(dist, 1) + - veloK; + double currentVelo; + if (lockFront && dist > closeBound){ + dist = closeBound; + } else if (lockBack && dist < farBound){ + dist = farBound; + } + if (dist < 54) { + velo = 2000; + } else if (dist > 181){ + velo = 3600; + } else { + currentVelo = veloA*Math.pow(dist, 4) + + veloB*Math.pow(dist, 3) + + veloC*Math.pow(dist, 2) + + veloD*Math.pow(dist, 1) + + veloE; + velo = 10 * Math.round((float) Math.max(200, Math.min(360, currentVelo))); } - velo = Math.round((float) Math.max(2000, Math.min(3760, currentVelo))); return velo; } - private final double hoodA = -4.3276177*Math.pow(10, -13); - 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; - private final double hoodF = 0.0540375808; - private final double hoodG = -1.82724027; - private final double hoodH = 33.4797545; - private final double hoodI = -246.888632; + final double hoodA = 9.04203*Math.pow(10, -8); + final double hoodB = -0.0000204165; + final double hoodC = -0.00252089; + final double hoodD = 1.06154; private void distToHood (double dist){ - if (dist > 112){ - hoodPos = 0.35; - } else if (dist < 49){ + if (dist > 174){ + hoodPos = 0.48; + } else if (dist < 54){ hoodPos = 0.88; } else { - hoodPos = hoodA*Math.pow(dist, 8) + - hoodB*Math.pow(dist, 7) + - hoodC*Math.pow(dist, 6) + - hoodD*Math.pow(dist, 5) + - hoodE*Math.pow(dist, 4) + - hoodF*Math.pow(dist, 3) + - hoodG*Math.pow(dist, 2) + - hoodH*Math.pow(dist, 1) + - hoodI; - - hoodPos = Math.max(0.35, Math.min(0.88, hoodPos)); + hoodPos = hoodA*Math.pow(dist, 3) + + hoodB*Math.pow(dist, 2) + + hoodC*Math.pow(dist, 1) + + hoodD; } + hoodPos = Math.max(0.48, Math.min(0.88, hoodPos)); } public double getHoodPredicted(){ return hoodPos; } private void distToTransferPow(double dist, double voltage){ - if (dist < 118){ - transferPow = -1; - } else if (dist < 125){ - transferPow = -0.7; + if (dist < 140){ + transferPow = -0.8; } else { transferPow = -0.5; } - -// transferPow = Math.max(-0.5, Math.min(-1, transferPow * (14/voltage))); } public double getTransferPow(){return transferPow;} @@ -99,7 +82,7 @@ public class VelocityCommander { } double predictedDist = 0; - public void getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc, double voltage) { + public void getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc, double voltage, double velocity) { 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 @@ -110,6 +93,17 @@ public class VelocityCommander { distToHood(predictedDist); distToTransferPow(predictedDist, voltage); distToRPM(predictedDist); + + hoodPos += adjustHood(predictedDist, velocity, velo); + } + + public double adjustHood(double dist, double currentVelocity, double targetVelocity){ + double error = targetVelocity - currentVelocity; + if (dist < farBound || error < 0){ + error = 0; + } + System.out.println("Error "+ error); + return error * errorHoodAdjustment; } public double getPredictedRPM(){return velo;}