AutoDriveStraightStage1.java

// RobotBuilder Version: 1.5
//
// 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.frc319.RecyleRush.commands;

import edu.wpi.first.wpilibj.command.PIDCommand;
import org.usfirst.frc319.RecyleRush.Robot;
import org.usfirst.frc319.RecyleRush.RobotMap;

/**
 *
 */
public class  AutoDriveStraightStage1 extends PIDCommand {
	
    public AutoDriveStraightStage1() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
        super("AutoDriveStraight", 1.0, 0.0, 0.0, 0.02);
        getPIDController().setContinuous(false);
        getPIDController().setAbsoluteTolerance(1.0);

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
        
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(Robot.driveTrain);
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
    }

    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:  
        // yourPot.getAverageVoltage() / kYourMaxVoltage;

        return Robot.driveTrain.getDegreesFromEncoderValues();
    }

    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);

        Robot.driveTrain.manualArcadeDrive(-0.57, output); //was originally -.9
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	
    	this.setSetpoint(0);
    	this.getPIDController().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() {
    	return Robot.elevator.isToteWithinRange(12.5);
    	/*double error = setpoint - returnPIDInput();
    	System.out.println("Error is "+ error);
        if(error < 5 && error > -5){
        	//System.out.println("isFinished");
        	return true;
        }
        else{
        	//System.out.println("notFinished");
        	return false;
        }*/
    }

    // Called once after isFinished returns true
    protected void end() {
    	
    	//possibly add 
    	//Robot.driveTrain.arcadeDrive(0,0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}