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

prototype auto

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-16 21:08:59 -06:00
parent ba251f8a46
commit 6dee088300
11 changed files with 122 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java
View File

@@ -103,17 +103,17 @@ public class AutoClose_V3 extends LinearOpMode {
if (gpp || pgp || ppg) { if (gpp || pgp || ppg) {
if (redAlliance){ if (redAlliance){
robot.limelight.pipelineSwitch(3); robot.limelight.pipelineSwitch(3);
double turretPID = servo.setTurrPos(turret_redClose); double turretPID = servo.setTurrPos(turret_redClose, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
return !servo.turretEqual(turret_redClose); return !servo.turretEqual(turret_redClose, robot.turr1Pos.getCurrentPosition());
} else { } else {
robot.limelight.pipelineSwitch(2); robot.limelight.pipelineSwitch(2);
double turretPID = servo.setTurrPos(turret_blueClose); double turretPID = servo.setTurrPos(turret_blueClose, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
return !servo.turretEqual(turret_blueClose); return !servo.turretEqual(turret_blueClose, robot.turr1Pos.getCurrentPosition());
} }
} else { } else {
return true; return true;
@@ -132,7 +132,7 @@ public class AutoClose_V3 extends LinearOpMode {
robot.shooter1.setPower(powPID); robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID); robot.shooter2.setPower(powPID);
spinPID = servo.setSpinPos(spindexer); spinPID = servo.setSpinPos(spindexer, robot.spin1Pos.getVoltage());
robot.spin1.setPower(spinPID); robot.spin1.setPower(spinPID);
robot.spin2.setPower(-spinPID); robot.spin2.setPower(-spinPID);
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);
@@ -142,7 +142,7 @@ public class AutoClose_V3 extends LinearOpMode {
drive.updatePoseEstimate(); drive.updatePoseEstimate();
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
if (servo.spinEqual(spindexer)){ if (servo.spinEqual(spindexer, robot.spin1Pos.getVoltage())){
robot.spin1.setPower(0); robot.spin1.setPower(0);
robot.spin2.setPower(0); robot.spin2.setPower(0);
return false; return false;
@@ -222,27 +222,27 @@ public class AutoClose_V3 extends LinearOpMode {
double s2D = robot.color2.getDistance(DistanceUnit.MM); double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM); double s3D = robot.color3.getDistance(DistanceUnit.MM);
if (!servo.spinEqual(position)){ if (!servo.spinEqual(position, robot.spin1Pos.getVoltage())){
double spinPID = servo.setSpinPos(position); double spinPID = servo.setSpinPos(position, robot.spin1Pos.getVoltage());
robot.spin1.setPower(spinPID); robot.spin1.setPower(spinPID);
robot.spin2.setPower(-spinPID); robot.spin2.setPower(-spinPID);
} }
if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){ if (s1D < 43 && servo.spinEqual(position, robot.spin1Pos.getVoltage()) && getRuntime() - stamp > 0.5){
if (s2D > 60){ if (s2D > 60){
if (servo.spinEqual(spindexer_intakePos1)){ if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos2; position = spindexer_intakePos2;
} else if (servo.spinEqual(spindexer_intakePos2)){ } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos3; position = spindexer_intakePos3;
} else if (servo.spinEqual(spindexer_intakePos3)){ } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos1; position = spindexer_intakePos1;
} }
} else if (s3D > 33){ } else if (s3D > 33){
if (servo.spinEqual(spindexer_intakePos1)){ if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos3; position = spindexer_intakePos3;
} else if (servo.spinEqual(spindexer_intakePos2)){ } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos1; position = spindexer_intakePos1;
} else if (servo.spinEqual(spindexer_intakePos3)){ } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos2; position = spindexer_intakePos2;
} }
} }
@@ -411,7 +411,7 @@ public class AutoClose_V3 extends LinearOpMode {
double turrPID; double turrPID;
if (redAlliance){ if (redAlliance){
turrPID = servo.setTurrPos(turret_detectRedClose); turrPID = servo.setTurrPos(turret_detectRedClose, robot.turr1Pos.getCurrentPosition());
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
@@ -430,7 +430,7 @@ public class AutoClose_V3 extends LinearOpMode {
shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
} else { } else {
turrPID = servo.setTurrPos(turret_detectBlueClose); turrPID = servo.setTurrPos(turret_detectBlueClose, robot.turr1Pos.getCurrentPosition());
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1); .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
@@ -458,8 +458,8 @@ public class AutoClose_V3 extends LinearOpMode {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);
TELE.addData("Turret Pos", servo.getTurrPos()); TELE.addData("Turret Pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition()));
TELE.addData("Spin Pos", servo.getSpinPos()); TELE.addData("Spin Pos", servo.getSpinPos(robot.spin1Pos.getVoltage()));
TELE.update(); TELE.update();
} }
@@ -565,8 +565,8 @@ public class AutoClose_V3 extends LinearOpMode {
bearing = result.getTx(); bearing = result.getTx();
} }
} }
double turretPos = servo.getTurrPos() - (bearing / 1300); double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
double turretPID = servo.setTurrPos(turretPos); double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
} }

