#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 SampleRobot {
	CANTalon r1_drive, r2_drive, l1_drive, l2_drive;
	TankDrive 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(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}),
       m_stick(0)
 {}

	/**
	 * Runs the motor from the output of a Joystick.
	 */
	void OperatorControl() {
		while (IsOperatorControl() && IsEnabled()) {
			// Do Drive.
			drive.Drive(&m_stick);
			Wait(kUpdatePeriod); // Wait a bit so that the loop doesn't lock everything up.
		}
	}
};

START_ROBOT_CLASS(Robot)