Implement Imaginary Drivetrain

[gerth2]

Using 2019's code as a reference, implement the Imaginary Drivetrain class.

https://github.com/RobotCasserole1736/RobotCasserole2019/blob/master/Robotcode/RobotCode2019/src/main/java/frc/robot/DrivetrainSim.java

https://github.com/RobotCasserole1736/RobotCasserole2019/blob/master/Robotcode/RobotCode2019/src/main/java/frc/robot/RobotPose.java

It can mostly be copy-paste.

Integrate such that the appropriate update() methods are called in the teleopPeriodic() method in Robot.java.

To test: In teleop periodic, add a call to the setClosedLoop... function with 5 ft/sec on each wheel. Run the code in simulation. Load up the robot webpage (should be at localhost:5805). Look at the webpage which shows the robot position, and see it moving forward across the field.

Change up what the wheel velocity commands are, and observe the robot move differently.

[Treecko120]

I think Chris finished this.

[NathanHuut]

I think so too. He said he would finish it up for me