#include "WPILib.h"
#include "TankDrive.h"
#include "Shooter.h"
#include <ctime>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <sys/time.h>
#include <vector>
#include <cmath>

/**
 * 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, r2_drive,
		l1_drive, l2_drive,
		shooter1, shooter2;
	TankDrive drive;
	Shooter shooter;
	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), 	// right wheel 1
			r2_drive(2), 	// right wheel 2
			l1_drive(3), 	// left wheel 1
			l2_drive(4), 	// left wheel 2
			shooter1(10),  	// shooter drive 1
			shooter2(11),   // shooter drive 2
			drive(			// initialize TankDrive object.
					&l1_drive, &l2_drive, &r1_drive, &r2_drive),
			shooter(		// initialize Shooter object.
					&shooter1, &shooter2),
			rstick(0), 		// right stick (operator)
			lstick(1)		// left stick (driver)
	{

	}

	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);
		shooter.SetPower(power);

	}

};

START_ROBOT_CLASS(Robot)