package org.usfirst.frc.team2503.r2016;

import org.usfirst.frc.team2503.r2016.component.RhinoTrack;
import org.usfirst.frc.team2503.r2016.component.Winch;
import org.usfirst.frc.team2503.r2016.input.gamepad.Gamepad;
import org.usfirst.frc.team2503.r2016.input.gamepad.LogitechDualActionGamepad;
import org.usfirst.frc.team2503.r2016.input.gamepad.LogitechF310Gamepad;
import org.usfirst.frc.team2503.r2016.input.joystick.MadCatzV1Joystick;
import org.usfirst.frc.team2503.r2016.subsystem.DriveBase;
import org.usfirst.frc.team2503.r2016.subsystem.MainDriveBase;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Talon;

public class Robot extends IterativeRobot {
	
	public MadCatzV1Joystick leftJoystick;
	public MadCatzV1Joystick rightJoystick;
	
	public RhinoTrack leftTrack;
	public RhinoTrack rightTrack;
	
	public Encoder leftTrackEncoder;
	public Encoder rightTrackEncoder;
	
	public Winch winch;
	
	public DriveBase driveBase;
	
	public Gamepad gamepad;
	
	public Robot() {
		System.setProperty("java.awt.headless", "true");

		leftTrackEncoder = new Encoder(Constants.leftTrackEncoderAChannel, Constants.leftTrackEncoderBChannel);
		rightTrackEncoder = new Encoder(Constants.rightTrackEncoderAChannel, Constants.rightTrackEncoderBChannel);
		
		leftJoystick = new MadCatzV1Joystick(0);
		rightJoystick = new MadCatzV1Joystick(1);
		
		gamepad = new LogitechDualActionGamepad(2);
		
		leftTrack = new RhinoTrack(Constants.leftTrackSpeedController);
		rightTrack = new RhinoTrack(Constants.rightTrackSpeedController);

		driveBase = new MainDriveBase(leftTrack, rightTrack);
		
		winch = new Winch(Constants.winchSpeedController);
	}

	public void robotInit() {
	}

	public void disabledInit() {
	}

	public void disabledPeriodic() {
	}

	public void autonomousInit() {
	}

	public void autonomousPeriodic() {
	}

	public void teleopInit() {
	}

	public void teleopPeriodic() {
		double leftY = leftJoystick.y.get();
		
		System.out.println(leftY);
		
		winch.set(leftY);
	}

	public void testInit() {
	}

	public void testPeriodic() {
	}

}

@@ -721,17 +721,16 @@
 Robot %7B%0A
-%09
 %0A%09public
@@ -800,24 +800,48 @@
 tJoystick;%0A%09
+public Gamepad gamepad;%0A
 %0A%09public Rhi
@@ -890,17 +890,16 @@
 tTrack;%0A
-%09
 %0A%09public
@@ -960,17 +960,16 @@
 ncoder;%0A
-%09
 %0A%09public
@@ -982,18 +982,16 @@
  winch;%0A
-%09%0A
 %09public 
@@ -1015,36 +1015,8 @@
 se;%0A
-%09%0A%09public Gamepad gamepad;%0A%09
 %0A%09pu
@@ -1086,226 +1086,8 @@
 );%0A%0A
-%09%09leftTrackEncoder = new Encoder(Constants.leftTrackEncoderAChannel, Constants.leftTrackEncoderBChannel);%0A%09%09rightTrackEncoder = new Encoder(Constants.rightTrackEncoderAChannel, Constants.rightTrackEncoderBChannel);%0A%09%09%0A
 %09%09le
@@ -1169,18 +1169,16 @@
 ick(1);%0A
-%09%09
 %0A%09%09gamep
@@ -1216,18 +1216,16 @@
 pad(2);%0A
-%09%09
 %0A%09%09leftT
@@ -1358,64 +1358,221 @@
 %0A%0A%09%09
-driveBase = new MainDriveBase(leftTrack, rightTrack
+leftTrackEncoder = new Encoder(Constants.leftTrackEncoderAChannel, Constants.leftTrackEncoderBChannel);%0A%09%09rightTrackEncoder = new Encoder(Constants.rightTrackEncoderAChannel, Constants.rightTrackEncoderBChannel
 );%0A
-%09%09
 %0A%09%09w
@@ -1621,16 +1621,72 @@
 oller);%0A
+%09%09driveBase = new MainDriveBase(leftTrack, rightTrack);%0A
 %09%7D%0A%0A%09pub
@@ -1928,102 +1928,8 @@
 ) %7B%0A
-%09%09double leftY = leftJoystick.y.get();%0A%09%09%0A%09%09System.out.println(leftY);%0A%09%09%0A%09%09winch.set(leftY);%0A
 %09%7D%0A%0A

Clean up