private static void paint(final Graphics2D g2d, final float size) {
final Path2D.Float path = new Path2D.Float();
for (float angle = 1f / 3f; angle <= 90f; angle += 1f) {
double angRad = Math.toRadians(angle);
double cos = Math.cos(angRad);
double sin = Math.sin(angRad);
path.reset();
path.moveTo(5f * cos, 5f * sin);
path.lineTo(size * cos, size * sin);
g2d.draw(path);
}
}
private void loop() {
speed = Math.min(speed + DELAY / 1000d / RAMP, targetPower);
if (Math.abs(angleError(targetAngle)) > Math.PI) System.out.println(angleError(targetAngle));
if (Math.abs(angleError(targetAngle)) > Math.PI / 2) {
steer.set(
Math.max(
-STEER_CAP, Math.min(STEER_CAP, STEER_GAIN * angleError(targetAngle - Math.PI))));
drive.set(-speed);
} else {
steer.set(Math.max(-STEER_CAP, Math.min(STEER_CAP, STEER_GAIN * angleError(targetAngle))));
drive.set(speed);
}
if (driveEncoder != null) {
double enc = driveEncoder.get();
double angle = getAngle() - gyro.getAngle() * Math.PI / 180 + Math.PI / 2;
x += (enc - encLast) * Math.cos(angle);
y += (enc - encLast) * Math.sin(angle);
encLast = enc;
}
try {
Thread.sleep(DELAY);
} catch (Exception e) {
}
}