42
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java
View File

@@ -103,17 +103,17 @@ public class AutoFar_V1 extends LinearOpMode {
if (gpp || pgp || ppg) { if (gpp || pgp || ppg) {
if (redAlliance){ if (redAlliance){
robot.limelight.pipelineSwitch(3); robot.limelight.pipelineSwitch(3);
double turretPID = servo.setTurrPos(turret_redFar); double turretPID = servo.setTurrPos(turret_redFar, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
return !servo.turretEqual(turret_redFar); return !servo.turretEqual(turret_redFar, robot.turr1Pos.getCurrentPosition());
} else { } else {
robot.limelight.pipelineSwitch(2); robot.limelight.pipelineSwitch(2);
double turretPID = servo.setTurrPos(turret_blueFar); double turretPID = servo.setTurrPos(turret_blueFar, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
return !servo.turretEqual(turret_blueFar); return !servo.turretEqual(turret_blueFar, robot.turr1Pos.getCurrentPosition());
} }
} else { } else {
return true; return true;
@@ -132,7 +132,7 @@ public class AutoFar_V1 extends LinearOpMode {
robot.shooter1.setPower(powPID); robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID); robot.shooter2.setPower(powPID);
spinPID = servo.setSpinPos(spindexer); spinPID = servo.setSpinPos(spindexer, robot.spin1Pos.getVoltage());
robot.spin1.setPower(spinPID); robot.spin1.setPower(spinPID);
robot.spin2.setPower(-spinPID); robot.spin2.setPower(-spinPID);
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);
@@ -142,7 +142,7 @@ public class AutoFar_V1 extends LinearOpMode {
drive.updatePoseEstimate(); drive.updatePoseEstimate();
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
if (servo.spinEqual(spindexer)){ if (servo.spinEqual(spindexer, robot.spin1Pos.getVoltage())){
robot.spin1.setPower(0); robot.spin1.setPower(0);
robot.spin2.setPower(0); robot.spin2.setPower(0);
return false; return false;
@@ -222,27 +222,27 @@ public class AutoFar_V1 extends LinearOpMode {
double s2D = robot.color2.getDistance(DistanceUnit.MM); double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM); double s3D = robot.color3.getDistance(DistanceUnit.MM);
if (!servo.spinEqual(position)){ if (!servo.spinEqual(position, robot.spin1Pos.getVoltage())){
double spinPID = servo.setSpinPos(position); double spinPID = servo.setSpinPos(position, robot.spin1Pos.getVoltage());
robot.spin1.setPower(spinPID); robot.spin1.setPower(spinPID);
robot.spin2.setPower(-spinPID); robot.spin2.setPower(-spinPID);
} }
if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){ if (s1D < 43 && servo.spinEqual(position, robot.spin1Pos.getVoltage()) && getRuntime() - stamp > 0.5){
if (s2D > 60){ if (s2D > 60){
if (servo.spinEqual(spindexer_intakePos1)){ if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos2; position = spindexer_intakePos2;
} else if (servo.spinEqual(spindexer_intakePos2)){ } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos3; position = spindexer_intakePos3;
} else if (servo.spinEqual(spindexer_intakePos3)){ } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos1; position = spindexer_intakePos1;
} }
} else if (s3D > 33){ } else if (s3D > 33){
if (servo.spinEqual(spindexer_intakePos1)){ if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos3; position = spindexer_intakePos3;
} else if (servo.spinEqual(spindexer_intakePos2)){ } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos1; position = spindexer_intakePos1;
} else if (servo.spinEqual(spindexer_intakePos3)){ } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos2; position = spindexer_intakePos2;
} }
} }
@@ -399,9 +399,9 @@ public class AutoFar_V1 extends LinearOpMode {
double turrPID; double turrPID;
if (redAlliance){ if (redAlliance){
turrPID = servo.setTurrPos(turret_detectRedClose); turrPID = servo.setTurrPos(turret_detectRedClose, robot.turr1Pos.getCurrentPosition());
} else { } else {
turrPID = servo.setTurrPos(turret_detectBlueClose); turrPID = servo.setTurrPos(turret_detectBlueClose, robot.turr1Pos.getCurrentPosition());
} }
robot.turr1.setPower(turrPID); robot.turr1.setPower(turrPID);
@@ -412,8 +412,8 @@ public class AutoFar_V1 extends LinearOpMode {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);
TELE.addData("Turret Pos", servo.getTurrPos()); TELE.addData("Turret Pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition()));
TELE.addData("Spin Pos", servo.getSpinPos()); TELE.addData("Spin Pos", servo.getSpinPos(robot.spin1Pos.getVoltage()));
TELE.update(); TELE.update();
} }
@@ -466,8 +466,8 @@ public class AutoFar_V1 extends LinearOpMode {
bearing = result.getTx(); bearing = result.getTx();
} }
} }
double turretPos = servo.getTurrPos() - (bearing / 1300); double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
double turretPID = servo.setTurrPos(turretPos); double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
} }

