protected void SetDriveDistance(int ticks, double seconds) {
Log.debug("Seting PID set point of= " + ticks + " currently at " + currentEncoderReading);
// drive.SetPIDSetPoint(ticks, seconds);
driveDirection.setHigh(ticks < currentEncoderReading);
driveEnable.setHigh(false);
ThreadUtil.wait((int) (seconds * 1000));
driveEnable.setHigh(true);
Log.debug("Arrived at= " + currentEncoderReading);
}
protected void SetDriveVelocity(int ticksPerSecond) {
Log.debug("Seting PID Velocity set point of=" + ticksPerSecond);
if (ticksPerSecond > 0) {
driveDirection.setHigh(ticksPerSecond > 0);
driveEnable.setHigh(false);
} else {
driveEnable.setHigh(true);
}
}