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(); }
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; } }
public static boolean onTarget() { return controller.onTarget(); }
// Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { return pid.onTarget(); }
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()); }
public boolean anglegood() { return pidLeft.onTarget() && pidRight.onTarget(); }