Fix Swerve Issues

[WesleyMcGinn]

This branch was created to account for issue #6. It is ready to be tested, modified, and merged into the main robot code. The following problems have been accounted for:

• [x] When steering, the robot moves more quickly.
• [x] When resistance is turned on, the navx yaw readings sometimes slowly drift without an easy way to get the robot back on track.
• [x] When resistance is turned on in headless mode, the robot rotates very slowly while moving and immediately rotates to face the right direction when it stops moving.
• [x] When the controller tells the robot to stop moving, all of the wheels on the robot immediately turn to stop the robot rather than pointing in the same direction so that the robot will slow to a stop. (This is a tipping risk.)

---

Here are some issues that could result from changes made in this branch, along with how to fix them:	Issue	Fix
Robot wheels point in the wrong direction when the controller puts no input into the robot.	Modify first two lines in navx.botDir().
Robot turns are much too sharp.	Change steeringAmplifier back to 0.5.
Robot turns much too far when trying to point in the right direction.	Adjust 0.015 in robot.java to be a lower number.
Robot overadjusts its rotation while driving.	Adjust 0.1 in robot.java to be a lower number.

[WesleyMcGinn]

While we did fix some swerve issues, some issues have not yet been fully resolved:

• [ ] When resistance is turned on in headless mode, the robot rotates very slowly while moving and immediately rotates to face the right direction when it stops moving.
• [ ] When the controller tells the robot to stop moving, all of the wheels on the robot immediately turn to stop the robot rather than pointing in the same direction so that the robot will slow to a stop. (This is a tipping risk.)

I will try to fix the first issue by modifying line 235 in Weswerve.java:

x = (speed*y)/(steeringAmplifier*rotationalInput);

After all, what right does the speed of the robot have to decrease the sharpness of the turn? If I am driving in a circle and I speed up, the circle shouldn't widen, yet this is what the code currently does - the faster the robot moves, the broader the turn. I forget why I first decided to implement this idea into the code, but I am going to try to remove it. (Note that x is the distance, in robot lengths, to the point that the robot travels around.)

[WesleyMcGinn]

MAJOR PROBLEM! ⚠️

The code in this branch was tested and has been proven to contain so many errors that it will simply be better to go with the old programming. With this code, the robot is unable to rotate in-place, the robot rotates extremely slowly when it can rotate, the robot automatically assumes an immediate ≈-120° rotation whenever teleop is enabled, the robotic arm goes flying up into the air in raw mode, and the main issue - the fact that "when the controller tells the robot to stop moving, all of the wheels on the robot immediately turn to stop the robot rather than pointing in the same direction so that the robot will slow to a stop" - became even worse. After 1 hour of testing and code-fixing, I have concluded that this branch of code is simply not worth it, and the changes made will therefore be deleted.