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

yoooo

Browse Source
This commit is contained in:
Keshav Anand
2026-01-16 20:29:32 -06:00
parent 4aca64f61c
commit ef5d615f91
5 changed files with 235 additions and 246 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
@Config @Config
public class ServoPositions { public class ServoPositions {
public static double spindexer_intakePos1 = 0.34; public static double spindexer_intakePos1 = 0.39;
public static double spindexer_intakePos2 = 0.5; public static double spindexer_intakePos2 = 0.5;

454
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -3,8 +3,13 @@ package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.utils.Servos.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.utils.Servos.spin_scalar; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turrDefault;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinD;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinF;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinI;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinP;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
@@ -16,11 +21,11 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions; import com.acmerobotics.roadrunner.ftc.Actions;
import com.arcrobotics.ftclib.controller.PIDFController; import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.lynx.LynxModule;
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;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
@@ -39,7 +44,9 @@ public class TeleopV3 extends LinearOpMode {
public static double hoodDefaultPos = 0.5; public static double hoodDefaultPos = 0.5;
public static double desiredTurretAngle = 180; public static double desiredTurretAngle = 180;
public static double shootStamp2 = 0.0; public static double shootStamp2 = 0.0;
public static double spinningPow = 0.15; public static double spinningPow = 0.2;
public static double spindexPos = spindexer_intakePos1;
public static double spinPow = 0.08;
public double vel = 3000; public double vel = 3000;
public boolean autoVel = true; public boolean autoVel = true;
public double manualOffset = 0.0; public double manualOffset = 0.0;
@@ -51,11 +58,9 @@ public class TeleopV3 extends LinearOpMode {
public boolean circle = false; public boolean circle = false;
public boolean square = false; public boolean square = false;
public boolean triangle = false; public boolean triangle = false;
PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF);
public TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200); public TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200);
public ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200); public ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200);
PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF);
Robot robot; Robot robot;
MultipleTelemetry TELE; MultipleTelemetry TELE;
Servos servo; Servos servo;
@@ -84,6 +89,7 @@ public class TeleopV3 extends LinearOpMode {
boolean shootB = true; boolean shootB = true;
boolean shootC = true; boolean shootC = true;
boolean manualTurret = false; boolean manualTurret = false;
boolean autoSpintake = true;
List<Integer> shootOrder = new ArrayList<>(); List<Integer> shootOrder = new ArrayList<>();
boolean outtake1 = false; boolean outtake1 = false;
boolean outtake2 = false; boolean outtake2 = false;
@@ -92,7 +98,6 @@ public class TeleopV3 extends LinearOpMode {
double turretPID = 0.0; double turretPID = 0.0;
double turretPos = 0.5; double turretPos = 0.5;
double spindexPID = 0.0; double spindexPID = 0.0;
public static double spindexPos = spindexer_intakePos1;
double error = 0.0; double error = 0.0;
double spinCurrentPos = 0.0, spinInitPos = 0.0, intakeStamp = 0.0; double spinCurrentPos = 0.0, spinInitPos = 0.0, intakeStamp = 0.0;
boolean reverse = false; boolean reverse = false;
@@ -146,11 +151,14 @@ public class TeleopV3 extends LinearOpMode {
while (opModeIsActive()) { while (opModeIsActive()) {
//DRIVETRAIN: //DRIVETRAIN:
double y = 0.0;
double x = 0.0;
double rx = 0.0;
if (!autoDrive) { if (!autoDrive) {
double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed
double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
double rx = gamepad1.left_stick_x; rx = gamepad1.left_stick_x;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator; double frontLeftPower = (y + x + rx) / denominator;
@@ -197,51 +205,16 @@ public class TeleopV3 extends LinearOpMode {
} }
// // PID SERVOS // PID SERVOS
// turretPID = servo.setTurrPos(turretPos); turretPID = servo.setTurrPos(turretPos);
// robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
// robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
spinCurrentPos = servo.getSpinPos();
double spindexPID = spinPID.calculate(spinCurrentPos, spindexPos);
robot.spin1.setPower(spindexPID);
robot.spin2.setPower(-spindexPID);
}
//TODO: Use color sensors to switch between positions...switch after ball detected in //TODO: Use color sensors to switch between positions...switch after ball detected in
if (gamepad1.right_bumper) { if (autoSpintake) {
intakeTicker++;
if (intakeTicker % 3 == 0) { if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
spinCurrentPos = servo.getSpinPos();
if (Math.abs(spinCurrentPos - spinInitPos) < 0.02) {
reverse = true;
} else {
reverse = false;
}
spinInitPos = spinCurrentPos;
}
if (reverse && intakeTicker % intakeJamSwap < (intakeJamSwap / 2)) {
robot.spin1.setPower(1);
robot.spin2.setPower(-1);
} else if (reverse) {
robot.spin1.setPower(-1);
robot.spin2.setPower(1);
} else {
robot.spin1.setPower(-spinningPow);
robot.spin2.setPower(spinningPow);
}
robot.intake.setPower(1);
intakeStamp = getRuntime();
TELE.addData("Reverse?", reverse);
TELE.update();
} else {
if (!servo.spinEqual(spindexPos)) {
spinCurrentPos = servo.getSpinPos(); spinCurrentPos = servo.getSpinPos();
double spindexPID = spinPID.calculate(spinCurrentPos, spindexPos); double spindexPID = spinPID.calculate(spinCurrentPos, spindexPos);
@@ -249,19 +222,58 @@ public class TeleopV3 extends LinearOpMode {
robot.spin1.setPower(spindexPID); robot.spin1.setPower(spindexPID);
robot.spin2.setPower(-spindexPID); robot.spin2.setPower(-spindexPID);
} else {
robot.spin1.setPower(0);
robot.spin2.setPower(0);
} }
if (getRuntime() - intakeStamp < 1) { if (gamepad1.right_bumper) {
robot.intake.setPower(-(getRuntime() - intakeStamp) * 2); intakeTicker++;
if (intakeTicker % 4 == 0) {
spinCurrentPos = servo.getSpinPos();
if (Math.abs(spinCurrentPos - spinInitPos) < 0.02) {
reverse = true;
} else {
reverse = false;
}
spinInitPos = spinCurrentPos;
}
if (intakeTicker % 12 < 3) {
robot.spin1.setPower(-1);
robot.spin2.setPower(1);
} else if (reverse) {
robot.spin1.setPower(1);
robot.spin2.setPower(-1);
} else {
robot.spin1.setPower(-spinningPow);
robot.spin2.setPower(spinningPow);
}
robot.intake.setPower(1);
intakeStamp = getRuntime();
TELE.addData("Reverse?", reverse);
TELE.update();
} else { } else {
if (!servo.spinEqual(spindexPos)) {
spinCurrentPos = servo.getSpinPos();
double spindexPID = spinPID.calculate(spinCurrentPos, spindexPos);
robot.spin1.setPower(spindexPID);
robot.spin2.setPower(-spindexPID);
} else {
robot.spin1.setPower(0);
robot.spin2.setPower(0);
}
spindexPos = spindexer_intakePos1;
robot.intake.setPower(0); robot.intake.setPower(0);
}
intakeTicker = 0; intakeTicker = 0;
}
} }
//COLOR: //COLOR:
@@ -403,83 +415,82 @@ public class TeleopV3 extends LinearOpMode {
manualVel = 3100; manualVel = 3100;
} }
// //TODO: test the camera teleop code //TODO: test the camera teleop code
// double pos = turrDefault + (error / 8); // adds the overall error to the default double pos = turrDefault + (error / 8); // adds the overall error to the default
//
// TELE.addData("offset", offset);
//
// pos -= offset * (0.9 / 360);
//
// if (pos < 0.02) {
// pos = 0.02;
// } else if (pos > 0.97) {
// pos = 0.97;
// }
//
// if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1) { //not moving
// double bearing;
//
// LLResult result = robot.limelight.getLatestResult();
// if (result != null) {
// if (result.isValid()) {
// bearing = result.getTx();
// overrideTurr = true;
// turretPos = servo.getTurrPos() - (bearing / 1300);
//
// double bearingCorrection = bearing / 1300;
//
// // deadband: ignore tiny noise
// if (Math.abs(bearing) > 0.3 && camTicker < 8) {
//
// // only accumulate if bearing direction is consistent
// if (Math.signum(bearingCorrection) == Math.signum(error) ||
// error == 0) {
// error += bearingCorrection;
// }
// }
// camTicker++;
// TELE.addData("tx", bearing);
// TELE.addData("ty", result.getTy());
// }
// }
//
// } else {
// camTicker = 0;
// overrideTurr = false;
// }
//
// if (manualTurret) {
// pos = turrDefault + (manualOffset / 100);
// }
//
// if (!overrideTurr) {
// turretPos = pos;
// }
//
// if (gamepad2.dpad_right) {
// manualOffset -= 2;
// } else if (gamepad2.dpad_left) {
// manualOffset += 2;
// }
//
// //HOOD: TELE.addData("offset", offset);
//
// if (autoHood) { pos -= offset * (0.9 / 360);
// robot.hood.setPosition(hoodAnglePrediction(distanceToGoal) + autoHoodOffset);
// } else { if (pos < 0.02) {
// robot.hood.setPosition(hoodDefaultPos + hoodOffset); pos = 0.02;
// } } else if (pos > 0.97) {
// pos = 0.97;
// if (gamepad2.dpadUpWasPressed()) { }
// hoodOffset -= 0.03;
// autoHoodOffset -= 0.02; if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving
// double bearing;
// } else if (gamepad2.dpadDownWasPressed()) {
// hoodOffset += 0.03; LLResult result = robot.limelight.getLatestResult();
// autoHoodOffset += 0.02; if (result != null) {
// if (result.isValid()) {
// } bearing = result.getTx();
overrideTurr = true;
turretPos = servo.getTurrPos() - (bearing / 1300);
double bearingCorrection = bearing / 1300;
// deadband: ignore tiny noise
if (Math.abs(bearing) > 0.3 && camTicker < 8) {
// only accumulate if bearing direction is consistent
if (Math.signum(bearingCorrection) == Math.signum(error) ||
error == 0) {
error += bearingCorrection;
}
}
camTicker++;
TELE.addData("tx", bearing);
TELE.addData("ty", result.getTy());
}
}
} else {
camTicker = 0;
overrideTurr = false;
}
if (manualTurret) {
pos = turrDefault + (manualOffset / 100);
}
if (!overrideTurr) {
turretPos = pos;
}
if (gamepad2.dpad_right) {
manualOffset -= 2;
} else if (gamepad2.dpad_left) {
manualOffset += 2;
}
//HOOD:
if (autoHood) {
robot.hood.setPosition(hoodAnglePrediction(distanceToGoal) + autoHoodOffset);
} else {
robot.hood.setPosition(hoodDefaultPos + hoodOffset);
}
if (gamepad2.dpadUpWasPressed()) {
hoodOffset -= 0.03;
autoHoodOffset -= 0.02;
} else if (gamepad2.dpadDownWasPressed()) {
hoodOffset += 0.03;
autoHoodOffset += 0.02;
}
// //
// //TODO: FIX THIS GOOFY THING BECAUSE IT SUCKS @KeshavAnandCode // //TODO: FIX THIS GOOFY THING BECAUSE IT SUCKS @KeshavAnandCode
// if (gamepad2.left_stick_x > 0.5) { // if (gamepad2.left_stick_x > 0.5) {
@@ -504,83 +515,54 @@ public class TeleopV3 extends LinearOpMode {
// headingOffset = robotHeading; // headingOffset = robotHeading;
// } // }
// } // }
//
// if (shootAll) { if (gamepad1.leftBumperWasPressed()) {
//
// TELE.addData("100% works", shootOrder); robot.transferServo.setPosition(transferServo_out);
//
// intake = false; autoSpintake = false;
// reject = false;
// robot.spin1.setPower(1);
// if (!shootOrder.isEmpty() && (getRuntime() - shootStamp < 12)) { robot.spin2.setPower(-1);
// int currentSlot = shootOrder.get(0); // Peek, do NOT remove yet
// boolean shootingDone = false; }
//
// if (!outtake1) { if (gamepad1.leftBumperWasReleased()) {
// outtake1 = (servo.spinEqual(spindexer_outtakeBall1)); shootStamp = getRuntime();
// } shootAll = true;
// if (!outtake2) { spindexPos = spindexer_intakePos1;
// outtake2 = (servo.spinEqual(spindexer_outtakeBall2)); }
// }
// if (!outtake3) { if (shootAll) {
// outtake3 = (servo.spinEqual(spindexer_outtakeBall3));
// } TELE.addData("100% works", shootOrder);
//
// switch (currentSlot) { intake = false;
// case 1: reject = false;
// shootA = shootTeleop(spindexer_outtakeBall1, outtake1, shootStamp2);
// TELE.addData("shootA", shootA); spindexPos = spindexer_intakePos1;
//
// if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) { if (getRuntime() - shootStamp < 2.5) {
// shootingDone = !shootA;
// } else { robot.transferServo.setPosition(transferServo_in);
// shootingDone = true;
// } autoSpintake = false;
// break;
// case 2: robot.spin1.setPower(-spinPow);
// shootB = shootTeleop(spindexer_outtakeBall2, outtake2, shootStamp2); robot.spin2.setPower(spinPow);
// TELE.addData("shootB", shootB);
// if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) { } else {
// shootingDone = !shootB; robot.transferServo.setPosition(transferServo_out);
// } else { spindexPos = spindexer_intakePos1;
// shootingDone = true;
// } shootAll = false;
// break;
// case 3: autoSpintake = true;
// shootC = shootTeleop(spindexer_outtakeBall3, outtake3, shootStamp2);
// TELE.addData("shootC", shootC); robot.transferServo.setPosition(transferServo_out);
// if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) { }
// shootingDone = !shootC;
// } else { }
// shootingDone = true;
// }
// break;
// }
//
// // Remove from the list only if shooting is complete
// if (shootingDone) {
// shootOrder.remove(0);
// shootStamp2 = getRuntime();
//
// }
//
// } else {
// // Finished shooting all balls
// spindexPos = spindexer_intakePos1;
// shootA = true;
// shootB = true;
// shootC = true;
// reject = false;
// intake = true;
// shootAll = false;
// outtake1 = false;
// outtake2 = false;
// outtake3 = false;
//
// overrideTurr = false;
//
// }
//
// if (gamepad1.squareWasPressed()) { // if (gamepad1.squareWasPressed()) {
// square = true; // square = true;
// shootStamp = getRuntime(); // shootStamp = getRuntime();
@@ -696,13 +678,18 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("pose", drive.localizer.getPose()); TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
// TELE.addData("distanceToGoal", distanceToGoal); TELE.addData("distanceToGoal", distanceToGoal);
TELE.addData("hood", robot.hood.getPosition()); TELE.addData("hood", robot.hood.getPosition());
TELE.addData("targetVel", vel); TELE.addData("targetVel", vel);
TELE.addData("Velocity", flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()));
TELE.addData("shootOrder", shootOrder); TELE.addData("shootOrder", shootOrder);
TELE.addData("oddColor", oddBallColor); TELE.addData("oddColor", oddBallColor);
TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1));
TELE.addData("autoSpintake", autoSpintake);
TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.update(); TELE.update();
ticker++; ticker++;
@@ -772,24 +759,25 @@ public class TeleopV3 extends LinearOpMode {
// } // }
// } // }
// //
// public double hoodAnglePrediction(double x) { public double hoodAnglePrediction(double x) {
// if (x < 34) { if (x < 34) {
// double L = 1.04471; double L = 1.04471;
// double U = 0.711929; double U = 0.711929;
// double Q = 120.02263; double Q = 120.02263;
// double B = 0.780982; double B = 0.780982;
// double M = 20.61191; double M = 20.61191;
// double v = 10.40506; double v = 10.40506;
//
// double inner = 1 + Q * Math.exp(-B * (x - M)); double inner = 1 + Q * Math.exp(-B * (x - M));
// return L + (U - L) / Math.pow(inner, 1.0 / v); return L + (U - L) / Math.pow(inner, 1.0 / v);
//
// } else { } else {
// // x >= 34 // x >= 34
// return 1.94372 * Math.exp(-0.0528731 * x) + 0.39; return 1.94372 * Math.exp(-0.0528731 * x) + 0.39;
// } }
// } }
//
//
// void addOddThenRest(List<Integer> order, boolean oddColor) { // void addOddThenRest(List<Integer> order, boolean oddColor) {
// // Odd ball first // // Odd ball first
// for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i); // for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i);

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java
View File

@@ -31,7 +31,7 @@ public class PIDServoTest extends LinearOpMode {
PIDFController controller = new PIDFController(p, i, d, f); PIDFController controller = new PIDFController(p, i, d, f);
controller.setTolerance(0); controller.setTolerance(0.001);
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
@@ -44,12 +44,14 @@ public class PIDServoTest extends LinearOpMode {
controller.setPIDF(p, i, d, f); controller.setPIDF(p, i, d, f);
if (mode == 0) { if (mode == 0) {
pos = robot.turr1Pos.getCurrentPosition(); pos = (double) ((double)robot.turr1Pos.getCurrentPosition() /1024.0) * ((double) 44.0 /(double)77.0);
double pid = controller.calculate(pos, target); double pid = controller.calculate(pos, target);
robot.turr1.setPower(pid); robot.turr1.setPower(pid);
robot.turr2.setPower(-pid); robot.turr2.setPower(-pid);
} else if (mode == 1) { } else if (mode == 1) {
pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3); pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3);

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java
View File

@@ -84,7 +84,7 @@ public class FlywheelV2 {
double d = -0.289647; double d = -0.289647;
double h = -1.1569; double h = -1.1569;
double feed = Math.log((a / (targetVelocity + c)) + d) / h; double feed = Math.log10((a / (targetVelocity + c)) + d) / h;
// --- PROPORTIONAL CORRECTION --- // --- PROPORTIONAL CORRECTION ---
double error = targetVelocity - velo; double error = targetVelocity - velo;
@@ -94,7 +94,7 @@ public class FlywheelV2 {
correction = Math.max(-maxStep, Math.min(maxStep, correction)); correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER --- // --- FINAL MOTOR POWER ---
powPID = feed; powPID = feed + correction;
// clamp to allowed range // clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID)); powPID = Math.max(0, Math.min(1, powPID));

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
View File

@@ -6,26 +6,24 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
@Config @Config
public class Servos { public class Servos {
Robot robot;
PIDFController spinPID;
PIDFController turretPID;
//PID constants //PID constants
// TODO: get PIDF constants // TODO: get PIDF constants
public static double spinP = 3.3, spinI = 0, spinD = 0.1, spinF = 0.02; public static double spinP = 3.3, spinI = 0, spinD = 0.1, spinF = 0.02;
public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF = 0.0; public static double turrP = 1.1, turrI = 0.25, turrD = 0.0625, turrF = 0.01;
public static double spin_scalar = 1.0086; public static double spin_scalar = 1.0086;
public static double spin_restPos = 0.0; public static double spin_restPos = 0.0;
public static double turret_scalar = 1.009; public static double turret_scalar = 1.009;
public static double turret_restPos = 0.0; public static double turret_restPos = 0.0;
Robot robot;
PIDFController spinPID;
PIDFController turretPID;
public Servos(HardwareMap hardwareMap) { public Servos(HardwareMap hardwareMap) {
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
spinPID = new PIDFController(spinP, spinI, spinD, spinF); spinPID = new PIDFController(spinP, spinI, spinD, spinF);
turretPID = new PIDFController(turrP, turrI, turrD, turrF); turretPID = new PIDFController(turrP, turrI, turrD, turrF);
turretPID.setTolerance(0.001);
} }
// In the code below, encoder = robot.servo.getVoltage() // In the code below, encoder = robot.servo.getVoltage()
@@ -33,6 +31,7 @@ public class Servos {
public double getSpinPos() { public double getSpinPos() {
return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3); return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3);
} }
//TODO: PID warp so 0 and 1 are usable positions //TODO: PID warp so 0 and 1 are usable positions
public double setSpinPos(double pos) { public double setSpinPos(double pos) {
spinPID.setPIDF(spinP, spinI, spinD, spinF); spinPID.setPIDF(spinP, spinI, spinD, spinF);