protected void execute() {
    double delta = timer.get();
    distance += (drivetrain.getAverageForwardVelocity() * delta);
    timer.reset();

    // PID tuning code
    pidControllerDistance.setPID(
        pidConfigDistance.getP(P),
        pidConfigDistance.getI(I),
        pidConfigDistance.getD(D),
        pidConfigDistance.getF(F));
    pidConfigDistance.setSetpoint(pidControllerDistance.getSetpoint());
    pidConfigDistance.setValue(drivetrainRotation.pidGet());
  }
Exemple #2
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());
  }