Пример #1
0
  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);
    }
  }
Пример #2
0
 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) {
   }
 }