#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;
	CANTalon shooter1;
	CANTalon shooter2;
	TankDrive drive;

  Joystick rstick, lstick;

	// 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),
	   shooter1(10),
	   shooter2(11),
	   drive(&l1_drive, &l2_drive, &r1_drive, &r2_drive),
       rstick(0),
	   lstick(1)
 {

 }

 	 void DisabledInit()
 	 {
 		 r1_drive.SetSafetyEnabled(false);
 		 r2_drive.SetSafetyEnabled(false);
 		 l1_drive.SetSafetyEnabled(false);
 		 l2_drive.SetSafetyEnabled(false);
 		 shooter1.SetSafetyEnabled(false);
 		 shooter2.SetSafetyEnabled(false);
 	 }

 	 void TeleopInit()
 	 {
 		r1_drive.Enable();
 		r2_drive.Enable();
 		l1_drive.Enable();
 		l2_drive.Enable();



 	 }

 	 void TeleopPeriodic()
 	 {
 		 drive.Drive(&lstick);

			float power = (1.0 - rstick.GetThrottle()) / 2.0;
			shooter1.Set(power);
			shooter2.Set(power);



 	 }

};

START_ROBOT_CLASS(Robot)