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

update

Browse Source
This commit is contained in:
KeshavAnandCode
2025-12-01 21:43:03 -06:00
parent 0752c7c5f5
commit 55dbfaaa98
2 changed files with 37 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -12,9 +12,8 @@ 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 org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
//import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
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.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
import java.util.ArrayList; import java.util.ArrayList;
@@ -37,8 +36,8 @@ public class TeleopV2 extends LinearOpMode {
List<Boolean> s1 = new ArrayList<>(); List<Boolean> s1 = new ArrayList<>();
List<Boolean> s2 = new ArrayList<>(); List<Boolean> s2 = new ArrayList<>();
List<Boolean> s3 = new ArrayList<>(); List<Boolean> s3 = new ArrayList<>();
double desiredTurretAngle = 180; public static double desiredTurretAngle = 180;
// MecanumDrive drive = new MecanumDrive(hardwareMap, teleStart); MecanumDrive drive;
private double lastEncoderRevolutions = 0.0; private double lastEncoderRevolutions = 0.0;
private double lastTimeStamp = 0.0; private double lastTimeStamp = 0.0;
private double velo1, velo; private double velo1, velo;
@@ -59,7 +58,7 @@ public class TeleopV2 extends LinearOpMode {
telemetry, FtcDashboard.getInstance().getTelemetry() telemetry, FtcDashboard.getInstance().getTelemetry()
); );
// drive = new MecanumDrive(hardwareMap, teleStart); drive = new MecanumDrive(hardwareMap, teleStart);
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
@@ -129,6 +128,8 @@ public class TeleopV2 extends LinearOpMode {
if (gP >= 0.43) { if (gP >= 0.43) {
s1.add(true); s1.add(true);
} else {
s1.add(false);
} }
} }
@@ -144,6 +145,8 @@ public class TeleopV2 extends LinearOpMode {
if (gP >= 0.43) { if (gP >= 0.43) {
s2.add(true); s2.add(true);
} else {
s2.add(false);
} }
} }
@@ -159,6 +162,8 @@ public class TeleopV2 extends LinearOpMode {
if (gP >= 0.43) { if (gP >= 0.43) {
s3.add(true); s3.add(true);
} else {
s3.add(false);
} }
} }
@@ -166,16 +171,14 @@ public class TeleopV2 extends LinearOpMode {
boolean green2 = false; boolean green2 = false;
boolean green3 = false; boolean green3 = false;
if (!s1.isEmpty()) { if (!s1.isEmpty()) {
green1 = s1.get(s1.size() - 1); green1 = s1.get(s1.size() - 1);
} }
if (!s2.isEmpty()) { if (!s2.isEmpty()) {
green2 = s2.get(s2.size() - 1); green2 = s2.get(s2.size() - 1);
} }
if (!s3.isEmpty()) { if (!s3.isEmpty()) {
green3 = s3.get(s3.size() - 1); green3 = s3.get(s3.size() - 1);
} }
//SHOOTER: //SHOOTER:
@@ -240,31 +243,30 @@ public class TeleopV2 extends LinearOpMode {
//TURRET: //TURRET:
// double offset = desiredTurretAngle - Math.toDegrees(drive.localizer.getPose().heading.toDouble()); double offset;
//
// if (offset < -90) {
// offset+=360;
// }
//
// double pos = 0.3;
//
// pos += offset * (0.9/360);
//
// if (pos < 0.02){
// pos = 0.02;
// } else if (pos > 0.91) {
// pos = 0.91;
// }
//
// robot.turr1.setPosition(pos);
// robot.turr2.setPosition(1-pos);
//
offset = desiredTurretAngle - Math.toDegrees(drive.localizer.getPose().heading.toDouble());
if (offset > 90) {
offset -= 360;
}
double pos = 0.3;
pos -= offset * (0.9 / 360);
if (pos < 0.02) {
pos = 0.02;
} else if (pos > 0.91) {
pos = 0.91;
}
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1 - pos);
//MISC: //MISC:
// drive.updatePoseEstimate(); drive.updatePoseEstimate();
for (LynxModule hub : allHubs) { for (LynxModule hub : allHubs) {
hub.clearBulkCache(); hub.clearBulkCache();
@@ -273,9 +275,9 @@ public class TeleopV2 extends LinearOpMode {
TELE.addData("Spin1Green", green1); TELE.addData("Spin1Green", green1);
TELE.addData("Spin2Green", green2); TELE.addData("Spin2Green", green2);
TELE.addData("Spin3Green", green3); TELE.addData("Spin3Green", green3);
//
// 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.update(); TELE.update();

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

@@ -105,6 +105,8 @@ public class Robot {
shooter1.setDirection(DcMotorSimple.Direction.REVERSE); shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
shooterEncoder = shooter1;
hood = hardwareMap.get(Servo.class, "hood"); hood = hardwareMap.get(Servo.class, "hood");
hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos"); hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");