#include "WPILib.h"
#include "TankDrive.h"

/**
 * This sample shows how to use the new CANTalon to just run a motor in a basic
 *   throttle mode, in the same manner as you might control a traditional PWM
 *   controlled motor.
 *
 */
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;

public:
 Robot()
     : 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.
			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)