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

62
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.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.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Robot;
import java.util.ArrayList;
@@ -37,8 +36,8 @@ public class TeleopV2 extends LinearOpMode {
List<Boolean> s1 = new ArrayList<>();
List<Boolean> s2 = new ArrayList<>();
List<Boolean> s3 = new ArrayList<>();
double desiredTurretAngle = 180;
// MecanumDrive drive = new MecanumDrive(hardwareMap, teleStart);
public static double desiredTurretAngle = 180;
MecanumDrive drive;
private double lastEncoderRevolutions = 0.0;
private double lastTimeStamp = 0.0;
private double velo1, velo;
@@ -59,7 +58,7 @@ public class TeleopV2 extends LinearOpMode {
telemetry, FtcDashboard.getInstance().getTelemetry()
);
// drive = new MecanumDrive(hardwareMap, teleStart);
drive = new MecanumDrive(hardwareMap, teleStart);
waitForStart();
if (isStopRequested()) return;
@@ -129,6 +128,8 @@ public class TeleopV2 extends LinearOpMode {
if (gP >= 0.43) {
s1.add(true);
} else {
s1.add(false);
}
}
@@ -144,6 +145,8 @@ public class TeleopV2 extends LinearOpMode {
if (gP >= 0.43) {
s2.add(true);
} else {
s2.add(false);
}
}
@@ -159,6 +162,8 @@ public class TeleopV2 extends LinearOpMode {
if (gP >= 0.43) {
s3.add(true);
} else {
s3.add(false);
}
}
@@ -166,8 +171,6 @@ public class TeleopV2 extends LinearOpMode {
boolean green2 = false;
boolean green3 = false;
if (!s1.isEmpty()) {
green1 = s1.get(s1.size() - 1);
}
@@ -240,31 +243,30 @@ public class TeleopV2 extends LinearOpMode {
//TURRET:
// double 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);
//
double offset;
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:
// drive.updatePoseEstimate();
drive.updatePoseEstimate();
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
@@ -273,9 +275,9 @@ public class TeleopV2 extends LinearOpMode {
TELE.addData("Spin1Green", green1);
TELE.addData("Spin2Green", green2);
TELE.addData("Spin3Green", green3);
//
// TELE.addData("pose", drive.localizer.getPose());
// TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
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);
shooterEncoder = shooter1;
hood = hardwareMap.get(Servo.class, "hood");
hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");