 /* 
  * Authors: Nagavarun Kanaparthy
  * Resources:
+ * Arduino Mega Reference
+ * https://www.arduino.cc/en/Main/arduinoBoardMega
  * Sabertooth Motor Controller Library
  * https://www.dimensionengineering.com/software/SabertoothArduinoLibrary/html/class_sabertooth.html
  * Mechanum Drive Alogrithm Reference

 */
#include <Sabertooth.h>
//Pin Definitions

//Motor Serial only Transmit(TX) no Receive(RX).
#define MotorSerial Serial1
//MotorControllers
#define MotorAddress1 128
#define MotorAddress2 129
#define MotorRangeConst 127
//Constants
const float pi = 3.14;
//Variables
char incomingByte;
byte usbByteCount = 0;
String usbCommmand = "";
//Motors
Sabertooth frontMotors = Sabertooth(MotorAddress1, MotorSerial);
Sabertooth rearMotors = Sabertooth(MotorAddress2, MotorSerial);

//Support Functions
void setMotion(String angleString, String magString, String speedString){
  //conversion

void setup() {
  Serial.begin(9600);
  MotorSerial.begin(9600);
  pinMode(transPin, OUTPUT);
}
void loop() {
  while (Serial.available() && usbByteCount < 20) {
