// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.


package org.usfirst.frc1073.robot17.commands;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import org.usfirst.frc1073.robot17.Robot;

/**
 *
 */
public class DriveToBoiler extends Command {

    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
 
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
	//Set up for the NetworkTable variables the Raspberry Pi sends
		NetworkTable netTable;
	    double xDelta;
	    double xWidth;
	    
	    
	    //Variable for button used in isFinished
	    boolean isPressed = false;
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
    public DriveToBoiler() {

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING

        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
        requires(Robot.driveTrain);

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
        netTable = NetworkTable.getTable("VisionTable");
    }

protected void initialize() {
    	
    	SmartDashboard.putString("done?", "no");

    	Robot.bling.sendBoilerTargeting();
    }

    protected void execute() {
    	
    	//These are the variables that get manipulated in the code
        double forwardsRight = 0;
        double forwardsLeft = 0;
        double changeRight = 0;
        double changeLeft = 0;
        double finalRight = 0;
        double finalLeft = 0;
       
        //These are the variables for speed - start slow
        double driveMultiplier = 2;
        double changeMultiplier = 3.55;
        double driveSpeed = 0.3;
		double changeSpeed = 0.1;
		double slowWidth = 25;
		double side = 10;
		
		
		//This is the width of the Pixy
		double imageWidth = 320;
		
		//These are what the Pixy send us
		xDelta =  netTable.getNumber("centerDistHG", 0);
        xWidth =  netTable.getNumber("AverageWidthHG", 0);
		
		//This code modifies the speed based on how close you are to the peg
        if (xWidth > slowWidth) {
        	if (xWidth > slowWidth + 1) {
        		if (xWidth > slowWidth + 2) {
        			if (xWidth > slowWidth + 3) {
        				if (xWidth > slowWidth + 4) {
        					if (xWidth > slowWidth + 5) {
        						if (xWidth > slowWidth + 6) {
        							forwardsRight = driveSpeed + .03;
        		                	forwardsLeft = driveSpeed + .03;
        		                	changeRight = changeSpeed - .025;
        		                	changeLeft = changeSpeed - .025;
        						}
        						else{
        							forwardsRight = driveSpeed + .02;
        		                	forwardsLeft = driveSpeed + .02;
        		                	changeRight = changeSpeed - .025;
        		                	changeLeft = changeSpeed - .025;
        						}
        							}
        						else{
        		                	forwardsRight = driveSpeed + .035;
        		                	forwardsLeft = driveSpeed + .035;
        		                	changeRight = changeSpeed - .025;
        		                	changeLeft = changeSpeed - .025;
        		        		}}
        					else{
        	                	forwardsRight = driveSpeed + .05;
        	                	forwardsLeft = driveSpeed + .05;
        	                	
        	        		}}
        				else{
                        	forwardsRight = driveSpeed + .075;
                        	forwardsLeft = driveSpeed + .075;
                		}}
        			else{
                    	forwardsRight = driveSpeed + .1;
                    	forwardsLeft = driveSpeed + .1;
            		}}
        		else{
                	forwardsRight = driveSpeed + .15;
                	forwardsLeft = driveSpeed + .15;
        		}}
        	else{
            	forwardsRight = driveSpeed + .2;
            	forwardsLeft = driveSpeed + .2;
        	}
        
        if ( Math.abs(xDelta) < side + 10){
        	if (Math.abs(xDelta) < side + 20){
        		changeRight = changeSpeed + .1;
            	changeLeft = changeSpeed + .1;
        	}
        	else if (Math.abs(xDelta) < side + 30){
        		changeRight = changeSpeed + .2;
            	changeLeft = changeSpeed + .2;
        	}
        	else {
        		changeRight = changeSpeed + .05;
            	changeLeft = changeSpeed + .05;
        	}
        }
        else {
    		changeRight = changeSpeed;
        	changeLeft = changeSpeed;
        }
        
        //This code handles the left and right vs forward motion of the robot
        //based on the Pixy's values
        if (xDelta > side) {
        	finalRight = forwardsRight - changeRight;
        	finalLeft = forwardsLeft + changeLeft;
        	SmartDashboard.putString("Peg Direction", "Left");
        }
        else if (xDelta < -side) {
        	finalRight = forwardsRight + changeRight;
        	finalLeft = forwardsLeft - changeLeft;
        	SmartDashboard.putString("Peg Direction", "Right");
        }
        else {
        	finalRight = forwardsRight;
        	finalLeft = forwardsLeft;
        	SmartDashboard.putString("Peg Direction", "Center");
        }
        
//        //Makes it so the drive command cannot send a lower value than .1
//        /*
//          Resolves problem that GearToPeg wasn't able to overcome friction 
//          when close to peg 
//        */
//        if(finalRight <= .075) finalRight = .075;
//        if(finalLeft <= .075) finalLeft = .075;
        //This sends the final numbers to the drivetrain
        Robot.driveTrain.basicDrive(-finalLeft, -finalRight);
       
    }
    
    protected boolean isFinished() {
    	
    	//Checks the cancel button for its state
    	isPressed = Robot.oi.cancelAny.get();
    	
    	//Stops the robot if:
    	/*
    	 * Change this to affect how close the robot gets. Bigger means closer and smaller means farther.
    	 * 27 should be ok to start.
    	 * 			|
    	 * 			|
    	 * 			|
    	 *			|
    	 *		   \ /
    	 */
    	if (xWidth > 45 || isPressed) {
    		Robot.driveTrain.basicDrive(0, 0);
    		Robot.oi.driverControl.rumbleTimeRep(1, 150, 2);
    		SmartDashboard.putString("done?", "yes");
    		return true;
    	}
    	else {
    		return false;
    	}
    	
    }
    
    protected void end() {
    	//Stops motors and sets bling
    	Robot.driveTrain.basicDrive(0, 0);
    	Robot.bling.sendRemoveGear();
    }

    protected void interrupted() {
    	//Stops motors and sets bling
    	Robot.driveTrain.basicDrive(0, 0);
    	Robot.bling.sendRemoveGear();
    }
}

@@ -6498,10 +6498,10 @@
 %09 * 
-27
+40
  sho

comment fix