package org.robockets.stronghold.robot.drivetrain;

import org.robockets.stronghold.robot.CompassPIDSource;
import org.robockets.stronghold.robot.DummyPIDOutput;
import org.robockets.stronghold.robot.GyroPIDSource;
import org.robockets.stronghold.robot.RobotMap;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
 *
 */
public class Drivetrain extends Subsystem {
	public final PIDController compassPID;
	public final PIDController gyroPID;
	
	public Drivetrain() {
		compassPID = new PIDController(0.1, 0, 0, new CompassPIDSource(), new DummyPIDOutput());
		gyroPID = new PIDController(0.01, 0.0001, 0.00001, new GyroPIDSource(), new DummyPIDOutput());
		
		compassPID.disable();
		compassPID.setOutputRange(-1.0, 1.0); // Set turning speed range
		compassPID.setPercentTolerance(5.0); // Set tolerance of 5%
		
		gyroPID.disable();
		gyroPID.setOutputRange(-1.0, 1.0); // Set turning speed range
		gyroPID.setPercentTolerance(5.0); // Set tolerance of 5%
	}
	
    public void initDefaultCommand() {

    }
    
    public void driveTank(double leftValue, double rightValue) {
    	RobotMap.robotDrive.tankDrive(leftValue, rightValue);
    }
    
    public void driveArcade(double moveValue, double rotateValue) {
    	RobotMap.robotDrive.arcadeDrive(moveValue, rotateValue);
    }
    
    /*
     * Drive with PID. First must enable the correct PID
     */
    public void driveAssisted(double moveValue, double angle, boolean compassAssist) {
    	if (compassAssist) { // Use compass for PID
    		driveArcade(moveValue, compassPID.get());
    	} else {
    		driveArcade(0, -gyroPID.get());
    	}
    }
    
    public void setAngle(double angle, boolean compassAssist) {
    	if (compassAssist) {
    		compassPID.setSetpoint(angle);
    	} else {
    		gyroPID.setSetpoint(angle);
    	}
    }
    
    public void stop() {
    	driveTank(0, 0);
    }
    
    public void enableCompassPID() {
    	gyroPID.disable();
    	
    	compassPID.enable();
    	compassPID.reset();
    	compassPID.setSetpoint(RobotMap.navX.getCompassHeading()); // Make sure setpoint starts as current position
    }
    
    public void enableGyroPID(){
    	compassPID.disable();
    	
    	gyroPID.enable();
    	gyroPID.reset();
    	gyroPID.setSetpoint(RobotMap.navX.getYaw());
    }
}


@@ -155,16 +155,72 @@
 Output;%0A
+import org.robockets.stronghold.robot.EncoderPIDSource;%0A
 import o
@@ -256,32 +256,32 @@
 .GyroPIDSource;%0A
-
 import org.roboc
@@ -538,16 +538,56 @@
 yroPID;%0A
+%09public final PIDController encoderPID;%0A
 %09%0A%09publi
@@ -791,16 +791,105 @@
 put());%0A
+%09%09encoderPID = new PIDController(0, 0, 0, new EncoderPIDSource(), new DummyPIDOutput());%0A
 %09%09%0A%09%09com
@@ -1124,16 +1124,16 @@
 d range%0A
-
 %09%09gyroPI
@@ -1183,16 +1183,172 @@
 e of 5%25%0A
+%09%09%0A%09%09encoderPID.disable();%0A%09%09encoderPID.setOutputRange(-1.0, 1.0); // Set turning speed range%0A%09%09encoderPID.setPercentTolerance(5.0); // Set tolerance of 5%25%0A
 %09%7D%0A%09%0A   
@@ -2657,11 +2657,168 @@
 ;%0A    %7D%0A
+    %0A    public void enableEncoderPID() %7B%0A    %09encoderPID.enable();%0A    %09encoderPID.reset();%0A    %09encoderPID.setSetpoint(RobotMap.driveEncoder.get());%0A    %7D%0A
 %7D%0A%0A

Add encoder PID.