Esempio n. 1
0
  private void initTurnPID() {
    turnPID =
        new SendablePIDController(
            turnKp,
            turnKi,
            turnKd,
            gyro,
            new PIDOutput() {

              public void pidWrite(double output) {
                tankDrive(-output, output);
                SmartDashboard.putDouble("left pwm", RobotMap.roundtoTwo(-output));
                SmartDashboard.putDouble("right pwm", RobotMap.roundtoTwo(output));
                SmartDashboard.putDouble("turn", RobotMap.roundtoTwo(gyro.getAngle()));
                SmartDashboard.putDouble("turn error", RobotMap.roundtoTwo(turnPID.getError()));
              }
            });
    turnPID.setTolerance(2.0);
    turnPID.setInputRange(-90, 90);
    turnPID.setOutputRange(-.55, .55);
    SmartDashboard.putData("turn PID", turnPID);
    // affects turning, .5 is default set in RobotDrive class.
    // drive.setSensitivity(.5);

  }
Esempio n. 2
0
  private SendablePIDController initDistancePID() {
    SendablePIDController pid =
        new SendablePIDController(
            distKp,
            distKi,
            distKd,
            ultrasonic,
            new PIDOutput() {

              public void pidWrite(double output) {
                // tankDrive(output, -output);
              }
            });
    pid.setTolerance(10.0);
    pid.setInputRange(6, 254);
    pid.setOutputRange(-.8, .8);
    SmartDashboard.putData("drive PID", pid);

    return pid;
  }