From 82ba59546d558f6ebdc9dc367449837d91786880 Mon Sep 17 00:00:00 2001
From: Brendan <brendan.haines@gmail.com>
Date: Tue, 18 Aug 2015 21:58:14 -0600
Subject: [PATCH] adds untested buddy box support

---
 src/Robot.cpp | 31 ++++++++++++++++++++++++-------
 1 file changed, 24 insertions(+), 7 deletions(-)

diff --git a/src/Robot.cpp b/src/Robot.cpp
index 17ad7a9..55b24a5 100644
--- a/src/Robot.cpp
+++ b/src/Robot.cpp
@@ -28,7 +28,8 @@ const double backLeftAngle = 3*PI/4;
 const double frontRightAngle = -PI/4;
 const double backRightAngle = -3*PI/4;
 
-#define InterLink_ID			0
+#define Master_InterLink_ID			0
+#define Slave_InterLink_ID			1
 
 ///// END USER PARAMETERS /////
 
@@ -44,7 +45,9 @@ class Robot: public IterativeRobot
 	Talon *backRight;
 
 	// Input Devices
-	InterLinkElite *InterLink;
+	InterLinkElite *MasterInterLink;
+	InterLinkElite *SlaveInterLink;
+	InterLinkElite *ActiveInterLink;
 
 public:
 
@@ -59,7 +62,8 @@ public:
 		frontRight = new Talon( frontRightChannel );
 		backRight = new Talon( backRightChannel );
 
-		InterLink = new InterLinkElite( InterLink_ID );
+		MasterInterLink = new InterLinkElite( Master_InterLink_ID );
+		SlaveInterLink = new InterLinkElite( Slave_InterLink_ID );
 		ds = DriverStation::GetInstance();
 		lw = LiveWindow::GetInstance();
 	}
@@ -83,10 +87,23 @@ public:
 
 	void TeleopPeriodic()
 	{
-		double ch6 = InterLink->getCh6();
-		double aile = InterLink->getAile();
-		double elev = InterLink->getElev();
-		double rudd = InterLink->getRudd();
+		double throttle;
+
+		bool BuddyBoxEnabled = SmartDashboard::GetBoolean( "Buddy Box Enabled" , false);
+		bool SlaveInControl = MasterInterLink->GetCh5();
+		SmartDashboard::PutBoolean( "Slave In Control", SlaveInControl );
+		bool SlaveControlsSpeed = SmartDashboard::GetBoolean( "Slave Controls Speed", false );
+
+		if( BuddyBoxEnabled && SlaveInControl ) ActiveInterLink = SlaveInterLink;
+		else ActiveInterLink = MasterInterLink;
+
+		if( SlaveInControl && SlaveControlsSpeed ) throttle = SlaveInterLink->getCh6();
+		else throttle = MasterInterLink->getCh6();
+
+		double ch6 = ActiveInterLink->getCh6();
+		double aile = ActiveInterLink->getAile();
+		double elev = ActiveInterLink->getElev();
+		double rudd = ActiveInterLink->getRudd();
 		SmartDashboard::PutNumber( "Rudder", rudd );
 		SmartDashboard::PutNumber( "Throttle", ch6 );