// RobotBuilder Version: 0.0.2
//
// 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 th future.
package org.usfirst.frc330.Beachbot2013Java.commands;
import edu.wpi.first.wpilibj.command.AutoSpreadsheetCommand;
import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc330.Beachbot2013Java.Robot;
/**
 *
 */
public class  TurnGyroAbs extends Command implements AutoSpreadsheetCommand {
    double angle;
    boolean stopAtEnd = false;
    
    public TurnGyroAbs(double angle) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        this(angle, 0, 15, false);
    }
    
    public TurnGyroAbs(double angle, double tolerance)
    {
        this(angle, tolerance, 15, false);
    
    }
    public TurnGyroAbs(double angle, double tolerance, double timeout, boolean stopAtEnd) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
	
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
        requires(Robot.chassis);
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
        this.angle = angle;
        Robot.chassis.gyroPIDLow.setAbsoluteTolerance(tolerance);
        setTimeout(timeout);
        this.stopAtEnd = stopAtEnd;
    }
    // Called just before this Command runs the first time
    protected void initialize() {
        Robot.chassis.leftDrivePIDLow.disable();
        Robot.chassis.rightDrivePIDLow.disable();  
        Robot.chassis.gyroPIDLow.setSetpoint(angle);
        Robot.chassis.gyroPIDLow.enable();
    }
    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        Robot.chassis.pidDrive();
    }
    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        if (Robot.chassis.gyroPIDLow.onTarget() || isTimedOut())
        {
            return true;            
        }
        else
        {
            return false;
        }
    }
    // Called once after isFinished returns true
    protected void end() {
        if (stopAtEnd)
        {
            Robot.chassis.gyroPIDLow.disable();
            Robot.chassis.tankDrive(0, 0);
        }
    }
    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
        stopAtEnd = true;
        end();
    }
    public void setParam1(double angle) {
        this.angle = angle;
    }
    public void setParam2(double tolerance) {
        Robot.chassis.gyroPIDLow.setAbsoluteTolerance(tolerance);
    }
    public void setParam3(double param3) {
    }
    public void setStopAtEnd(boolean stopAtEnd) {
        this.stopAtEnd = stopAtEnd;
    }
    public Command copy() {
        return new TurnGyroAbs(0);
    }
}

@@ -791,24 +791,35 @@
 double angle
+, tolerance
 ;%0A    boolea
@@ -1541,53 +1541,22 @@
     
-Robot.chassis.gyroPIDLow.setAbsoluteT
+this.t
 olerance
 (tol
@@ -1543,33 +1543,35 @@
   this.tolerance
-(
+ = 
 tolerance);%0A    
@@ -1559,25 +1559,24 @@
  = tolerance
-)
 ;%0A        se
@@ -1830,16 +1830,357 @@
 le();  %0A
+        Robot.chassis.leftDrivePIDHigh.disable();%0A        Robot.chassis.rightDrivePIDHigh.disable();%0A        Robot.chassis.gyroPIDLow.setAbsoluteTolerance(tolerance);%0A        Robot.chassis.gyroPIDHigh.setAbsoluteTolerance(tolerance);%0A        if (!Robot.chassis.getShiftState())%0A        %7B%0A            Robot.chassis.gyroPIDHigh.disable();%0A    
         
@@ -2220,24 +2220,28 @@
 int(angle);%0A
+    
         Robo
@@ -2267,24 +2267,221 @@
 w.enable();%0A
+        %7D%0A        else%0A        %7B%0A            Robot.chassis.gyroPIDLow.disable();%0A            Robot.chassis.gyroPIDHigh.setSetpoint(angle);%0A            Robot.chassis.gyroPIDHigh.enable();%0A        %7D%0A
     %7D%0A    //
@@ -2572,42 +2572,8 @@
 ) %7B%0A
-        Robot.chassis.pidDrive();%0A
     
@@ -2687,24 +2687,82 @@
 inished() %7B%0A
+        if (!Robot.chassis.getShiftState())%0A        %7B%0A    
         if (
@@ -2814,32 +2814,36 @@
 dOut())%0A        
+    
 %7B%0A            re
@@ -2836,24 +2836,28 @@
             
+ 
+   
 return true;
@@ -2873,38 +2873,110 @@
    %0A        
-%7D%0A        else
+    %7D%0A        %7D%0A        else if (Robot.chassis.gyroPIDHigh.onTarget() %7C%7C isTimedOut())
 %0A        %7B%0A 
@@ -2993,20 +2993,19 @@
  return 
-fals
+tru
 e;%0A     
@@ -3001,32 +3001,54 @@
 true;%0A        %7D%0A
+        return false;%0A
     %7D%0A    // Cal
@@ -3176,70 +3176,18 @@
 sis.
-gyroPIDLow.disable();%0A            Robot.chassis.tank
+stop
 Drive(
-0, 0
 );%0A 
@@ -3529,53 +3529,22 @@
     
-Robot.chassis.gyroPIDLow.setAbsoluteT
+this.t
 olerance
 (tol
@@ -3531,33 +3531,35 @@
   this.tolerance
-(
+ = 
 tolerance);%0A    
@@ -3547,25 +3547,24 @@
  = tolerance
-)
 ;%0A    %7D%0A    

use high and low gear gyro PID controllers