@@ -0,0 +1,2247 @@
+package com.techhounds.commands.auton;%0A%0Aimport com.techhounds.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;%0Aimport edu.wpi.first.wpilibj.command.Command;%0Aimport edu.wpi.first.wpilibj.networktables.NetworkTable;%0A%0A/**%0A *%0A */%0Apublic class VisionAlignToTarget extends Command implements PIDSource, PIDOutput %7B%0A%0A%09private DriveSubsystem drive;%0A%09private double offAngle = 0;%0A%09private PIDController pid;%0A%09private NetworkTable netTable;%0A%09%0A    public VisionAlignToTarget() %7B%0A        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();%0A    %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 */%0A%7D%0A

Add command to align robot based on camera angle