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

stash

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-03 15:55:35 -06:00
parent 07297c60f1
commit 4b998766a1
10 changed files with 307 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java
View File

@@ -178,8 +178,8 @@ public class Blue_V2 extends LinearOpMode {
TELE.update();
if (gpp || pgp || ppg){
robot.turr1.setPosition(turret_blue);
robot.turr2.setPosition(1 - turret_blue);
robot.turr1.setPower(turret_blue);
robot.turr2.setPower(1 - turret_blue);
return false;
} else {
return true;
@@ -203,8 +203,8 @@ public class Blue_V2 extends LinearOpMode {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
if (ticker == 0) {
stamp = getRuntime();
@@ -264,8 +264,8 @@ public class Blue_V2 extends LinearOpMode {
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.spin1.setPosition(spindexer);
robot.spin2.setPosition(1-spindexer);
robot.spin1.setPower(spindexer);
robot.spin2.setPower(1-spindexer);
TELE.addData("Velocity", velo);
TELE.addLine("spindex");
TELE.update();
@@ -380,8 +380,8 @@ public class Blue_V2 extends LinearOpMode {
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
TELE.addData("Velocity", velo);
TELE.addLine("Intaking");
@@ -418,8 +418,8 @@ public class Blue_V2 extends LinearOpMode {
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
@@ -541,13 +541,13 @@ public class Blue_V2 extends LinearOpMode {
robot.hood.setPosition(hoodAuto);
robot.turr1.setPosition(turret_detectBlue);
robot.turr2.setPosition(1 - turret_detectBlue);
robot.turr1.setPower(turret_detectBlue);
robot.turr2.setPower(1 - turret_detectBlue);
robot.transferServo.setPosition(transferServo_out);
robot.spin1.setPosition(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1);
robot.spin1.setPower(spindexer_intakePos1);
robot.spin2.setPower(1 - spindexer_intakePos1);
aprilTag.update();
TELE.addData("Velocity", velo);

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java
View File

@@ -171,8 +171,8 @@ public class Red_V2 extends LinearOpMode {
TELE.update();
if (gpp || pgp || ppg){
robot.turr1.setPosition(turret_red);
robot.turr2.setPosition(1 - turret_red);
robot.turr1.setPower(turret_red);
robot.turr2.setPower(1 - turret_red);
return false;
} else {
return true;
@@ -190,8 +190,8 @@ public class Red_V2 extends LinearOpMode {
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.spin1.setPosition(spindexer);
robot.spin2.setPosition(1-spindexer);
robot.spin1.setPower(spindexer);
robot.spin2.setPower(1-spindexer);
TELE.addData("Velocity", velo);
TELE.addLine("spindex");
TELE.update();
@@ -272,8 +272,8 @@ public class Red_V2 extends LinearOpMode {
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
TELE.addData("Velocity", velo);
TELE.addLine("Intaking");
@@ -331,8 +331,8 @@ public class Red_V2 extends LinearOpMode {
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
@@ -454,13 +454,13 @@ public class Red_V2 extends LinearOpMode {
robot.hood.setPosition(hoodAuto);
robot.turr1.setPosition(turret_detectRed);
robot.turr2.setPosition(1 - turret_detectRed);
robot.turr1.setPower(turret_detectRed);
robot.turr2.setPower(1 - turret_detectRed);
robot.transferServo.setPosition(transferServo_out);
robot.spin1.setPosition(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1);
robot.spin1.setPower(spindexer_intakePos1);
robot.spin2.setPower(1 - spindexer_intakePos1);
aprilTag.update();
TELE.addData("Velocity", velo);

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
View File

@@ -1,6 +1,6 @@
package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
@@ -126,8 +126,8 @@ public class TeleopV2 extends LinearOpMode {
aprilTagWebcam.init(new Robot(hardwareMap), TELE);
robot.turr1.setPosition(0.4);
robot.turr2.setPosition(1 - 0.4);
robot.turr1.setPower(0.4);
robot.turr2.setPower(1 - 0.4);
waitForStart();
if (isStopRequested()) return;
@@ -181,14 +181,14 @@ public class TeleopV2 extends LinearOpMode {
position = spindexer_intakePos1 - 0.015;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
} else if (reject) {
robot.intake.setPower(-1);
double position = spindexer_intakePos1;
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
} else {
robot.intake.setPower(0);
}
@@ -363,8 +363,8 @@ public class TeleopV2 extends LinearOpMode {
if (!overrideTurr) {
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1 - pos);
robot.turr1.setPower(pos);
robot.turr2.setPower(1 - pos);
}
if (gamepad2.dpad_right) {
@@ -445,12 +445,12 @@ public class TeleopV2 extends LinearOpMode {
reject = true;
if (getRuntime() % 3 > 1.5) {
robot.spin1.setPosition(0);
robot.spin2.setPosition(1);
robot.spin1.setPower(0);
robot.spin2.setPower(1);
} else {
robot.spin1.setPosition(1);
robot.spin2.setPosition(0);
robot.spin1.setPower(1);
robot.spin2.setPower(0);
}
robot.transferServo.setPosition(transferServo_out);
@@ -475,9 +475,9 @@ public class TeleopV2 extends LinearOpMode {
overrideTurr = true;
double bearing = d20.ftcPose.bearing;
double finalPos = robot.turr1.getPosition() - (bearing / 1300);
robot.turr1.setPosition(finalPos);
robot.turr2.setPosition(1 - finalPos);
double finalPos = robot.turr1.getPower() - (bearing / 1300);
robot.turr1.setPower(finalPos);
robot.turr2.setPower(1 - finalPos);
TELE.addData("Bear", bearing);
@@ -487,9 +487,9 @@ public class TeleopV2 extends LinearOpMode {
overrideTurr = true;
double bearing = d24.ftcPose.bearing;
double finalPos = robot.turr1.getPosition() - (bearing / 1300);
robot.turr1.setPosition(finalPos);
robot.turr2.setPosition(1 - finalPos);
double finalPos = robot.turr1.getPower() - (bearing / 1300);
robot.turr1.setPower(finalPos);
robot.turr2.setPower(1 - finalPos);
}
@@ -543,8 +543,8 @@ public class TeleopV2 extends LinearOpMode {
} else {
// Finished shooting all balls
robot.spin1.setPosition(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1);
robot.spin1.setPower(spindexer_intakePos1);
robot.spin2.setPower(1 - spindexer_intakePos1);
shootA = true;
shootB = true;
shootC = true;
@@ -764,8 +764,8 @@ public class TeleopV2 extends LinearOpMode {
public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) {
// Set spin positions
robot.spin1.setPosition(spindexer);
robot.spin2.setPosition(1 - spindexer);
robot.spin1.setPower(spindexer);
robot.spin2.setPower(1 - spindexer);
// Check if spindexer has reached the target position
if (spinOk || getRuntime() - stamp > 1.5) {

167
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java Normal file
View File

@@ -0,0 +1,167 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
@Config
@TeleOp
public class IntakeTest extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
Servos servo;
public static int mode = 0; // 0 for teleop, 1 for auto
public static double manualPow = 0.5;
double stamp = 0;
int ticker = 0;
boolean b1 = false;
boolean b2 = false;
boolean b3 = false;
boolean steadySpin = false;
double powPID = 0.0;
double spindexerPos = spindexer_intakePos1;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
servo = new Servos(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
if (mode == 0) {
if (gamepad1.cross) {
ticker = 0;
robot.spin1.setPower(manualPow);
robot.spin2.setPower(-manualPow);
robot.intake.setPower(1);
} else {
robot.spin1.setPower(0);
robot.spin2.setPower(0);
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (getRuntime() - stamp < 0.5) {
robot.intake.setPower(-1);
} else {
robot.intake.setPower(0);
}
}
} else if (mode == 1) {
if (gamepad1.cross){
robot.intake.setPower(1);
} else if (gamepad1.circle){
robot.intake.setPower(-1);
} else {
robot.intake.setPower(0);
}
colorDetect();
spindexer();
if (b1 && steadySpin && getRuntime() - stamp > 0.5){
if (!b2){
if (servo.spinEqual(spindexer_intakePos1)){
spindexerPos = spindexer_intakePos2;
} else if (servo.spinEqual(spindexer_intakePos2)){
spindexerPos = spindexer_intakePos3;
} else if (servo.spinEqual(spindexer_intakePos3)){
spindexerPos = spindexer_intakePos1;
}
} else if (!b3){
if (servo.spinEqual(spindexer_intakePos1)){
spindexerPos = spindexer_intakePos3;
} else if (servo.spinEqual(spindexer_intakePos2)){
spindexerPos = spindexer_intakePos1;
} else if (servo.spinEqual(spindexer_intakePos3)){
spindexerPos = spindexer_intakePos2;
}
} else {
spindexerPos = spindexer_outtakeBall1;
}
}
powPID = servo.setSpinPos(spindexerPos);
} else if (mode == 2){ // switch to this mode before switching modes
powPID = 0;
spindexerPos = spindexer_intakePos1;
stamp = getRuntime();
ticker = 0;
}
TELE.addData("Manual Power", manualPow);
TELE.addData("PID Power", powPID);
TELE.addData("B1", b1);
TELE.addData("B2", b2);
TELE.addData("B3", b3);
TELE.addData("Spindex Pos", servo.getSpinPos());
}
}
public void colorDetect() {
// ----- COLOR 1 -----
double green1 = robot.color1.getNormalizedColors().green;
double blue1 = robot.color1.getNormalizedColors().blue;
double red1 = robot.color1.getNormalizedColors().red;
b1 = robot.color1.getDistance(DistanceUnit.MM) < 40;
TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM));
// ----- COLOR 2 -----
double green2 = robot.color2.getNormalizedColors().green;
double blue2 = robot.color2.getNormalizedColors().blue;
double red2 = robot.color2.getNormalizedColors().red;
b2 = robot.color2.getDistance(DistanceUnit.MM) < 40;
TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor());
TELE.addData("Color2 green", green2 / (green2 + blue2 + red2));
TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM));
// ----- COLOR 3 -----
double green3 = robot.color3.getNormalizedColors().green;
double blue3 = robot.color3.getNormalizedColors().blue;
double red3 = robot.color3.getNormalizedColors().red;
b3 = robot.color3.getDistance(DistanceUnit.MM) < 30;
TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor());
TELE.addData("Color3 green", green3 / (green3 + blue3 + red3));
TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM));
TELE.update();
}
public void spindexer(){
if (!servo.spinEqual(spindexerPos)){
robot.spin1.setPower(powPID);
robot.spin2.setPower(-powPID);
steadySpin = false;
ticker = 0;
} else{
robot.spin1.setPower(0);
robot.spin2.setPower(0);
steadySpin = true;
if (ticker == 0){
stamp = getRuntime();
}
ticker++;
}
}
}

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

@@ -1,11 +1,9 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDController;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CRServo;
@@ -17,14 +15,14 @@ import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
public class PIDServoTest extends LinearOpMode {
public static double p = 0.0003, i = 0, d = 0.00001;
public static double p = 2, i = 0, d = 0, f = 0;
public static double target = 0.5;
public static int mode = 0; //0 is for turret, 1 is for spindexer
public static double scalar = 1.112;
public static double restPos = 0.15;
public static double scalar = 1.01;
public static double restPos = 0.0;
Robot robot;
@@ -33,7 +31,7 @@ public class PIDServoTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
PIDController controller = new PIDController(p, i, d);
PIDFController controller = new PIDFController(p, i, d, f);
controller.setTolerance(0);
robot = new Robot(hardwareMap);
@@ -45,22 +43,22 @@ public class PIDServoTest extends LinearOpMode {
if (isStopRequested()) return;
while (opModeIsActive()) {
controller.setPID(p, i, d);
controller.setPIDF(p, i, d, f);
if (mode == 0) {
pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3);
double pid = controller.calculate(pos, target);
robot.turr1.setPosition(pid);
robot.turr2.setPosition(-pid);
robot.turr1.setPower(pid);
robot.turr2.setPower(-pid);
} else if (mode == 1) {
pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3);
double pid = controller.calculate(pos, target);
robot.spin1.setPosition(pid);
robot.spin2.setPosition(-pid);
robot.spin1.setPower(pid);
robot.spin2.setPower(-pid);
}
telemetry.addData("pos", pos);

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
View File

@@ -34,6 +34,7 @@ public class ShooterTest extends LinearOpMode {
robot = new Robot(hardwareMap);
DcMotorEx leftShooter = robot.shooter1;
DcMotorEx rightShooter = robot.shooter2;
flywheel = new Flywheel();
MultipleTelemetry TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -50,11 +51,9 @@ public class ShooterTest extends LinearOpMode {
leftShooter.setPower(parameter);
} else if (mode == 1) {
double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
TELE.addData("Velocity", flywheel.getVelo());
rightShooter.setPower(powPID);
leftShooter.setPower(powPID);
TELE.addData("PIDPower", powPID);
TELE.addData("Power", robot.shooter1.getPower());
TELE.addData("Steady?", flywheel.getSteady());
}
if (hoodPos != 0.501) {
@@ -62,8 +61,8 @@ public class ShooterTest extends LinearOpMode {
}
if (turretPos != 0.501) {
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(turretPos);
robot.turr1.setPower(turretPos);
robot.turr2.setPower(turretPos);
}
robot.transfer.setPower(transferPower);
@@ -72,6 +71,10 @@ public class ShooterTest extends LinearOpMode {
} else {
robot.transferServo.setPosition(transferServo_out);
}
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Power", robot.shooter1.getPower());
TELE.addData("Steady?", flywheel.getSteady());
TELE.addData("Position", robot.shooter1.getCurrentPosition());
TELE.update();

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
View File

@@ -51,7 +51,7 @@ public class Flywheel {
velo3 = velo2;
velo2 = velo1;
currentPos = shooter1CurPos / 2048;
currentPos = shooter1CurPos / 3072;
stamp = getTimeSeconds(); //getRuntime();
velo1 = -60 * ((currentPos - initPos) / (stamp - stamp1));
initPos = currentPos;

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java
View File

@@ -13,6 +13,8 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
public class PositionalServoProgrammer extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
Servos servo;
public static double spindexPos = 0.501;
public static double turretPos = 0.501;
public static double transferPos = 0.501;
@@ -22,16 +24,25 @@ public class PositionalServoProgrammer extends LinearOpMode {
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
servo = new Servos(hardwareMap);
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()){
if (spindexPos != 0.501){
robot.spin1.setPosition(spindexPos);
robot.spin2.setPosition(1-spindexPos);
if (spindexPos != 0.501 && !servo.spinEqual(spindexPos)){
double pos = servo.setSpinPos(spindexPos);
robot.spin1.setPower(pos);
robot.spin2.setPower(-pos);
} else{
robot.spin1.setPower(0);
robot.spin2.setPower(0);
}
if (turretPos != 0.501){
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1-turretPos);
if (turretPos != 0.501 && !servo.turretEqual(turretPos)){
double pos = servo.setTurrPos(turretPos);
robot.turr1.setPower(pos);
robot.turr2.setPower(-pos);
} else {
robot.turr1.setPower(0);
robot.turr2.setPower(0);
}
if (transferPos != 0.501){
robot.transferServo.setPosition(transferPos);
@@ -39,14 +50,15 @@ public class PositionalServoProgrammer extends LinearOpMode {
if (hoodPos != 0.501){
robot.hood.setPosition(hoodPos);
}
TELE.addData("spindexer", scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3));
TELE.addData("spindexer", servo.getSpinPos());
TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3));
TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3));
TELE.addData("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3));
TELE.addData("turret", servo.getTurrPos());
TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage());
TELE.addData("hood voltage", robot.hoodPos.getVoltage());
TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage());
TELE.addData("turret voltage", robot.turr1Pos.getVoltage());
TELE.addData("Spin Equal", servo.spinEqual(spindexPos));
TELE.update();
}
}

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
View File

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.utils;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -35,13 +36,13 @@ public class Robot {
public Servo rejecter;
public Servo turr1;
public CRServo turr1;
public Servo turr2;
public CRServo turr2;
public Servo spin1;
public CRServo spin1;
public Servo spin2;
public CRServo spin2;
public DigitalChannel pin0;
@@ -104,6 +105,8 @@ public class Robot {
shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
shooterEncoder = shooter1;
@@ -111,19 +114,19 @@ public class Robot {
hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
turr1 = hardwareMap.get(Servo.class, "t1");
turr1 = hardwareMap.get(CRServo.class, "t1");
turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
turr2 = hardwareMap.get(Servo.class, "t2");
turr2 = hardwareMap.get(CRServo.class, "t2");
turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
spin1 = hardwareMap.get(Servo.class, "spin1");
spin1 = hardwareMap.get(CRServo.class, "spin1");
spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
spin2 = hardwareMap.get(Servo.class, "spin2");
spin2 = hardwareMap.get(CRServo.class, "spin2");
spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");

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

@@ -1,52 +1,61 @@
package org.firstinspires.ftc.teamcode.utils;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDController;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
public class Servos {
PIDController spinPID;
Robot robot;
PIDController turretPID;
PIDFController spinPID;
PIDFController turretPID;
//PID constants
public static double spinP = 0.0, spinI = 0.0, spinD = 0.0;
public static double turrP = 0.0, turrI = 0.0, turrD = 0.0;
public static double spinP = 2.85, spinI = 0.015, spinD = 0.09, spinF = 0.03;
public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF;
public static double spin_scalar = 0.15;
public static double spin_restPos = 1.112;
public static double turret_scalar = 0.15;
public static double turret_restPos = 1.112;
public static double spin_scalar = 1.011;
public static double spin_restPos = 0.0;
public static double turret_scalar = 1.009;
public static double turret_restPos = 0.0;
public void initServos() {
spinPID = new PIDController(spinP, spinI, spinD);
turretPID = new PIDController(turrP, turrI, turrD);
public Servos(HardwareMap hardwareMap) {
robot = new Robot(hardwareMap);
spinPID = new PIDFController(spinP, spinI, spinD, spinF);
turretPID = new PIDFController(turrP, turrI, turrD, turrF);
}
// In the code below, encoder = robot.servo.getVoltage()
public double getSpinPos(double encoder){
return spin_scalar * ((encoder - spin_restPos) / 3.3);
}
public double setSpinPos(double pos, double encoder){
spinPID.setPID(spinP, spinI, spinD);
return spinPID.calculate(this.getSpinPos(encoder), pos);
}
public boolean spinEqual(double pos, double encoder){
return Math.abs(pos - this.getSpinPos(encoder)) < 0.01;
public double getSpinPos() {
return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3);
}
public double getTurrPos(double encoder){
return turret_scalar * ((encoder - turret_restPos) / 3.3);
}
public double setTurrPos(double pos, double encoder){
turretPID.setPID(turrP, turrI, turrD);
public double setSpinPos(double pos) {
spinPID.setPIDF(spinP, spinI, spinD, spinF);
return spinPID.calculate(this.getTurrPos(encoder), pos);
return spinPID.calculate(this.getSpinPos(), pos);
}
public boolean turretEqual(double pos, double encoder){
return Math.abs(pos - this.getTurrPos(encoder)) < 0.01;
public boolean spinEqual(double pos) {
return Math.abs(pos - this.getSpinPos()) < 0.01;
}
public double getTurrPos() {
return turret_scalar * ((robot.turr1Pos.getVoltage() - turret_restPos) / 3.3);
}
public double setTurrPos(double pos) {
turretPID.setPIDF(turrP, turrI, turrD, turrF);
return spinPID.calculate(this.getTurrPos(), pos);
}
public boolean turretEqual(double pos) {
return Math.abs(pos - this.getTurrPos()) < 0.01;
}
}