57
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java
View File

@@ -103,17 +103,17 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
if (gpp || pgp || ppg) { if (gpp || pgp || ppg) {
if (redAlliance){ if (redAlliance){
robot.limelight.pipelineSwitch(3); robot.limelight.pipelineSwitch(3);
double turretPID = servo.setTurrPos(turret_redClose); double turretPID = servo.setTurrPos(turret_redClose, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
return !servo.turretEqual(turret_redClose); return !servo.turretEqual(turret_redClose, robot.turr1Pos.getCurrentPosition());
} else { } else {
robot.limelight.pipelineSwitch(2); robot.limelight.pipelineSwitch(2);
double turretPID = servo.setTurrPos(turret_blueClose); double turretPID = servo.setTurrPos(turret_blueClose, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
return !servo.turretEqual(turret_blueClose); return !servo.turretEqual(turret_blueClose, robot.turr1Pos.getCurrentPosition());
} }
} else { } else {
return true; return true;
@@ -132,7 +132,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
robot.shooter1.setPower(powPID); robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID); robot.shooter2.setPower(powPID);
spinPID = servo.setSpinPos(spindexer); spinPID = servo.setSpinPos(spindexer, robot.spin1Pos.getVoltage());
robot.spin1.setPower(spinPID); robot.spin1.setPower(spinPID);
robot.spin2.setPower(-spinPID); robot.spin2.setPower(-spinPID);
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);
@@ -142,7 +142,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
drive.updatePoseEstimate(); drive.updatePoseEstimate();
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
if (servo.spinEqual(spindexer)){ if (servo.spinEqual(spindexer, robot.spin1Pos.getVoltage())){
robot.spin1.setPower(0); robot.spin1.setPower(0);
robot.spin2.setPower(0); robot.spin2.setPower(0);
return false; return false;
@@ -178,13 +178,12 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
robot.turr2.setPower(holdTurrPow); robot.turr2.setPower(holdTurrPow);
drive.updatePoseEstimate(); drive.updatePoseEstimate();
detectTag();
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
if (ticker == 1){ if (ticker == 1){
robot.transferServo.setPosition(transferServo_in); robot.transferServo.setPosition(transferServo_in);
initPos = servo.getSpinPos(); initPos = servo.getSpinPos(robot.spin1Pos.getVoltage());
finalPos = initPos + 0.6; finalPos = initPos + 0.6;
@@ -205,7 +204,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
} }
prevPos = currentPos; prevPos = currentPos;
currentPos = servo.getSpinPos(); currentPos = servo.getSpinPos(robot.spin1Pos.getVoltage());
if (zeroNeeded){ if (zeroNeeded){
if (currentPos - prevPos < -0.5){ if (currentPos - prevPos < -0.5){
zeroPassed = true; zeroPassed = true;
@@ -257,27 +256,27 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
double s2D = robot.color2.getDistance(DistanceUnit.MM); double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM); double s3D = robot.color3.getDistance(DistanceUnit.MM);
if (!servo.spinEqual(position)){ if (!servo.spinEqual(position, robot.spin1Pos.getVoltage())){
double spinPID = servo.setSpinPos(position); double spinPID = servo.setSpinPos(position, robot.spin1Pos.getVoltage());
robot.spin1.setPower(spinPID); robot.spin1.setPower(spinPID);
robot.spin2.setPower(-spinPID); robot.spin2.setPower(-spinPID);
} }
if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){ if (s1D < 43 && servo.spinEqual(position, robot.spin1Pos.getVoltage()) && getRuntime() - stamp > 0.5){
if (s2D > 60){ if (s2D > 60){
if (servo.spinEqual(spindexer_intakePos1)){ if (servo.spinEqual(spindexer_intakePos1,robot.spin1Pos.getVoltage())){
position = spindexer_intakePos2; position = spindexer_intakePos2;
} else if (servo.spinEqual(spindexer_intakePos2)){ } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos3; position = spindexer_intakePos3;
} else if (servo.spinEqual(spindexer_intakePos3)){ } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos1; position = spindexer_intakePos1;
} }
} else if (s3D > 33){ } else if (s3D > 33){
if (servo.spinEqual(spindexer_intakePos1)){ if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos3; position = spindexer_intakePos3;
} else if (servo.spinEqual(spindexer_intakePos2)){ } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos1; position = spindexer_intakePos1;
} else if (servo.spinEqual(spindexer_intakePos3)){ } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
position = spindexer_intakePos2; position = spindexer_intakePos2;
} }
} }
@@ -409,6 +408,8 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
0, 0, 0 0, 0, 0
)); ));
servo = new Servos();
robot.limelight.pipelineSwitch(1); robot.limelight.pipelineSwitch(1);
robot.limelight.start(); robot.limelight.start();
@@ -443,11 +444,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
redAlliance = !redAlliance; redAlliance = !redAlliance;
} }
double turrPID;
if (redAlliance){ if (redAlliance){
turrPID = servo.setTurrPos(turret_detectRedClose);
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
@@ -465,8 +462,6 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
} else { } else {
turrPID = servo.setTurrPos(turret_detectBlueClose);
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1); .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
@@ -485,16 +480,11 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1); .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
} }
robot.turr1.setPower(turrPID);
robot.turr2.setPower(-turrPID);
robot.hood.setPosition(hoodAuto); robot.hood.setPosition(hoodAuto);
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
TELE.addData("Velocity", velo); TELE.addData("Red?", redAlliance);
TELE.addData("Turret Pos", servo.getTurrPos());
TELE.addData("Spin Pos", servo.getSpinPos());
TELE.update(); TELE.update();
} }
@@ -507,8 +497,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
shoot0.build(), shoot0.build(),
initShooter(AUTO_CLOSE_VEL), initShooter(AUTO_CLOSE_VEL)
Obelisk()
) )
); );
drive.updatePoseEstimate(); drive.updatePoseEstimate();
@@ -598,8 +587,8 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
bearing = result.getTx(); bearing = result.getTx();
} }
} }
double turretPos = servo.getTurrPos() - (bearing / 1300); double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
double turretPID = servo.setTurrPos(turretPos); double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
} }

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

