package org.robockets.stronghold.robot.highgoalshooter;

import org.robockets.stronghold.robot.Robot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * Align the robot horizontally with the target.
 */
public class HorizontalAlign extends Command {

	NetworkTable table;
	boolean continuous;
	
	/**
	 * * @param continuos		If it should stop when on target.
	 */
    public HorizontalAlign(boolean continuous) {
    	requires(Robot.shooter);
    	this.continuous = continuous;
    }

    protected void initialize() {
    	table = NetworkTable.getTable("vision");
    }

    protected void execute() {
    	double pixelError = table.getNumber("horiz_offset", 10);
    	SmartDashboard.putNumber("factorz", SmartDashboard.getNumber("factorz", 0.02));
		double factor = SmartDashboard.getNumber("factorz", 0.02); // Or something.
    	
		SmartDashboard.putNumber("Setpoint delta", factor * pixelError);
		Robot.shooter.setTurnTableAngle(Robot.shooter.turnTableSource.pidGet() + (factor * pixelError));
    }

    protected boolean isFinished() {
    	if (continuous) return Robot.shooter.turnTablePidController.onTarget();
    	return false;
    }

    protected void end() {
    }

    protected void interrupted() {
    	end();
    }
}

@@ -776,9 +776,8 @@
 t%22, 
-1
 0);%0A
@@ -1012,16 +1012,63 @@
 Error);%0A
+%09%09if (table.getBoolean(%22heartbeat%22, false)) %7B%0A%09
 %09%09Robot.
@@ -1158,16 +1158,61 @@
 rror));%0A
+%09%09%09table.putBoolean(%22heartbeat%22, false);%0A%09%09%7D%0A
     %7D%0A%0A 

Add heartbeat implementation to horizontal align.