Permalink
Find file
Fetching contributors…
Cannot retrieve contributors at this time
96 lines (72 sloc) 2.87 KB
// 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() {
}
}