@@ -169,10 +169,10 @@ public class Red_V2 extends LinearOpMode {
TELE.update(); TELE.update();
if (gpp || pgp || ppg){ if (gpp || pgp || ppg){
double turretPID = servo.setTurrPos(turret_redClose); double turretPID = servo.setTurrPos(turret_redClose, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
return !servo.turretEqual(turret_redClose); return !servo.turretEqual(turret_redClose, robot.turr1Pos.getCurrentPosition());
} else { } else {
return true; return true;
} }
@@ -199,7 +199,7 @@ public class Red_V2 extends LinearOpMode {
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
return !servo.spinEqual(spindexer); return !servo.spinEqual(spindexer, robot.spin1Pos.getVoltage());
} }
}; };
} }
@@ -461,7 +461,7 @@ public class Red_V2 extends LinearOpMode {
aprilTag.update(); aprilTag.update();
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);
TELE.addData("Turret Pos", servo.getTurrPos()); TELE.addData("Turret Pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition()));
TELE.update(); TELE.update();
} }
@@ -585,8 +585,8 @@ public class Red_V2 extends LinearOpMode {
bearing = d24.ftcPose.bearing; bearing = d24.ftcPose.bearing;
TELE.addData("Bear", bearing); TELE.addData("Bear", bearing);
} }
double turretPos = servo.getTurrPos() - (bearing / 1300); double turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
double turretPID = servo.setTurrPos(turretPos); double turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
} }

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
View File

