diff --git a/src/InterLinkElite.cpp b/src/InterLinkElite.cpp
new file mode 100644
index 0000000..60b4415
--- /dev/null
+++ b/src/InterLinkElite.cpp
@@ -0,0 +1,96 @@
+/*
+ * InterLinkElite.cpp
+ *
+ *  Created on: May 16, 2015
+ *      Author: user
+ */
+
+#include "InterLinkElite.h"
+
+InterLinkElite::InterLinkElite( int port )
+{
+	stick = new Joystick( port );
+}
+
+double InterLinkElite::getAile()
+{
+	return stick->GetRawAxis( aile ) / (aileTrim[2]-aileTrim[0]);
+}
+
+double InterLinkElite::getElev()
+{
+	return stick->GetRawAxis( elev ) / (elevTrim[2]-elevTrim[0]);
+}
+
+double InterLinkElite::getThro()
+{
+	return stick->GetRawAxis( thro ) / (throTrim[2]-throTrim[0]);
+}
+
+double InterLinkElite::getRudd()
+{
+	return stick->GetRawAxis( rudd ) / (ruddTrim[2]-ruddTrim[0]);
+}
+
+double InterLinkElite::getCh5()
+{
+	if( GetCh5() )
+		return 1;
+	return -1;
+}
+
+double InterLinkElite::getCh6()
+{
+	return stick->GetRawAxis( ch6 ) / (ch6Trim[2]-ch6Trim[0]);
+}
+
+double InterLinkElite::getCh7()
+{
+	if( GetCh7() )
+		return 1;
+	return -1;
+}
+
+double InterLinkElite::getCh8()
+{
+	if( GetCh8Front() )
+		return 1;
+	else if( GetCh8Back() )
+		return -1;
+	return 0;
+}
+
+double InterLinkElite::getReset()
+{
+	if( GetReset() )
+		return 1;
+	return -1;
+}
+
+bool InterLinkElite::GetReset()
+{
+	return stick->GetRawButton( reset );
+}
+
+bool InterLinkElite::GetCh5()
+{
+	return stick->GetRawButton( ch5 );
+}
+
+bool InterLinkElite::GetCh7()
+{
+	return stick->GetRawButton( ch7 );
+}
+
+bool InterLinkElite::GetCh8Back()
+{
+	return stick->GetRawButton( ch8b );
+}
+
+bool InterLinkElite::GetCh8Front()
+{
+	return stick->GetRawButton( ch8f );
+}
+
+InterLinkElite::~InterLinkElite() {}
+
diff --git a/src/InterLinkElite.h b/src/InterLinkElite.h
new file mode 100644
index 0000000..ccc0c1b
--- /dev/null
+++ b/src/InterLinkElite.h
@@ -0,0 +1,58 @@
+/*
+ * InterLinkElite.h
+ *
+ *  Created on: May 16, 2015
+ *      Author: user
+ */
+
+#ifndef SRC_INTERLINKELITE_H_
+#define SRC_INTERLINKELITE_H_
+
+#include "WPIlib.h"
+
+class InterLinkElite {
+public:
+	InterLinkElite( int port );
+	virtual ~InterLinkElite();
+
+	double getAile();
+	double getElev();
+	double getThro();
+	double getRudd();
+	double getCh5();
+	double getCh6();
+	double getCh7();
+	double getCh8();
+	double getReset();
+
+	bool GetReset();
+	bool GetCh5();
+	bool GetCh7();
+	bool GetCh8Back();
+	bool GetCh8Front();
+
+	// trims	{ min, centered, max }
+	double aileTrim[3] = { -0.648,	0.016,	0.654 };
+	double elevTrim[3] = {	0.606,	0.000, -0.641 };
+	double throTrim[3] = {	0.638,	0.018, -0.602 };
+	double ch6Trim[3]  = { -1.000,  0.000,  1.000 };
+	double ruddTrim[3] = { -0.641,	0.024,	0.732 };
+private:
+	Joystick *stick;
+
+	// axis mapping
+	int aile = 0;
+	int elev = 1;
+	int thro = 2;
+	int ch6 = 3;
+	int rudd = 4;
+
+	// button mapping
+	int ch5 = 1;
+	int ch7 = 2;
+	int reset = 3;
+	int ch8b = 4;
+	int ch8f = 5;
+};
+
+#endif /* SRC_INTERLINKELITE_H_ */
diff --git a/src/Robot.cpp b/src/Robot.cpp
index a682bb5..1ced474 100644
--- a/src/Robot.cpp
+++ b/src/Robot.cpp
@@ -1,4 +1,7 @@
 #include "WPILib.h"
+#include "InterLinkElite.h"
+
+#define PI 3.141592653589793238462643383279
 
 ///// USER PARAMETERS /////
 
