package org.usfirst.frc.team6657.robot;

import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;

public class Robot extends IterativeRobot {

	// New Talon objects are passed a CAN bus ID (the IDs set in the web config)
	WPI_TalonSRX motorFrontLeft = new WPI_TalonSRX(1); // Front left
	WPI_TalonSRX motorBackLeft = new WPI_TalonSRX(2); // Back left
	// Controller groups put motor controllers together for easy access and control
	SpeedControllerGroup controllersLeft = new SpeedControllerGroup(motorFrontLeft, motorBackLeft);

	WPI_TalonSRX motorFrontRight = new WPI_TalonSRX(3); // Front right
	WPI_TalonSRX motorBackRight = new WPI_TalonSRX(4); // Back right
	SpeedControllerGroup controllersRight = new SpeedControllerGroup(motorFrontRight, motorBackRight);

	// Create new drive instance using the controller groups
	DifferentialDrive drive = new DifferentialDrive(controllersLeft, controllersRight);

	Joystick joy = new Joystick(0); // Grab joystick on port 0

	// Executed every few ms
	@Override
	public void teleopPeriodic() {
		double speed = joy.getX(); // Gets X (forward and back)
		double rotation = joy.getTwist(); // Gets the value of the twist axis

		drive.arcadeDrive(speed, rotation); // Preset drive function
	}
}