// RobotBuilder Version: 1.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.frc330.commands;

import edu.wpi.first.wpilibj.command.BBCommand;

import org.usfirst.frc330.Robot;
import org.usfirst.frc330.constants.ChassisConst;
import org.usfirst.frc330.wpilibj.PIDGains;

/**
 *
 */
public class  TurnGyroAbs extends BBCommand {
    double angle, tolerance;
    boolean stopAtEnd = false;
    boolean enable = true;
    PIDGains highGains, lowGains, gains;
    
    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)
    {
        this(angle, tolerance, timeout, false);
    
    }
    
    public TurnGyroAbs(double angle, double tolerance, double timeout, boolean stopAtEnd) {
        this(angle, tolerance, timeout, stopAtEnd, true);
    }
    
    public TurnGyroAbs(double angle, double tolerance, double timeout, boolean stopAtEnd, boolean enable) {
    	this(angle, tolerance, timeout, stopAtEnd, enable, ChassisConst.GyroTurnLow, ChassisConst.GyroTurnHigh);
    }
    
    public TurnGyroAbs(double angle, double tolerance, double timeout, boolean stopAtEnd, boolean enable, PIDGains low, PIDGains high) {
                // 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;
        this.tolerance = tolerance;
        if (timeout >= 0.0)
        	setTimeout(timeout);
        this.stopAtEnd = stopAtEnd;
        this.enable = enable;
        lowGains = low;
        highGains = high;
    }
    // Called just before this Command runs the first time
    protected void initialize() {
        Robot.chassis.leftDrivePID.disable();
        Robot.chassis.rightDrivePID.disable();  
        
        if (!Robot.chassis.isHighGear())
        	gains = lowGains;
        else
        	gains = highGains;
        Robot.chassis.gyroPID.setPID(gains);
        Robot.chassis.gyroPID.setMaxOutput(gains.getMaxOutput());
        Robot.chassis.gyroPID.setAbsoluteTolerance(tolerance);
        Robot.chassis.gyroPID.setSetpoint(angle);
        Robot.logger.println("TurnGyroAbs Setpoint: " + angle);
        Robot.logger.println("Max output: " + gains.getMaxOutput());
        if (enable) 
            Robot.chassis.gyroPID.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() {
        if (Robot.chassis.gyroPID.onTarget() || isTimedOut())
        {
            return true;
        }
        return false;
    }
    // Called once after isFinished returns true
    protected void end() {
        if (stopAtEnd)
        {
            Robot.chassis.stopDrive();
        }
        Robot.logger.println("TurnGyroAbs Final Angle: " + Robot.chassis.getAngle() + "  Setpoint: " + angle, true);
    }
    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
        stopAtEnd = true;
        end();
    }
}

@@ -794,24 +794,64 @@
 e, tolerance
+, maxOutput, maxOutputStep, maxOutputMax
 ;%0D%0A    boole
@@ -2839,24 +2839,26 @@
 D(gains);%0D%0A 
+//
        Robot
@@ -2903,32 +2903,32 @@
 tMaxOutput());%0D%0A
-
         Robot.ch
@@ -3018,32 +3018,129 @@
 tpoint(angle);%0D%0A
+        maxOutputMax = gains.getMaxOutput();%0D%0A        maxOutputStep = gains.getMaxOutputStep();%0D%0A
         Robot.lo
@@ -3388,32 +3388,32 @@
 heduled to run%0D%0A
-
     protected vo
@@ -3424,24 +3424,249 @@
 xecute() %7B%0D%0A
+    %09maxOutput += maxOutputStep;%0D%0A    %09if (maxOutput %3E= maxOutputMax) %0D%0A    %09%09maxOutput = maxOutputMax;%0D%0A    %09Robot.chassis.leftDrivePID.setMaxOutput(maxOutput);%0D%0A        Robot.chassis.rightDrivePID.setMaxOutput(maxOutput);%0D%0A
     %7D%0D%0A    /

Add ramping