/** * 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); }
/** * 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); }