One and half auton

[Chess318]

https://github.com/frc2399/2023-Season/blob/d16aaefaacc25f1689bc72083a3d73419dece2a1/src/main/java/frc/robot/commands/auton/OneAndHalfPieceEngage.java#L39-L42 Should we be lowering the arm into position BEFORE we drive up to the piece? And also, should we be intaking as we drive up to the game piece (ie in a parallel command group)?

[edf42001]

Heeyyyyy guys, remember when I suggested

The line currentAngle = modAngle(currentAngle); in TurnToNAngleCmd is unnecessary because of the lines double error = targetAngle - currentAngle; error = modAngle(error); which btw you could combine into double error = modAngle(targetAngle - currentAngle);

Turns out that broke the finish condition, because now current angle could be 360, target 0, but without modAngleing currentAngle those aren't within the range from each other :)

  public boolean isFinished() {
    return PIDUtil.checkWithinRange(targetAngle, currentAngle, range);
  }

The best way to fix this is to make the error variable a global member variable. Then you can call return PIDUtil.checkWithinRange(0, error, range); to check when the (modAngled) error is close to 0

You can test this in sim

[edf42001]

Next, I have two questions which I think you'll find very interesting:

• In sim, is the robot's starting position correct if you run two autos in a row?
• This one is extra interesting: In Sim, does the robot turn to the right angle? What about on the real robot?

Let me know if you want help investigating these!

[alicelin27]

Shouldn't it go to cone up intake height too at line 42?

[edf42001]

Hiiii, I discovered that the modAngle method actually only works for positive numbers :) This is due to how the % operator in Java is dumb, and is the "remainder" not the "modulus". For example, see the below test of modAngle:

  public double modAngle(double value) {
    return ((value + Math.PI) % (Math.PI * 2)) - Math.PI;
  }

    public static void main(String[] args){
        System.out.println(Test.modAngle(0));
        System.out.println(Test.modAngle(1.56));
        System.out.println(Test.modAngle(3.14));
        System.out.println(Test.modAngle(6.28));
        System.out.println(Test.modAngle(-3.14));
        System.out.println(Test.modAngle(-6.28));
    }
>>>
0.0
1.5600000000000005
3.1400000000000006
-0.003185307179585095
-3.14
-6.28

See how the negative 6.28 is too big?

I stack overflowed it and they suggest making your own modulus like this:

  public double modAngle(double value) {
    value = (value + Math.PI) % (Math.PI * 2);  // Take "remainder" (https://stackoverflow.com/a/2172061)
    value = value < 0 ? value + Math.PI * 2 : value;  // If less then 0, add the value to make it "modulus"
    return value - Math.PI;  // Subtract PI to make the angle in the range -PI to PI
  }
  >>>
  0.0
1.5600000000000005
3.1400000000000006
-0.003185307179585095
-3.14
0.0031853071795859833

See now the 6.28 has been made equal to 0 (like it should be) (-3.14 is in the range of -PI to PI because pi is 3.1415)

This is for turnToNAngle BTW