[wpimath] Add cosineScale method to SwerveModuleState and instance optimize

[narmstro2020]

This PR adds an instance method for cosineScaling to the SwerveModuleState object and makes the static optimize method an instance method.

[calcmogul]

cosineScale() needs a unit test, and a C++ port is needed. Otherwise, looks good.

[narmstro2020]

cosineScale() needs a unit test, and a C++ port is needed. Otherwise, looks good.

No problem. Working on it.

Also I should've mentioned earlier. This makes cosineScaling more visible. I just stumbled on this last year when doing a random look through examples.

[narmstro2020]

cosineScale() needs a unit test, and a C++ port is needed. Otherwise, looks good.

Should I model the unit test like the test for optimize?

[calcmogul]

Should I model the unit test like the test for optimize?

You could just write a single test that tests a positive and negative speed for different angles around the unit circle (e.g., 0°, 45°, -45°, 135°, -135°, 180°).

[narmstro2020]

Should I model the unit test like the test for optimize?

You could just write a single test that tests a positive and negative speed for different angles around the unit circle (e.g., 0°, 45°, -45°, 135°, -135°, 180°).

Just to clarify. Just pick an angle. And test both positive and negative speed against it.

[calcmogul]

Yea, for a range of angles. Just needs to be a representative sample of inputs.

[narmstro2020]

What are my c++ issues in the example SwerveModule's?

[Gold856]

You need to switch all uses of the static method to the instance method, including in the C++ tests.

[narmstro2020]

You need to switch all uses of the static method to the instance method, including in the C++ tests.

I was getting local build errors on just using the void method. I wanted to clear that up before the test. Maybe I was misreading things.

[narmstro2020]

/format