@Override
public void timestampedDataReceived(
long curr_system_timestamp, long curr_sensor_timestamp, Object kind) {
if (timestamped && (kind.getClass() == AHRS.DeviceDataType.class)) {
double process_value;
switch (timestamped_src) {
case YAW:
process_value = navx_device.getYaw();
break;
case PITCH:
process_value = navx_device.getPitch();
break;
case ROLL:
process_value = navx_device.getRoll();
break;
case COMPASS_HEADING:
process_value = navx_device.getCompassHeading();
break;
case FUSED_HEADING:
process_value = navx_device.getFusedHeading();
break;
case LINEAR_ACCEL_X:
process_value = navx_device.getWorldLinearAccelX();
break;
case LINEAR_ACCEL_Y:
process_value = navx_device.getWorldLinearAccelY();
break;
case LINEAR_ACCEL_Z:
process_value = navx_device.getWorldLinearAccelZ();
break;
default:
process_value = 0.0;
break;
}
int num_missed_samples = 0; /* TODO */
last_system_timestamp = curr_system_timestamp;
last_sensor_timestamp = curr_sensor_timestamp;
double output = this.stepController(process_value, num_missed_samples);
synchronized (sync_event) {
sync_event.notify();
}
}
}
@Override
public void untimestampedDataReceived(long curr_system_timestamp, Object kind) {
if (!timestamped && (kind.getClass() == AHRS.DeviceDataType.class)) {
double process_value;
switch (untimestamped_src) {
case RAW_GYRO_X:
process_value = navx_device.getRawGyroX();
break;
case RAW_GYRO_Y:
process_value = navx_device.getRawGyroY();
break;
case RAW_GYRO_Z:
process_value = navx_device.getRawGyroZ();
break;
case RAW_ACCEL_X:
process_value = navx_device.getRawAccelX();
break;
case RAW_ACCEL_Y:
process_value = navx_device.getRawAccelY();
break;
case RAW_MAG_X:
process_value = navx_device.getRawMagX();
break;
case RAW_MAG_Y:
process_value = navx_device.getRawMagY();
break;
case RAW_MAG_Z:
process_value = navx_device.getRawMagZ();
break;
default:
process_value = 0.0;
break;
}
int num_missed_samples = 0; /* TODO */
last_system_timestamp = curr_system_timestamp;
double output = this.stepController(process_value, num_missed_samples);
synchronized (sync_event) {
sync_event.notify();
}
}
}
/**
* This navXPIDController constructor is used when the PID Controller is to be driven by a
* navX-Model device input data source which is accompanied by a high-accuracy "sensor" timestamp.
*
* <p>The data source specified automatically determines the navXPIDController's input data range.
*/
public navXPIDController(AHRS navx_device, navXTimestampedDataSource src) {
this.navx_device = navx_device;
this.timestamped = true;
this.timestamped_src = src;
switch (src) {
case YAW:
this.setInputRange(-180.0, 180.0);
break;
case PITCH:
case ROLL:
this.setInputRange(-90.0, 90.0);
break;
case COMPASS_HEADING:
case FUSED_HEADING:
this.setInputRange(0.0, 360.0);
break;
case LINEAR_ACCEL_X:
case LINEAR_ACCEL_Y:
case LINEAR_ACCEL_Z:
this.setInputRange(-2.0, 2.0);
break;
}
navx_device.registerCallback(this);
}
public void close() {
enable(false);
navx_device.deregisterCallback(this);
}
/**
* This navXPIDController constructor is used when the PID Controller is to be driven by a
* navX-Model device input data source which is not accompanied by a high-accuracy "sensor"
* timestamp.
*
* <p>The data source specified automatically determines the navXPIDController's input data range.
*/
public navXPIDController(AHRS navx_device, navXUntimestampedDataSource src) {
this.navx_device = navx_device;
this.timestamped = false;
this.untimestamped_src = src;
navx_device.registerCallback(this);
}
