Example #1
0
  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();
  }
Example #2
0
  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;
    }
  }
Example #3
0
  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;
    }
  }
Example #4
0
 public static boolean onTarget() {
   return controller.onTarget();
 }
Example #5
0
 // Make this return true when this Command no longer needs to run execute()
 protected boolean isFinished() {
   return pid.onTarget();
 }
Example #6
0
  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();
 }