Improve stage avoidance to react correctly

[jkleiber]

Purpose The purpose of this change is to ensure that stage avoidance actually avoids the stage. This likely means increasing the time to put the shooter down based on angle.

Additionally, we should not avoid the stage if we are going to hit the podium. We could solve this by checking if we are intersecting / driving into that square region, and mark the stage as being avoided in that case

Project Scope

• [ ] Tune shooter deck lookup table for stage avoidance
• [ ] Add logic to not trigger stage avoidance when lining up at the podium or other feet of the stage

[aidnem]

Note from last night's meeting (@jkleiber please correct me if I'm wrong): We are now only not triggering stage avoidance when lining up at the podium, as we shouldn't really be lining up at any of the other legs and it seems like extra overhead for not much benefit.

[jkleiber]

yep that is correct

[aidnem]

Also @minhnguyenbhs as I was falling asleep last night I realized our isPointInQuad function doesn't work correctly. I'm dropping this comment here so I don't forget it. I didn't want to edit the branch in case you have local changes that would conflict. The following lines (line 242-245 in src/main/java/frc/robot/utils/FieldFinder.java):

        double a1 = areaOfQuad(x, y, x2, y2, x3, y3, x4, y4);
        double a2 = areaOfQuad(x1, y1, x, y, x3, y3, x4, y4);
        double a3 = areaOfQuad(x1, y1, x2, y2, x, y, x4, y4);
        double a4 = areaOfQuad(x1, y1, x2, y2, x3, y3, x, y);

Should actually be this:

        double a1 = areaOfTriangle(x1, y1, x2, y2, x, y);
        double a2 = areaOfTriangle(x2, y2, x3, y3, x, y);
        double a3 = areaOfTriangle(x3, y3, x4, y4, x, y);
        double a4 = areaOfTriangle(x4, y4, x1, y1, x, y);

This is because we should still be using triangles to sum up the area of the quadrilateral (one triangle per side of the quad). I can draw it tomorrow at shop to justify if needed.

[minhnguyenbhs]

oh haha you're right, oops! i probably should've drawn it out on monday before coding- thanks, will fix :D