Created SwerveControlAlgorithm for testing

This algorithm will simply output signals to the motors that will make the whole thing swerve back and forth.
For testing the operation of the simulation, seeing if it accurately represents the position of the wheels.