@@ -14,13 +14,13 @@ public class Poses {
public static Pose2d goalPose = new Pose2d(-15, 0, 0); public static Pose2d goalPose = new Pose2d(-15, 0, 0);
public static double rx1 = 45, ry1 = -7, rh1 = 0; public static double rx1 = 40, ry1 = -7, rh1 = 0;
public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140); public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140);
public static double rx2b = 31, ry2b = 32, rh2b = Math.toRadians(140); public static double rx2b = 31, ry2b = 32, rh2b = Math.toRadians(140);
public static double rx3a = 58, ry3a = 42, rh3a = Math.toRadians(140); public static double rx3a = 58, ry3a = 42, rh3a = Math.toRadians(140);
public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140); public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140);
public static double bx1 = 45, by1 = 6, bh1 = 0; public static double bx1 = 40, by1 = 6, bh1 = 0;
public static double bx2a = 53, by2a = -7, bh2a = Math.toRadians(-140); public static double bx2a = 53, by2a = -7, bh2a = Math.toRadians(-140);
public static double bx2b = 23, by2b = -39, bh2b = Math.toRadians(-140); public static double bx2b = 23, by2b = -39, bh2b = Math.toRadians(-140);
public static double bx3a = 56, by3a = -34, bh3a = Math.toRadians(-140); public static double bx3a = 56, by3a = -34, bh3a = Math.toRadians(-140);

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

