Create Subsystem_Functions.c

Subsystem_Functions.c

“Subsystem_Functions.c” contains the functions that control each subsystem.  Each is built in layers.  For instance, for the drivetrain, the first function defined is “setDriveMotors().”  This function allows for setting of all the drivetrain motor outputs with only two numbers, the left and right values.  On top of this is built a framework for interpreting the user’s inputs, filtering out noise, and computing the correct left and right values - the function “setDrive().”  In addition, “setDriveMotors()” is used in the autonomous driving function, “driveInches().”  “driveInches()” takes an input of a number of inches and controls the robot’s drivetrain via PID to drive that many inches.  Each subsystem follows a similar pattern, with a low level function handling the actual motor outputs, a user control function calling the low-level function based on user input, and an autonomous function using PID to command that system to a given position.  Encapsulating the subsystem functions with descriptive function names makes the code readable; it is relatively obvious what each function does.