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();
}