FRC 2019 LabVIEW Swerve project READ-ME File
--------------------------------------------

Revision 1.0
Date July 7, 2019

This project is public and freely distributed to the FIRST Robotics Competition (FRC) community.
Team 4048 a.k.a. Redshift welcomes code contributions and updates from anyone wishing to 
improve / update / modify this code to better support the FRC community.

This software is available without any warranty. FRC teams are free to use and modify it
for their own hardware. Changes are necessary to match each team's hardware. Use at your
own risk. Be sure to understand LabVIEW and FRC RoboRio programming.

Swerve Basics
-------------
This software implements 4-wheel swerve drive using AndyMark AM-3009 modules. Each module
contains a CIM for motion and a PG-71 for steering. In the software, both motors are 
controlled by Talon SRX motor controllers.

The user interface consists of 2 joysticks. The first joystick is for front/back and left/right
motion. The second joystick rotates the robot. 

Please make sure you understand the basics of 4-wheel swerve drives. Documentation and 
algorithms can be found on the chiefdelphi.com forum. Our implementation is derived from 
data found on that site (sorry we don't have a link handy at the moment).

Target and Dependencies
-----------------------
This code is built on LabVIEW 2018 used in FRC 2019 (Destination Deep Space).
It used Talon SRX motor controllers from Cross-the-road Electronics (CTRE). The CTRE
Phoenix installer software version used is 5.14.1.2.

Files
-----
Swerve Math.vi - this contains the core of the swerve algorithm and should mostly be 
device independent if your hardware is similar to our sample hardware. It takes data from 
the two joysticks, the width and length of the wheels center-to-center and an optional gyro
input (in degrees). The gyro is not implemented in this sample code but can easiy be done
with a Pigeon IMU. Using a gyro allows the robot to used in field-centric mode which is
extremely handy for the driver to use. It's also possible to switch between robot and 
field centric modes on-the-fly, or even change the "front" of the robot for automation
purposes. Calculations are performed for all four wheels at one time. The output of 
this VI contains wheel angle and speed information. Angular info is for the steering 
motor and speed info is for the drive motor.

Swerve Post.vi - this contains code that post-processing on a per-wheel basis. It takes
the wheel speed and angle info generated by Swerve Math.vi, plus some additional information
from the hardware and runtime environment. The output are values that can be sent to the 
hardware directly.
	Reverse Steer and Reverse Encoder - these are set to "true" for our hardware. They
		are made configurable for flexibility but we don't know if what will
		happen if set to "false" since our hardware doesn't work that way.
	Scale Speed - you can use this input to scale the output speed up or down 
		relative to the joystick input. 
	Gear ratio - You must provide a valid input based on your hardware. This number
		represents the total number of encoder counts for every full revolution
		of the wheel. The AndyMark swerve drive we're using is set to "1988/1.2". 
		1.2 = the external gearing (1.2)
		1988 = 7 (motor shaft encoder pulses per rotation) x 4 (quadrature counter
		counts per pulse) x 71 (PG71 gearing ratio)

Swerve ShouldReverse.vi - this is an optimization and therefore optional. The purpose of 
		this logic is perform the minimal amount of steering motion necessary.
		For example if the new wheel motion is a 180 degree turn, it's easier to 
		just reverse the drive wheel. Similarly any angular rotation greater than
		90 degrees (quarter turn) can be done more quickly by turning the other 
		way and reversing the wheel. If this function gives you problems, you can
		just have it return "false" all the time to effectively disable its effect.

Swerve steering init.vi - this is hardware dependent. Here it initializes the Talon SRX
		motor controllers. Steering is done through a PID and the PID parameters
		can be changed from this module's Front Panel.

Customization
-------------
Aside from the 4 swerve files mentioned above, here's some additional customization
you'll need to perform.

begin.vi - Set up your drive and steering motor controllers. Drive wheel motor controllers
	can be anything. Steering requires a PID which is why we used a Talon SRX. If you
	use a software PID, you're on your own. The AndyMark swerve drives also has an 
	absolute encoder. This is used to allow the software to automatically align each
	wheel at startup time. This is an analog encoder attached to an analog input on 
	the RoboRio. The constant "4000" is derived experimentally. The 12 bit RoboRio
	ADC give a max count of 4095 but the encoders never reach full voltage on their 
	output. Using "4000" gives the device slightly better accuracy than 4095 or 4096.
	If you don't use the absolute encoder (disabled by default via "Do Encoder Reset"
	on the Front Panel), you must physically align the robot wheels before running 
	the software.

teleop.vi - the swerve code is actually pretty straightforward since most of the heavy
	lifting is done in the "swerve xxx" code. Actual access to the hardware is done
	here so update the motor controller "set" functions as necessary. 

Be careful
----------
When looking at the code, be aware of the units and their range. 
- units don't matter on the robot wheelbase dimensions.
- encoder values are in counts when setting and getting. This is why you need the gear
ratio to convert to whole wheel rotations. Also remember that quad encoders don't reset
after a full turn in either direction; they just keep couting up or down.
- wheel speed range is from -1 to +1.
- wheel steering is usually in degrees but the trig math is usually in radians so converting
is important (see Swerve Math.vi). 
- be sure to understand the angular range at different parts of the code. For example, 
when first computed, angles are 0 to 180 but after optimizations, they are -90 to + 90

Feedback
--------
Contact Team 4048 (Redshift) at info@team4048.org