From 3d0cb6be079ebb955c32cc82a6116fe7072a8ca0 Mon Sep 17 00:00:00 2001
From: Jason Cox <jason@dublinschool.org>
Date: Thu, 28 Jan 2016 15:47:56 -0500
Subject: [PATCH] Insane TankDrive code.  NOT FUNCTIONAL

---
 DriveBase/src/Robot.cpp   | 56 ++++++++++++++++++++++++++++++---------
 DriveBase/src/TankDrive.h | 27 ++++++++++---------
 2 files changed, 57 insertions(+), 26 deletions(-)

diff --git a/DriveBase/src/Robot.cpp b/DriveBase/src/Robot.cpp
index 476f0d6..be21023 100644
--- a/DriveBase/src/Robot.cpp
+++ b/DriveBase/src/Robot.cpp
@@ -7,36 +7,66 @@
  *   controlled motor.
  *
  */
-class Robot : public SampleRobot {
-	CANTalon r1_drive, r2_drive, l1_drive, l2_drive;
-	TankDrive drive;
+class Robot : public IterativeRobot {
+	CANTalon r1_drive;
+	CANTalon r2_drive;
+	CANTalon l1_drive;
+	CANTalon l2_drive;
 
   Joystick m_stick;
 
 	// update every 0.01 seconds/10 milliseconds.
   // The talon only receives control packets every 10ms.
-	double kUpdatePeriod = 0.010;
+	//double kUpdatePeriod = 0.010;
 
 public:
  Robot()
-     : r1_drive(0),   // Initialize the Talon as device 1. Use the roboRIO web
-	   r2_drive(1)),             // interface to change the device number on the talons.
-	   l1_drive(2),
-	   l2_drive(3),
-	   drive(new CANTalon[] {&r1_drive,&r2_drive}, new CANTalon[] {&l1_drive, &l2_drive}),
+     : r1_drive(1),   // Initialize the Talon as device 1. Use the roboRIO web
+	   r2_drive(2),             // interface to change the device number on the talons.
+	   l1_drive(3),
+	   l2_drive(4),
        m_stick(0)
- {}
+ {
+	 printf("Initializing Robot...");
+	 printf("Initializing TankDrive");
+	 drive = new TankDrive(&l1_drive, &l2_drive, &r1_drive, &r2_drive);
+	 printf("Ready!");
+ }
 
+ 	 void DisabledInit()
+ 	 {
+
+ 	 }
+
+ 	 void TeleopInit()
+ 	 {
+ 		printf("Starting Teleop mode");
+ 		r1_drive.Enable();
+ 		r2_drive.Enable();
+ 		l1_drive.Enable();
+ 		l2_drive.Enable();
+
+ 	 }
+
+ 	 void TeleopPeriodic()
+ 	 {
+ 		 printf("Teleop Periodic Start.");
+ 		 drive->Drive(&m_stick);
+ 		 printf("~Teleop Periodic End.");
+ 	 }
 	/**
 	 * Runs the motor from the output of a Joystick.
-	 */
+	 *
 	void OperatorControl() {
 		while (IsOperatorControl() && IsEnabled()) {
 			// Do Drive.
-			drive.Drive(&m_stick);
+			std::cerr << "In Operator Control";
+			drive->Drive(&m_stick);
 			Wait(kUpdatePeriod); // Wait a bit so that the loop doesn't lock everything up.
 		}
-	}
+	}*/
+
+	TankDrive *drive;
 };
 
 START_ROBOT_CLASS(Robot)
diff --git a/DriveBase/src/TankDrive.h b/DriveBase/src/TankDrive.h
index be880b0..395a842 100644
--- a/DriveBase/src/TankDrive.h
+++ b/DriveBase/src/TankDrive.h
@@ -18,12 +18,14 @@
 class TankDrive {
 
 public:
-	TankDrive(CANTalon *left[], CANTalon *right[])
+	TankDrive(CANTalon *l1, CANTalon *l2, CANTalon* r1, CANTalon *r2)
 	{
-		Left = new CANTalon[] {left[0], left[1]};
-		Right = new CANTalon[] {right[0], right[1]};
+		this->l1 = l1;
+		this->l2 = l2;
+		this->r1 = r1;
+		this->r2 = r2;
 	}
-	virtual ~TankDrive() { delete Left[0]; delete Left[1]; delete Right[0]; delete Right[1]; }
+	virtual ~TankDrive() { delete l1; delete l2; delete r1; delete r2; }
 
 	void Drive(Joystick *js)
 	{
@@ -34,14 +36,14 @@ public:
 		// set deadzone
 		if(x > -DEADZONE_RADIUS && x < DEADZONE_RADIUS)
 		{
-			x = 0f;
+			x = 0;
 		}
 		if(y > -DEADZONE_RADIUS && y < DEADZONE_RADIUS)
 		{
-			y = 0f;
+			y = 0;
 		}
 
-		float left=0f, right=0f;
+		float left=0, right=0;
 
 		if(x == 0)
 		{
@@ -59,16 +61,15 @@ public:
 			left = th;
 		}
 
-		Left[0]->Set(left);
-		Left[1]->Set(left);
-		Right[0]->Set(right);
-		Right[1]->Set(right);
+		l1->Set(left);
+		l2->Set(left);
+		r1->Set(right);
+		r2->Set(right);
 	}
 
 private:
 
-	CANTalon* Left[];
-	CANTalon* Right[];
+	CANTalon *l1, *l2, *r1, *r2;
 };
 
 #endif /* SRC_TANKDRIVE_H_ */