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

tele pose transfer

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-05 18:51:54 -05:00
parent c36cac12f2
commit e065084964
3 changed files with 20 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
View File

@@ -19,6 +19,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.constants.TeleStart;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.utilsv2.*; import org.firstinspires.ftc.teamcode.utilsv2.*;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
@@ -388,9 +389,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
follower.update(); follower.update();
pathStateMachine(); pathStateMachine();
Pose currentPose = follower.getPose(); Pose currentPose = follower.getPose();
teleStartPoseX = currentPose.getX(); TeleStart.teleStart = currentPose;
teleStartPoseY = currentPose.getY();
teleStartPoseH = Math.toDegrees(currentPose.getHeading());
spindexer.update(); spindexer.update();
@@ -402,9 +401,9 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.addData("X:", currentPose.getX()); TELE.addData("X:", teleStartPoseX);
TELE.addData("Y:", currentPose.getY()); TELE.addData("Y:", teleStartPoseY);
TELE.addData("H:", currentPose.getHeading()); TELE.addData("H:", teleStartPoseH);
TELE.update(); TELE.update();
} }
} }

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TeleStart.java Normal file
View File

@@ -0,0 +1,7 @@
package org.firstinspires.ftc.teamcode.constants;
import com.pedropathing.geometry.Pose;
public class TeleStart {
public static Pose teleStart = new Pose(72,72,0);
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
View File

@@ -13,6 +13,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.TeleStart;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utilsv2.*; import org.firstinspires.ftc.teamcode.utilsv2.*;
@@ -50,8 +51,10 @@ public class TeleopV4 extends LinearOpMode {
commander = new VelocityCommander(); commander = new VelocityCommander();
drivetrain = new Drivetrain(robot, TELE); drivetrain = new Drivetrain(robot, TELE);
follower = Constants.createFollower(hardwareMap); follower = Constants.createFollower(hardwareMap);
Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH)); follower.setStartingPose(TeleStart.teleStart);
follower.setStartingPose(start); sleep(500);
follower.setPose(TeleStart.teleStart);
follower.update();
flywheel = new Flywheel(robot); flywheel = new Flywheel(robot);
turret = new Turret(robot); turret = new Turret(robot);
@@ -73,6 +76,9 @@ public class TeleopV4 extends LinearOpMode {
limelightUsed = true; limelightUsed = true;
TELE.addLine("Initialization is done");
TELE.update();
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;