package org.usfirst.frc.team7128.robot.subsystems;

import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import org.usfirst.frc.team7128.robot.Robot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/*
 * Incorporates the encoders, gyro sensor and the 4 motors attached to the chassis
 */

public class DriveTrain extends Subsystem {
	private SpeedController leftMotor
		= new SpeedControllerGroup(new Spark(0), new Spark(1)); //will have to change these, don't remember the ports
	private SpeedController rightMotor
		= new SpeedControllerGroup(new Spark(2), new Spark(3)); //see line 13 comments

	private DifferentialDrive drive
		= new DifferentialDrive(leftMotor, rightMotor);

	Encoder encRight = new Encoder(0,1,false,Encoder.EncodingType.k4X); //0 and 1 are the digital port numbers, false tells the encoder not to invert the direction, and k4X im not so sure about but like just use it and if doesn't work change it to 1X or 2X...something about counting edges...
	Encoder encLeft = new Encoder(2,3,true,Encoder.EncodingType.k4X); //maybe change port numbers??? maybe change reversedirection???
	Gyro gyro;

	public DriveTrain {
		super();

		if(RobotBase.isReal()) {
			encRight.setDistancePerPulse(18.849556/800);
			encLeft.setDistancePerPulse(18.849556/800);

		}
	}

}