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

organize/expand #1

Open
Abhiram wants to merge 3 commits from organize/expand into danielv2
pull from: organize/expand
merge into: KeshavAnandCode:danielv2
Conversation 0 Commits 3 Files Changed 4 +22 -25
1 changed files with 22 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 5931b2e7fa - Show all commits

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

@@ -9,7 +9,6 @@ import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -17,6 +16,7 @@ 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.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.LimelightManager;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
@@ -31,6 +31,7 @@ public class TeleopV3 extends LinearOpMode {
Servos servo;
Flywheel flywheel;
MecanumDrive drive;
LimelightManager limelightManager;
public static double manualVel = 3000;
public static double hoodDefaultPos = 0.5;
@@ -101,15 +102,15 @@ public class TeleopV3 extends LinearOpMode {
servo = new Servos(hardwareMap);
flywheel = new Flywheel();
drive = new MecanumDrive(hardwareMap, teleStart);
limelightManager = new LimelightManager(hardwareMap, Robot.usingLimelight);
limelightManager.init();
if (redAlliance) {
robot.limelight.pipelineSwitch(3);
limelightManager.switchMode(LimelightManager.LimelightMode.RED_GOAL);
} else {
robot.limelight.pipelineSwitch(2);
limelightManager.switchMode(LimelightManager.LimelightMode.BLUE_GOAL);
}
robot.limelight.start();
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
@@ -304,30 +305,26 @@ public class TeleopV3 extends LinearOpMode {
}
if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1) { //not moving
double bearing;
limelightManager.update();
double bearing = limelightManager.getBearing();
LLResult result = robot.limelight.getLatestResult();
if (result != null) {
if (result.isValid()) {
bearing = result.getTx();
overrideTurr = true;
turretPos = servo.getTurrPos() - (bearing / 1300);
if (limelightManager.getLatestResult() != null && limelightManager.getLatestResult().isValid()) {
overrideTurr = true;
turretPos = servo.getTurrPos() - (bearing / 1300);
double bearingCorrection = 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;
}
// 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());
}
camTicker++;
TELE.addData("tx", bearing);
TELE.addData("ty", limelightManager.getLatestResult().getTy());
}
} else {