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
 public void autoDrive() {
   double speed = 0;
   if (distPID.isEnable()) {
     speed = distPID.get();
   }
   double turn = 0;
   if (turnPID.isEnable()) {
     turn = turnPID.get();
   }
   if (speed > 0) {
     drive.drive(speed, turn);
   } else {
     if (turn > 0) {
       // turn right
       tankDrive(turn, -turn);
     } else if (turn < 0) {
       tankDrive(-turn, turn);
     }
   }
   SmartDashboard.putDouble("left pwm", leftJag.get());
   SmartDashboard.putDouble("right pwm", rightJag.get());
   SmartDashboard.putDouble("speed", RobotMap.roundtoTwo(speed));
   SmartDashboard.putDouble("turn", RobotMap.roundtoTwo(turn));
   SmartDashboard.putDouble("turn error", turnPID.getError());
   // System.out.println("driving " + speed + " " + turn);
 }
Esempio n. 3
0
 public void turnStart(double degrees) {
   turnPID.reset();
   gyro.reset();
   turnPID.setSetpoint(degrees);
   turnPID.enable();
   Timer.delay(.1);
   System.out.println(
       "start turning from " + RobotMap.roundtoTwo(gyro.getAngle()) + " to setpoint " + degrees);
 }
Esempio n. 4
0
 public boolean isTurnDone() {
   System.out.println(
       "is turn done "
           + turnPID.onTarget()
           + " deg="
           + RobotMap.roundtoTwo(gyro.getAngle())
           + " out="
           + RobotMap.roundtoTwo(turnPID.get())
           + " err="
           + RobotMap.roundtoTwo(turnPID.getError()));
   return turnPID.onTarget();
 }
Esempio n. 5
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;
  }
Esempio n. 6
0
 public boolean isDistDone() {
   return distPID.onTarget();
 }
Esempio n. 7
0
 public void setDistanceSetpoint(double inches) {
   distPID.setSetpoint(inches);
 }
Esempio n. 8
0
 public void distStop() {
   distPID.disable();
 }
Esempio n. 9
0
 public void distStart(double inches) {
   distPID.setSetpoint(inches);
   distPID.enable();
 }
Esempio n. 10
0
 public void setTurnSetpoint(double degrees) {
   turnPID.setSetpoint(degrees);
 }
Esempio n. 11
0
 public void turnStop() {
   turnPID.disable();
   System.out.println("stop turning at " + RobotMap.roundtoTwo(gyro.getAngle()));
 }
 public NetworkTable getTable() {
   return controller.getTable();
 }
 /**
  * Returns the setpoint.
  *
  * @return the setpoint
  */
 protected double getSetpoint() {
   return controller.getSetpoint();
 }
 /**
  * Sets the setpoint to the given value. If {@link PIDCommand#setRange(double, double)
  * setRange(...)} was called, then the given setpoint will be trimmed to fit within the range.
  *
  * @param setpoint the new setpoint
  */
 protected void setSetpoint(double setpoint) {
   controller.setSetpoint(setpoint);
 }
 void _end() {
   controller.disable();
 }
 void _initialize() {
   controller.enable();
 }