package com.techhounds.commands.auton;

import com.techhounds.subsystems.DriveSubsystem;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.networktables.NetworkTable;

/**
 *
 */
public class VisionAlignToTarget extends Command implements PIDSource, PIDOutput {

	private DriveSubsystem drive;
	private double offAngle = 0;
	private PIDController pid;
	private NetworkTable netTable;
	
    public VisionAlignToTarget() {
        drive = DriveSubsystem.getInstance();
        requires(drive);
        
        pid = new PIDController(0, 0, 0, this, this);
        pid.setOutputRange(-1, 1);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	NetworkTable.setServerMode();
    	NetworkTable.initialize();
    	netTable = NetworkTable.getTable("SmartDashboard");
    	
    	if(netTable != null) {
    		offAngle = netTable.getNumber("OffCenterDegreesX", 0);
    	}
    	
    	pid.setSetpoint(offAngle);
    	pid.enable();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return pid.onTarget();
    }

    // Called once after isFinished returns true
    protected void end() {
    	pid.disable();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }

	@Override
	public void pidWrite(double output) {
		drive.setLeftPower(-output);
		drive.setRightPower(output);
	}

	@Override
	public void setPIDSourceType(PIDSourceType pidSource) {
		// TODO Auto-generated method stub
		
	}

	@Override
	public PIDSourceType getPIDSourceType() {
		// TODO Auto-generated method stub
		return null;
	}

	@Override
	public double pidGet() {
		return offAngle;
	}
	
	/*
	 * if angle is positive,
	 * we need to rotate to the left (counterclockwise)
	 * thus left power should be negative, right power should be positive
	 */
}

@@ -59,203 +59,31 @@
 nds.
-subsystems.DriveSubsystem;%0A%0Aimport edu.wpi.first.wpilibj.PIDController;%0Aimport edu.wpi.first.wpilibj.PIDOutput;%0Aimport edu.wpi.first.wpilibj.PIDSource;%0Aimport edu.wpi.first.wpilibj.PIDSourceType;
+commands.gyro.SetGyro;%0A
 %0Aimp
@@ -123,16 +123,21 @@
 .Command
+Group
 ;%0Aimport
@@ -251,74 +251,20 @@
 mand
- implements PIDSource, PIDOutput %7B%0A%0A%09private DriveSubsystem drive;
+Group %7B%0A    
 %0A%09pr
@@ -294,36 +294,8 @@
  0;%0A
-%09private PIDController pid;%0A
 %09pri
@@ -335,16 +335,17 @@
  public 
+ 
 VisionAl
@@ -368,1632 +368,323 @@
     
-    drive = DriveSubsystem.getInstance();%0A        requires(drive);%0A        %0A        pid = new PIDController(0, 0, 0, this, this);%0A        pid.setOutputRange(-1, 1);%0A    %7D%0A%0A    // Called just before this Command runs the first time%0A    protected void initialize() %7B%0A    %09NetworkTable.setServerMode();%0A    %09NetworkTable.initialize();%0A    %09netTable = NetworkTable.getTable(%22SmartDashboard%22);%0A    %09%0A    %09if(netTable != null) %7B%0A    %09%09offAngle = netTable.getNumber(%22OffCenterDegreesX%22, 0);%0A    %09%7D%0A    %09%0A    %09pid.setSetpoint(offAngle);%0A    %09pid.enable();%0A    %7D%0A%0A    // Called repeatedly when this Command is scheduled to run%0A    protected void execute() %7B%0A    %7D%0A%0A    // Make this return true when this Command no longer needs to run execute()%0A    protected boolean isFinished() %7B%0A        return pid.onTarget()
+%09%0A    %09NetworkTable.setServerMode();%0A    %09NetworkTable.initialize();%0A    %09netTable = NetworkTable.getTable(%22SmartDashboard%22);%0A    %09%0A    %09if(netTable != null) %7B%0A    %09%09offAngle = netTable.getNumber(%22OffCenterDegreesX%22, 0);%0A    %09%7D else %7B%0A    %09%09offAngle = 0
 ;%0A    
+%09
 %7D%0A
-%0A    // Called once after isFinished returns true%0A    protected void end() %7B%0A    %09pid.disable();%0A    %7D%0A%0A    // Called when another command which requires one or more of the same%0A    // subsystems is scheduled to run%0A    protected void interrupted() %7B%0A    %7D%0A%0A%09@Override%0A%09public void pidWrite(double output) %7B%0A%09%09drive.setLeftPower(-output);%0A%09%09drive.setRightPower(output);%0A%09%7D%0A%0A%09@Override%0A%09public void setPIDSourceType(PIDSourceType pidSource) %7B%0A%09%09// TODO Auto-generated method stub%0A%09%09%0A%09%7D%0A%0A%09@Override%0A%09public PIDSourceType getPIDSourceType() %7B%0A%09%09// TODO Auto-generated method stub%0A%09%09return null;%0A%09%7D%0A%0A%09@Override%0A%09public double pidGet() %7B%0A%09%09return offAngle;%0A%09%7D%0A%09%0A%09/*%0A%09 * if angle is positive,%0A%09 * we need to rotate to the left (counterclockwise)%0A%09 * thus left power should be negative, right power should be positive%0A%09 */
+    %09%0A    %09addSequential(new SetGyro(offAngle));%0A    %7D
 %0A%7D%0A

Convert VisionAlign Command to CommandGroup