Skip to content

Commit f1d2ec7

Browse files
today work
1 parent 4b3fe78 commit f1d2ec7

File tree

4 files changed

+32
-14
lines changed

4 files changed

+32
-14
lines changed

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ public void runOpMode() throws InterruptedException {
2525
cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 8, Math.toRadians(-70), Math.toRadians(45));
2626

2727
waitForStart();
28-
28+
//ryan is a silly goofy fella
2929

3030
while (!isStopRequested()) {
3131
drive.update(); // MUST be called every loop cycle so that RoadRunner calculates the pose correctly

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java

Lines changed: 24 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -13,10 +13,12 @@
1313
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
1414
import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.WaitSegment;
1515
import org.firstinspires.ftc.teamcode.util.GlobalOpMode;
16+
import org.firstinspires.ftc.teamcode.util.Timeout;
1617
import org.firstinspires.ftc.teamcode.vision.AprilTagCamera;
1718
import org.firstinspires.ftc.teamcode.vision.AprilTagLocalizer;
1819
import org.firstinspires.ftc.teamcode.vision.PropellerDetection1;
1920
import org.firstinspires.ftc.teamcode.vision.TensorFlowDetection;
21+
import org.tensorflow.lite.Tensor;
2022

2123
@Autonomous
2224
@Config
@@ -37,41 +39,50 @@ private Pose2d estimateWithAllCameras(AprilTagCamera[] cameras, AprilTagLocalize
3739
return pose;
3840
}
3941

42+
public static double LEFTFORTYFIVE = -45;
43+
public static double LEFTSEVENTY = 70;
44+
public static double RIGHTFORTYFIVE = 45;
45+
public static double RIGHTSEVENTY = -70;
46+
4047
@Override
4148
public void runOpMode() throws InterruptedException {
4249
GlobalOpMode.opMode = this;
4350
TestBotDrive drive = new TestBotDrive(hardwareMap);
4451

4552
AprilTagCamera[] cameras = new AprilTagCamera[3];
46-
cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 8, Math.toRadians(70), Math.toRadians(-45));
53+
cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 8, Math.toRadians(LEFTSEVENTY), Math.toRadians(LEFTFORTYFIVE));
4754
cameras[1] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Center"), 7, Math.toRadians(90), Math.toRadians(0));
48-
cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 8, Math.toRadians(-70), Math.toRadians(45));
55+
cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 8, Math.toRadians(RIGHTSEVENTY), Math.toRadians(RIGHTFORTYFIVE));
4956

5057

51-
TensorFlowDetection tensor = new TensorFlowDetection(cameras[1].webcamName);
58+
//TensorFlowDetection tensor = new TensorFlowDetection(cameras[1].webcamName);
59+
TensorFlowDetection tensor = null;
5260
Log.i("PROGRESS", "I did this!");
5361
// while (!isStopRequested()) {
5462
// tensor.getPropPosition();
5563
// android.util.Log.w("LOCATION", "tensor result " + tensor.getPropPosition());
5664
// }
5765

5866

59-
TensorFlowDetection.PropPosition tensorPos = tensor.getPropPosition();
67+
6068

6169
// TensorFlowDetection.PropPosition tensorPos = TensorFlowDetection.PropPosition.LEFT;
62-
Log.i("PROGRESS", "Made it here");
63-
Log.i("PROGRESS", tensorPos + "");
64-
Log.i("PROGRESS", tensor.getPropPosition() + "");
70+
6571

6672

6773
AprilTagLocalizer aprilTag = new AprilTagLocalizer(cameras);
6874
Pose2d startPose = null;
6975
Log.i("PROGRESS", "here?");
70-
while(opModeInInit() && !isStopRequested() && startPose == null) {
76+
while(opModeInInit() && !isStopRequested() ) {
7177
Log.i("PROGRESS", "before");
7278
startPose = estimateWithAllCameras(cameras, aprilTag);
7379
Log.i("PROGRESS", "after");
80+
if (startPose != null) {
81+
drive.setPoseEstimate(startPose);
82+
drive.update();
83+
}
7484
}
85+
waitForStart();
7586
Log.i("PROGRESS", "Maybe?");
7687
if (startPose == null) {
7788
// Check one more time
@@ -90,6 +101,11 @@ public void runOpMode() throws InterruptedException {
90101
drive.setPoseEstimate(startPose);
91102
Log.i("PROGRESS", "Im here too!");
92103

104+
TensorFlowDetection.PropPosition tensorPos = tensor.getPropPosition();
105+
if (tensorPos == null) {
106+
tensorPos = TensorFlowDetection.PropPosition.CENTER;
107+
}
108+
93109

94110
switch (AprilTagLocalizer.whichQuadrant(startPose)) {
95111
case RED_BOARD:
@@ -285,7 +301,6 @@ public void runOpMode() throws InterruptedException {
285301
break;
286302
}
287303
Log.i("PROGRESS", "I got here???");
288-
waitForStart();
289304

290305
if (!isStopRequested())
291306
drive.followTrajectorySequence(traj);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagCamera.java

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -21,8 +21,8 @@ public AprilTagCamera(WebcamName webcamName, double offset, double orientation,
2121
this.angle = angle;
2222
}
2323

24-
public Pose2d translatePoseOrig(Pose2d inputPose) {
25-
double thetaNeed = -orientation + (inputPose.getHeading() - Math.toRadians(90)) + angle;
24+
public Pose2d translatePose(Pose2d inputPose) {
25+
double thetaNeed = Math.toRadians(180) - angle - inputPose.getHeading() + orientation;
2626
android.util.Log.e("APRILTAG", "MAGIC ANGLE: " + Math.toDegrees(thetaNeed));
2727

2828
double offsetX = offset * Math.cos(thetaNeed);
@@ -44,7 +44,7 @@ public Pose2d translatePoseOrig(Pose2d inputPose) {
4444
* @param inputPose Pose2d to translate
4545
* @return translated Pose2d
4646
*/
47-
public Pose2d translatePose(Pose2d inputPose) {
47+
public Pose2d translatePoseFudged(Pose2d inputPose) {
4848
// 6.5: Distance from left camera to center of robot
4949
// -6.5: Distance from right camera to center of robot
5050
// 5: distance from center of robot to cameras (forward)

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TensorFlowDetection.java

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
77
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
88
import org.firstinspires.ftc.teamcode.util.GlobalOpMode;
9+
import org.firstinspires.ftc.teamcode.util.Timeout;
910
import org.firstinspires.ftc.vision.VisionPortal;
1011
import org.firstinspires.ftc.vision.tfod.TfodProcessor;
1112

@@ -105,6 +106,8 @@ public enum PropPosition{
105106
LEFT, RIGHT, CENTER
106107
}
107108

109+
110+
108111
public PropPosition getPropPosition() {
109112
List<Recognition> currentRecognitions = tfod.getRecognitions();
110113
while(currentRecognitions.size() < 1 && !GlobalOpMode.opMode.isStopRequested()) {
@@ -122,7 +125,7 @@ public PropPosition getPropPosition() {
122125

123126
// GET MORE CURRENT RECOGNITIONS?
124127
// }
125-
visionPortal.close();
128+
//visionPortal.close();
126129

127130
double centerX = (recognition.getLeft() + recognition.getRight()) / 2 ;
128131
if (centerX < 214) {

0 commit comments

Comments
 (0)