@@ -11,23 +14,21 @@
 #define frontRightChannel	1
 #define backRightChannel	2
 
-#define InterLink_Elite_ID			0
+// adjust sign so that (+)*speed results in bot turning clockwise
+#define frontLeftSpeed	1.0
+#define backLeftSpeed	1.0
+#define frontRightSpeed	1.0
+#define backRightSpeed	1.0
+
+const double frontLeftAngle = PI/4;
+const double backLeftAngle = 3*PI/4;
+const double frontRightAngle = -PI/4;
+const double backRightAngle = -3*PI/4;
+
+#define InterLink_ID			0
 
 ///// END USER PARAMETERS /////
 
-// InterLink Elite Channel Mapping
-#define InterLink_Elite_AILE		0
-#define InterLink_Elite_ELEV		1
-#define InterLink_Elite_THRO		2
-#define	InterLink_Elite_CH6			3
-#define InterLink_Elite_RUDD		4
-
-#define InterLink_Elite_CH5			1
-#define InterLink_Elite_CH7			2
-#define InterLink_Elite_RESET		3
-#define InterLink_Elite_CH8_BACK	4
-#define InterLink_Elite_CH8_FRONT	5
-
 class Robot: public IterativeRobot
 {
 	LiveWindow *lw;
@@ -40,21 +41,23 @@ class Robot: public IterativeRobot
 	Talon *backRight;
 
 	// Input Devices
-	Joystick *InterLink;
+	InterLinkElite *InterLink;
+
 public:
-	Robot()
+
+	////////////////////////////////
+	///// ROBOT INITIALIZATION /////
+	////////////////////////////////
+
+	void RobotInit()
 	{
 		frontLeft = new Talon( frontLeftChannel );
 		backLeft = new Talon( backLeftChannel );
 		frontRight = new Talon( frontRightChannel );
 		backRight = new Talon( backRightChannel );
 
+		InterLink = new InterLinkElite( InterLink_ID );
 		ds = DriverStation::GetInstance();
-		InterLink = new Joystick( InterLink_Elite_ID );
-	}
-
-	void RobotInit()
-	{
 		lw = LiveWindow::GetInstance();
 	}
 
@@ -77,26 +80,19 @@ public:
 
 	void TeleopPeriodic()
 	{
-		// Write Buttons to SmartDashboard
-		SmartDashboard::PutBoolean( "CH5", InterLink->GetRawButton( InterLink_Elite_CH5 ) );
-		SmartDashboard::PutBoolean( "CH7", InterLink->GetRawButton( InterLink_Elite_CH7 ) );
-		SmartDashboard::PutBoolean( "RESET", InterLink->GetRawButton( InterLink_Elite_RESET ) );
-		if( InterLink->GetRawButton( InterLink_Elite_CH8_BACK ) )
-			SmartDashboard::PutNumber( "CH8", -1 );
-		else if( InterLink->GetRawButton( InterLink_Elite_CH8_FRONT ) )
-			SmartDashboard::PutNumber( "CH8", 1 );
-		else
-			SmartDashboard::PutNumber( "CH8", 0 );
+		double aile = InterLink->getAile();
+		double elev = InterLink->getElev();
 
-		// Write axes to SmartDashboard
-		SmartDashboard::PutNumber( "AILE", InterLink->GetRawAxis( InterLink_Elite_AILE ) );
-		SmartDashboard::PutNumber( "ELEV", InterLink->GetRawAxis( InterLink_Elite_ELEV ) );
-		SmartDashboard::PutNumber( "THRO", InterLink->GetRawAxis( InterLink_Elite_THRO ) );
-		SmartDashboard::PutNumber( "RUDD", InterLink->GetRawAxis( InterLink_Elite_RUDD ) );
-		SmartDashboard::PutNumber( "CH6", InterLink->GetRawAxis( InterLink_Elite_CH6 ) );
+		double driveAngle = atan2( -aile, -elev );
+		SmartDashboard::PutNumber( "Drive Angle", driveAngle );
 
-		// Write system into to SmartDashboard
-		SmartDashboard::PutNumber( "BATT", DriverStation::GetInstance()->GetBatteryVoltage() );
+		double driveSpeed = sqrt( aile*aile + elev*elev );
+		SmartDashboard::PutNumber( "Drive Speed", driveSpeed );
+
+		frontLeft->Set( (float)( frontLeftSpeed * driveSpeed * sin( frontLeftAngle-driveAngle ) ) );
+		backLeft->Set( (float)( backLeftSpeed * driveSpeed * sin( backLeftAngle-driveAngle ) ) );
+		frontRight->Set( (float)( frontRightSpeed * driveSpeed * sin( frontRightAngle-driveAngle ) ) );
+		backRight->Set( (float)( backRightSpeed * driveSpeed * sin( backRightAngle-driveAngle ) ) );
 	}
 
 	////////////////