@@ -24,7 +24,7 @@ public class ServoPositions {
public static double hoodDefault = 0.6; public static double hoodDefault = 0.6;
public static double hoodAuto = 0.55; public static double hoodAuto = 0.22;
public static double hoodAutoFar = 0.5; //TODO: change this; public static double hoodAutoFar = 0.5; //TODO: change this;

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

@@ -128,7 +128,7 @@ public class TeleopV2 extends LinearOpMode {
TELE = new MultipleTelemetry( TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry() telemetry, FtcDashboard.getInstance().getTelemetry()
); );
servo = new Servos(hardwareMap); servo = new Servos();
flywheel = new Flywheel(); flywheel = new Flywheel();
drive = new MecanumDrive(hardwareMap, teleStart); drive = new MecanumDrive(hardwareMap, teleStart);
@@ -159,13 +159,13 @@ public class TeleopV2 extends LinearOpMode {
robot.backRight.setPower(backRightPower); robot.backRight.setPower(backRightPower);
// PID SERVOS // PID SERVOS
turretPID = servo.setTurrPos(turretPos); turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
//TODO: make sure changing position works throughout opmode //TODO: make sure changing position works throughout opmode
if (!servo.spinEqual(spindexPos)){ if (!servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage())){
spindexPID = servo.setSpinPos(spindexPos); spindexPID = servo.setSpinPos(spindexPos, robot.spin1Pos.getVoltage());
robot.spin1.setPower(spindexPID); robot.spin1.setPower(spindexPID);
robot.spin2.setPower(-spindexPID); robot.spin2.setPower(-spindexPID);
} else{ } else{
@@ -348,7 +348,7 @@ public class TeleopV2 extends LinearOpMode {
bearing = d24.ftcPose.bearing; bearing = d24.ftcPose.bearing;
} }
overrideTurr = true; overrideTurr = true;
turretPos = servo.getTurrPos() - (bearing/1300); turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing/1300);
TELE.addData("Bear", bearing); TELE.addData("Bear", bearing);
double bearingCorrection = bearing / 1300; double bearingCorrection = bearing / 1300;
@@ -481,13 +481,13 @@ public class TeleopV2 extends LinearOpMode {
boolean shootingDone = false; boolean shootingDone = false;
if (!outtake1) { if (!outtake1) {
outtake1 = (servo.spinEqual(spindexer_outtakeBall1)); outtake1 = (servo.spinEqual(spindexer_outtakeBall1, robot.spin1Pos.getVoltage()));
} }
if (!outtake2) { if (!outtake2) {
outtake2 = (servo.spinEqual(spindexer_outtakeBall2)); outtake2 = (servo.spinEqual(spindexer_outtakeBall2, robot.spin1Pos.getVoltage()));
} }
if (!outtake3) { if (!outtake3) {
outtake3 = (servo.spinEqual(spindexer_outtakeBall3)); outtake3 = (servo.spinEqual(spindexer_outtakeBall3, robot.spin1Pos.getVoltage()));
} }
switch (currentSlot) { switch (currentSlot) {

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

@@ -98,7 +98,7 @@ public class TeleopV3 extends LinearOpMode {
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
servo = new Servos(hardwareMap); servo = new Servos();
flywheel = new FlywheelV2(); flywheel = new FlywheelV2();
drive = new MecanumDrive(hardwareMap, teleStart); drive = new MecanumDrive(hardwareMap, teleStart);
@@ -131,12 +131,12 @@ public class TeleopV3 extends LinearOpMode {
robot.backRight.setPower(backRightPower); robot.backRight.setPower(backRightPower);
// PID SERVOS // PID SERVOS
turretPID = servo.setTurrPos(turretPos); turretPID = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(turretPID); robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID); robot.turr2.setPower(-turretPID);
if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) { if (!servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage()) && !gamepad1.right_bumper) {
spindexPID = servo.setSpinPos(spindexPos); spindexPID = servo.setSpinPos(spindexPos, robot.spin1Pos.getVoltage());
robot.spin1.setPower(spindexPID); robot.spin1.setPower(spindexPID);
robot.spin2.setPower(-spindexPID); robot.spin2.setPower(-spindexPID);
} else { } else {
@@ -149,7 +149,7 @@ public class TeleopV3 extends LinearOpMode {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
intakeTicker++; intakeTicker++;
if (intakeTicker % 16 == 0) { if (intakeTicker % 16 == 0) {
spinCurrentPos = servo.getSpinPos(); spinCurrentPos = servo.getSpinPos(robot.spin1Pos.getVoltage());
if (Math.abs(spinCurrentPos - spinInitPos) == 0.0) { if (Math.abs(spinCurrentPos - spinInitPos) == 0.0) {
reverse = !reverse; reverse = !reverse;
} }
@@ -311,7 +311,7 @@ public class TeleopV3 extends LinearOpMode {
if (result.isValid()) { if (result.isValid()) {
bearing = result.getTx(); bearing = result.getTx();
overrideTurr = true; overrideTurr = true;
turretPos = servo.getTurrPos() - (bearing / 1300); turretPos = servo.getTurrPos(robot.turr1Pos.getCurrentPosition()) - (bearing / 1300);
double bearingCorrection = bearing / 1300; double bearingCorrection = bearing / 1300;
@@ -427,13 +427,13 @@ public class TeleopV3 extends LinearOpMode {
boolean shootingDone = false; boolean shootingDone = false;
if (!outtake1) { if (!outtake1) {
outtake1 = (servo.spinEqual(spindexer_outtakeBall1)); outtake1 = (servo.spinEqual(spindexer_outtakeBall1, robot.spin1Pos.getVoltage()));
} }
if (!outtake2) { if (!outtake2) {
outtake2 = (servo.spinEqual(spindexer_outtakeBall2)); outtake2 = (servo.spinEqual(spindexer_outtakeBall2, robot.spin1Pos.getVoltage()));
} }
if (!outtake3) { if (!outtake3) {
outtake3 = (servo.spinEqual(spindexer_outtakeBall3)); outtake3 = (servo.spinEqual(spindexer_outtakeBall3, robot.spin1Pos.getVoltage()));
} }
switch (currentSlot) { switch (currentSlot) {

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

@@ -53,7 +53,7 @@ public class IntakeTest extends LinearOpMode {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
} }
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
servo = new Servos(hardwareMap); servo = new Servos();
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
waitForStart(); waitForStart();
@@ -65,7 +65,7 @@ public class IntakeTest extends LinearOpMode {
if (gamepad1.right_bumper) { if (gamepad1.right_bumper) {
ticker++; ticker++;
if (ticker % 16 == 0){ if (ticker % 16 == 0){
currentPos = servo.getSpinPos(); currentPos = servo.getSpinPos(robot.spin1Pos.getVoltage());
if (Math.abs(currentPos - initPos) == 0.0){ if (Math.abs(currentPos - initPos) == 0.0){
reverse = !reverse; reverse = !reverse;
} }
@@ -109,19 +109,19 @@ public class IntakeTest extends LinearOpMode {
if (ballIn(1) && steadySpin && intake && getRuntime() - stamp > 0.5){ if (ballIn(1) && steadySpin && intake && getRuntime() - stamp > 0.5){
if (!ballIn(2)){ if (!ballIn(2)){
if (servo.spinEqual(spindexer_intakePos1)){ if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
spindexerPos = spindexer_intakePos2; spindexerPos = spindexer_intakePos2;
} else if (servo.spinEqual(spindexer_intakePos2)){ } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
spindexerPos = spindexer_intakePos3; spindexerPos = spindexer_intakePos3;
} else if (servo.spinEqual(spindexer_intakePos3)){ } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
spindexerPos = spindexer_intakePos1; spindexerPos = spindexer_intakePos1;
} }
} else if (!ballIn(3)){ } else if (!ballIn(3)){
if (servo.spinEqual(spindexer_intakePos1)){ if (servo.spinEqual(spindexer_intakePos1, robot.spin1Pos.getVoltage())){
spindexerPos = spindexer_intakePos3; spindexerPos = spindexer_intakePos3;
} else if (servo.spinEqual(spindexer_intakePos2)){ } else if (servo.spinEqual(spindexer_intakePos2, robot.spin1Pos.getVoltage())){
spindexerPos = spindexer_intakePos1; spindexerPos = spindexer_intakePos1;
} else if (servo.spinEqual(spindexer_intakePos3)){ } else if (servo.spinEqual(spindexer_intakePos3, robot.spin1Pos.getVoltage())){
spindexerPos = spindexer_intakePos2; spindexerPos = spindexer_intakePos2;
} }
} }
@@ -158,7 +158,7 @@ public class IntakeTest extends LinearOpMode {
TELE.addData("B1", ballIn(1)); TELE.addData("B1", ballIn(1));
TELE.addData("B2", ballIn(2)); TELE.addData("B2", ballIn(2));
TELE.addData("B3", ballIn(3)); TELE.addData("B3", ballIn(3));
TELE.addData("Spindex Pos", servo.getSpinPos()); TELE.addData("Spindex Pos", servo.getSpinPos(robot.spin1Pos.getVoltage()));
TELE.update(); TELE.update();
} }
} }
@@ -187,10 +187,10 @@ public class IntakeTest extends LinearOpMode {
} }
public void spindexer() { public void spindexer() {
boolean atTarget = servo.spinEqual(spindexerPos); boolean atTarget = servo.spinEqual(spindexerPos, robot.spin1Pos.getVoltage());
if (!atTarget) { if (!atTarget) {
powPID = servo.setSpinPos(spindexerPos); powPID = servo.setSpinPos(spindexerPos, robot.spin1Pos.getVoltage());
robot.spin1.setPower(powPID); robot.spin1.setPower(powPID);
robot.spin2.setPower(-powPID); robot.spin2.setPower(-powPID);

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

@@ -29,12 +29,12 @@ public class PositionalServoProgrammer extends LinearOpMode {
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
servo = new Servos(hardwareMap); servo = new Servos();
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
while (opModeIsActive()){ while (opModeIsActive()){
if (spindexPos != 0.501 && !servo.spinEqual(spindexPos) && mode == 0){ if (spindexPos != 0.501 && !servo.spinEqual(spindexPos, robot.spin1Pos.getVoltage()) && mode == 0){
double pos = servo.setSpinPos(spindexPos); double pos = servo.setSpinPos(spindexPos, robot.spin1Pos.getVoltage());
robot.spin1.setPower(pos); robot.spin1.setPower(pos);
robot.spin2.setPower(-pos); robot.spin2.setPower(-pos);
} else if (mode == 0){ } else if (mode == 0){
@@ -44,8 +44,8 @@ public class PositionalServoProgrammer extends LinearOpMode {
robot.spin1.setPower(spindexPow); robot.spin1.setPower(spindexPow);
robot.spin2.setPower(-spindexPow); robot.spin2.setPower(-spindexPow);
} }
if (turretPos != 0.501 && !servo.turretEqual(turretPos)){ if (turretPos != 0.501 && !servo.turretEqual(turretPos, robot.turr1Pos.getCurrentPosition())){
double pos = servo.setTurrPos(turretPos); double pos = servo.setTurrPos(turretPos, robot.turr1Pos.getCurrentPosition());
robot.turr1.setPower(pos); robot.turr1.setPower(pos);
robot.turr2.setPower(-pos); robot.turr2.setPower(-pos);
} else if (mode == 0){ } else if (mode == 0){
@@ -72,8 +72,8 @@ public class PositionalServoProgrammer extends LinearOpMode {
// If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE // If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE
//TODO: @KeshavAnandCode do the above please //TODO: @KeshavAnandCode do the above please
TELE.addData("spindexer pos", servo.getSpinPos()); TELE.addData("spindexer pos", servo.getSpinPos(robot.spin1Pos.getVoltage()));
TELE.addData("turret pos", servo.getTurrPos()); TELE.addData("turret pos", servo.getTurrPos(robot.turr1Pos.getCurrentPosition()));
TELE.addData("spindexer voltage 1", robot.spin1Pos.getVoltage()); TELE.addData("spindexer voltage 1", robot.spin1Pos.getVoltage());
TELE.addData("spindexer voltage 2", robot.spin2Pos.getVoltage()); TELE.addData("spindexer voltage 2", robot.spin2Pos.getVoltage());
TELE.addData("hood pos", robot.hood.getPosition()); TELE.addData("hood pos", robot.hood.getPosition());

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

@@ -22,39 +22,38 @@ public class Servos {
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;
public Servos(HardwareMap hardwareMap) { public Servos() {
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);
} }
// In the code below, encoder = robot.servo.getVoltage() // In the code below, encoder = robot.servo.getVoltage()
public double getSpinPos() { public double getSpinPos(double voltage) {
return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3); return spin_scalar * ((voltage - 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, double voltage) {
spinPID.setPIDF(spinP, spinI, spinD, spinF); spinPID.setPIDF(spinP, spinI, spinD, spinF);
return spinPID.calculate(this.getSpinPos(), pos); return spinPID.calculate(this.getSpinPos(voltage), pos);
} }
public boolean spinEqual(double pos) { public boolean spinEqual(double pos, double voltage) {
return Math.abs(pos - this.getSpinPos()) < 0.02; return Math.abs(pos - this.getSpinPos(voltage)) < 0.02;
} }
public double getTurrPos() { public double getTurrPos(double apos) {
return robot.turr1Pos.getCurrentPosition(); return apos;
} }
public double setTurrPos(double pos) { public double setTurrPos(double pos, double apos) {
turretPID.setPIDF(turrP, turrI, turrD, turrF); turretPID.setPIDF(turrP, turrI, turrD, turrF);
return spinPID.calculate(this.getTurrPos(), pos); return spinPID.calculate(this.getTurrPos(apos), pos);
} }
public boolean turretEqual(double pos) { public boolean turretEqual(double pos, double apos) {
return Math.abs(pos - this.getTurrPos()) < 0.01; return Math.abs(pos - this.getTurrPos(apos)) < 0.01;
} }
} }