Chassis Alignment

[pknessness]

Write simple code that aligns the chassis to the head when pressing a button, so that when we want to jump the ramp, we can align the chassis in the correct orientation and then simply move forwards.

The goal is to passively (and without much power limiting, align the wheels to the direction where

/..........\
.....o==
\........../

where the == is the turret, and the /'s are wheels

The goal is to eventually have an alignment angle that we can modify, because at low speeds, a different alignment of wheels is good, and then we can have that alignment angle change based on the chassis velocity.

For note, at low velocities, the following alignment is optimal, so being able to align the chassis for optimal direction of acceleration and then movement is beneficial.
.....---
|.....o==|
.....---

At low chassis velocities, when we need acceleration, we can have the offset closer to the bottom image, and when our velocities are higher and we want a higher top speed, we can have an offset closer to the top image.

The preferred way to implement this would be in the chassis class, with a variable, in which a rotation is applied to the robot in the setChassisSpeeds function, to bring the alignment to its correct value. There should be an extra variable with something like "springiness" which changes how hard the system attempts to correct to the given alignment.