public boolean shiftDone() {
    double diff = two.pidGet();

    SmartDashboard.putNumber("diff", diff);
    SmartDashboard.putNumber("Setpoint", setPoint);
    System.out.println("In shiftDone(), diff: " + diff + "  Setpoint: " + setPoint);

    if (diff < setPoint) setSpeed(0.1);
    else setSpeed(-0.1);

    if (Math.abs(diff - setPoint) < 0.03) return true;

    return false;
  }
 public double getDiff() {
   return two.getDiff();
 }