updated driveDistance for lens auto

SugarDulcePan · SugarDulcePan · commit f04b08183b79 · 2026-03-07T09:28:47.000-05:00
- coordinated strafe with milliseconds
- deleted driveDistance test in Blue_close_w_huskey
diff --git a/TeamCode/src/main/java/org/mrpsvt/capital_robotics/auto/Blue_close_w_huskey.java b/TeamCode/src/main/java/org/mrpsvt/capital_robotics/auto/Blue_close_w_huskey.java
@@ -104,22 +104,13 @@ private void autonomousSequence() throws InterruptedException
         telemetry.update();
         driveDistance(.5, 0, 0, 1700);
 
-        // drive distance test
-        driveDistance(0, 1, 0, 400);
-        sleep(3000);
-        driveDistance(0, -1, 0, 400);
-        sleep(3000);
-        driveDistance(0, .3, 0, 200);
-        sleep(3000);
-        driveDistance(0, -.3, 0, 200);
-        sleep(3000);
 
         // step 2.5: activate camera homing for x coordinate
-        /*
+
         telemetry.addData("step", "huskey x homing");
         telemetry.update();
         ourHuskeymethod.homingLens();
-        */
+
 
         /*
         //+ to the rigth - to the lefft
diff --git a/TeamCode/src/main/java/org/mrpsvt/capital_robotics/auto/husky_method.java b/TeamCode/src/main/java/org/mrpsvt/capital_robotics/auto/husky_method.java
@@ -82,46 +82,62 @@ public void homingLens()
                 int h = 41;
                 int w = 41;
                 int change = 0;
+                int lor = 0;
 
                 // driveDistance(), - = foreward, - = right, unknown
                 while (tagX > (x - 3) || tagX < (x + 3))
                 {
                     telemetry.addData("action", "triangulating x");
                     if (tagX > x && change == 0)
                     {
-                        driveDistance(0, .3, 0, 400);
+                        driveDistance(0, .5, 0, 400);
                         tagX = block.x;
                         if (tagX < x)
                         {
                             change++;
                         }
+                        lor = 0;
                     }
                     else if (tagX > x && change > 1)
                     {
-                        driveDistance(0, .3, 0, (long) (400 * Math.pow(.75, change)));
+                        driveDistance(0, .5 * Math.pow(.75, change), 0, (long) (400 * Math.pow(.75, change)));
                         tagX = block.x;
                         if (tagX < x)
                         {
                             change++;
                         }
+                        lor = 0;
                     }
                     else if (tagX < x && change == 0)
                     {
-                        driveDistance(0, -.3, 0, 400);
+                        driveDistance(0, -.5, 0, 400);
                         tagX = block.x;
                         if (tagX > x)
                         {
                             change++;
                         }
+                        lor = 1;
                     }
                     else if (tagX < x && change > 1)
                     {
-                        driveDistance(0, -.3, 0, (long) (400 * Math.pow(.75, change)));
+                        driveDistance(0, -.5 * Math.pow(.75, change), 0, (long) (400 * Math.pow(.75, change)));
                         tagX = block.x;
                         if (tagX > x)
                         {
                             change++;
                         }
+                        lor = 1;
+                    }
+                    else
+                    {
+                        if (lor == 0)
+                        {
+                            driveDistance(0, -.3, 0, 400);
+                        }
+                        else
+                        {
+                            driveDistance(0, .3, 0, 400);
+                        }
                     }
                 }
 

Original file line number	Diff line number	Diff line change
`@@ -82,46 +82,62 @@ public void homingLens()`
`82`	`82`	`int h = 41;`
`83`	`83`	`int w = 41;`
`84`	`84`	`int change = 0;`
	`85`	`+ int lor = 0;`
`85`	`86`
`86`	`87`	`// driveDistance(), - = foreward, - = right, unknown`
`87`	`88`	`while (tagX > (x - 3) \|\| tagX < (x + 3))`
`88`	`89`	`{`
`89`	`90`	`telemetry.addData("action", "triangulating x");`
`90`	`91`	`if (tagX > x && change == 0)`
`91`	`92`	`{`
`92`		`- driveDistance(0, .3, 0, 400);`
	`93`	`+ driveDistance(0, .5, 0, 400);`
`93`	`94`	`tagX = block.x;`
`94`	`95`	`if (tagX < x)`
`95`	`96`	`{`
`96`	`97`	`change++;`
`97`	`98`	`}`
	`99`	`+ lor = 0;`
`98`	`100`	`}`
`99`	`101`	`else if (tagX > x && change > 1)`
`100`	`102`	`{`
`101`		`- driveDistance(0, .3, 0, (long) (400 * Math.pow(.75, change)));`
	`103`	`+ driveDistance(0, .5 * Math.pow(.75, change), 0, (long) (400 * Math.pow(.75, change)));`
`102`	`104`	`tagX = block.x;`
`103`	`105`	`if (tagX < x)`
`104`	`106`	`{`
`105`	`107`	`change++;`
`106`	`108`	`}`
	`109`	`+ lor = 0;`
`107`	`110`	`}`
`108`	`111`	`else if (tagX < x && change == 0)`
`109`	`112`	`{`
`110`		`- driveDistance(0, -.3, 0, 400);`
	`113`	`+ driveDistance(0, -.5, 0, 400);`
`111`	`114`	`tagX = block.x;`
`112`	`115`	`if (tagX > x)`
`113`	`116`	`{`
`114`	`117`	`change++;`
`115`	`118`	`}`
	`119`	`+ lor = 1;`
`116`	`120`	`}`
`117`	`121`	`else if (tagX < x && change > 1)`
`118`	`122`	`{`
`119`		`- driveDistance(0, -.3, 0, (long) (400 * Math.pow(.75, change)));`
	`123`	`+ driveDistance(0, -.5 * Math.pow(.75, change), 0, (long) (400 * Math.pow(.75, change)));`
`120`	`124`	`tagX = block.x;`
`121`	`125`	`if (tagX > x)`
`122`	`126`	`{`
`123`	`127`	`change++;`
`124`	`128`	`}`
	`129`	`+ lor = 1;`
	`130`	`+ }`
	`131`	`+ else`
	`132`	`+ {`
	`133`	`+ if (lor == 0)`
	`134`	`+ {`
	`135`	`+ driveDistance(0, -.3, 0, 400);`
	`136`	`+ }`
	`137`	`+ else`
	`138`	`+ {`
	`139`	`+ driveDistance(0, .3, 0, 400);`
	`140`	`+ }`
`125`	`141`	`}`
`126`	`142`	`}`
`127`	`143`