// Called just before this Command runs the first time
protected void initialize() {
Robot.driveTrain.reset();
// Make input infinite
leftPID.setContinuous();
rightPID.setContinuous();
// Set output speed range
if (speed > 0) {
leftPID.setOutputRange(-speed, speed);
rightPID.setOutputRange(-speed, speed);
} else {
leftPID.setOutputRange(speed, -speed);
rightPID.setOutputRange(speed, -speed);
}
Robot.driveTrain.initGyro = Robot.driveTrain.getHeading();
leftPID.setSetpoint(distance);
rightPID.setSetpoint(distance);
// Will accept within 5 inch of target
leftPID.setAbsoluteTolerance(5.0 / 12);
rightPID.setAbsoluteTolerance(5.0 / 12);
// Start going to location
leftPID.enable();
// rightPID.enable();
}
public void autonomousInit() {
try {
robot.arm.moveTo(PegPosition.RESET);
} catch (CANTimeoutException e1) {
// TODO Auto-generated catch block
e1.printStackTrace();
}
robot.compressor.start();
Timer.delay(3.5);
robot.grabber.tiltDown();
Timer.delay(2);
robot.grabber.grab();
robot.lineSensor.setLinePreference(LinePreference.LEFT);
controller.enable();
Timer.delay(2.5);
robot.grabber.tiltUp();
Timer.delay(2.5);
try {
robot.arm.moveTo(PegPosition.MIDDLE_OFFSET);
} catch (CANTimeoutException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
public boolean elevatePIDTuning(double height, double speed) {
elevatorPID.setOutputRange(-0.5, speed);
elevatorPID.setSetpoint(height);
elevatorPID.enable();
if (elevatorPID.onTarget()) {
elevatorPID.disable();
robot.elevator.elevatorVics.set(0);
return true;
} else {
return false;
}
}
public boolean elevatePID(double height) {
elevatorPID.setOutputRange(-1, 1);
elevatorPID.setSetpoint(-height);
elevatorPID.enable();
if (elevatorPID.onTarget()) {
elevatorPID.disable();
robot.elevator.elevatorSingle.set(0);
return true;
} else {
return false;
}
}
// Initialize your subsystem here
public LoaderClimberOld2PID() {
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
// super("LoaderClimberOld2PID", 1.5, 0.1, 0.0);
// setAbsoluteTolerance(0.02);
// getPIDController().setContinuous(false);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
m_preferences = Preferences.getInstance();
if (leftJag != null) { // for practice robot
// Fix for motors going in opposite directios
PIDOutput rightOutput =
new PIDOutput() {
public void pidWrite(double output) {
rightJag.pidWrite(-1 * output);
}
};
pidLeft = new PIDController(P, I, D, leftAngleCh, leftJag); // PID Output Interface
pidRight = new PIDController(P, I, D, rightAngleCh, rightOutput); // PID Output Interface*/
LiveWindow.addActuator("LoaderClimber", "PIDLeft Angle Controller", pidLeft);
LiveWindow.addActuator("LoaderClimber", "PIDLeft Angle Controller", pidRight);
pidLeft.setSetpoint(m_preferences.getFloat("Retract", 2.4f));
pidRight.setSetpoint(m_preferences.getFloat("RetractR", 2.4f));
pidLeft.setInputRange(0, 4);
pidRight.setInputRange(0, 4);
pidLeft.setPercentTolerance(PID_TOL);
pidRight.setPercentTolerance(PID_TOL);
pidLeft.enable(); // - Enables the PID controller.
pidRight.enable(); // - Enables the PID controller.
// disable();
}
}
public void teleopPeriodic() {
armpid.setPID(
SmartDashboard.getNumber("kP"),
SmartDashboard.getNumber("kI"),
SmartDashboard.getNumber("kD"));
if (joy1.getRawButton(1)) {
armpid.enable();
} else {
armpid.disable();
arm.set(0);
}
SmartDashboard.putBoolean("Checkbox 2", limit.get());
System.out.println("limit " + limit.get());
}
public boolean elevatePIDLock(double height) {
if (robot.elevator.locked) {
robot.elevator.elevatorAutoRelease();
}
elevatorPID.setOutputRange(-1, 1);
elevatorPID.setSetpoint(height);
elevatorPID.enable();
if (elevatorPID.onTarget()) {
elevatorPID.disable();
robot.elevator.elevatorVics.set(0);
if (height > 0) {
robot.elevator.elevatorLock();
}
}
return elevatorPID.onTarget();
}
protected void initialize() {
// Use standard mecanum drive
drivebase.initMecanum(false);
/*
* Enable drivebase rotational PID
*/
// Set target angle
m_pc.setSetpoint(m_target);
// Throttle down the max allowed speed
m_pc.setOutputRange(-0.5, 0.5);
// Set tolerance around the target angle
m_pc.setAbsoluteTolerance(5);
// Reset and enable PID controller
m_pc.reset();
m_pc.enable();
}
public static void pidEnable() {
controller.enable();
}
// Called just before this Command runs the first time
protected void initialize() {
// Get everything in a safe starting state.
Robot.drivetrain.reset();
pid.reset();
pid.enable();
}
public void teleopPeriodic() {
/*SmartDashboard.getNumber("Accel x: ", accel.getX());
SmartDashboard.getNumber("Accel Y: ", accel.getY());
SmartDashboard.getNumber("Accel Z: ", accel.getZ());*/
if (controller.isEnabled()) {
if (((Timer.getFPGATimestamp() - timerStart) > timeToCancel) || controller.onTarget()) {
controller.disable();
timerStart = -1;
timeToCancel = -1;
}
}
if (j1.getRawButton(2)) {
enc.reset();
controller.setSetpoint(120); // destination 24 inches -> NO!! Trying to figure out this value
timerStart = Timer.getFPGATimestamp();
timeToCancel = 10; // timeout after 10 seconds
controller.enable();
} else if (j1.getRawButton(
1)) { // this button stops the robot if the button 2 command goes crazy
controller.disable();
timerStart = -1;
timeToCancel = -1;
} else { // if time out or distance, end
controller.disable();
timerStart = -1;
timeToCancel = -1;
}
if (!controller.isEnabled()) {
rd.arcadeDrive(-j1.getY(), -j1.getX()); // normal arcade drive
}
if (j2.getRawAxis(2) != 0) { // set shooter values to the left trigger value
shooter.set(-j2.getRawAxis(2));
shooter2.set(-j2.getRawAxis(2));
SmartDashboard.putNumber("Shooter: ", shooter.get());
} else { // stop shooter
shooter.set(0);
shooter2.set(0);
}
/*if(j2.getRawButton(8)){ //runs intake, waits, runs shooter
shooter.set(-1);
intake.set(.5);
Timer.delay(.50);
intake.set(0);
Timer.delay(1.5);
intake.set(-1);
Timer.delay(1);
shooter.set(0);
intake.set(0);
}*/
// Need to mess around with sensitivity of light sensor
if (j2.getRawAxis(3) != 0 && !light.get()) { // run intake at speed of right trigger
intake.set(-1 * j2.getRawAxis(3));
} else if (j2.getRawButton(5) && !light.get()) { // run intake into robot
intake.set(-1);
} else if (j2.getRawButton(6)) { // run intake out of robot
intake.set(1);
} else { // stop intake
intake.set(0);
}
if (j2.getRawButton(2)) { // lift intake
inup.set(DoubleSolenoid.Value.kForward);
inup2.set(DoubleSolenoid.Value.kForward);
} else if (j2.getRawButton(3)) { // drop intake
inup.set(DoubleSolenoid.Value.kReverse);
inup2.set(DoubleSolenoid.Value.kReverse);
} else { // solenoids off
inup.set(DoubleSolenoid.Value.kOff);
inup2.set(DoubleSolenoid.Value.kOff);
}
// reading values
SmartDashboard.putNumber("Encoder Dist: ", enc.getDistance()); // Distance is in inches
SmartDashboard.putNumber("Encoder: ", enc.get());
SmartDashboard.putNumber("Encoder Rate: ", enc.getRate());
SmartDashboard.putNumber("Gyro: ", gyro.getAngle());
SmartDashboard.putNumber("Encoder Raw", enc.getRaw());
SmartDashboard.putNumber("Controller: ", controller.getSetpoint());
SmartDashboard.putBoolean("Light Sensor: ", light.get());
}
protected void initialize() {
pidControllerDistance.enable();
pidControllerDistance.setSetpoint(0);
drivetrainStrafe.mecanumPolarStrafe(FORWARD_SPEED, 0);
timer.start();
}
public void startBalancing() {
enabled = true;
controller.setSetpoint(0.0);
controller.enable();
}
public void turnToHeading(double heading) {
ahrs.zeroYaw();
turnController.setSetpoint(heading);
turnController.enable();
SmartDashboard.putNumber("Turn Requests", turnRequests++);
}
