public void setSpeed(OutputStream out, byte[] buf, int len) {
    int leftSign = 1;
    int rightSign = 1;
    int p = 2;

    if (buf[p] == '-') { // if input is '-', move right one place
      leftSign = -1;
      p++;
    }
    int leftPwr = 0;
    while ('0' <= buf[p] && buf[p] <= '9')
      leftPwr = leftPwr * 10 + (buf[p++] - '0'); // accumulate the decimal value
    if (leftPwr > H_MAX_POWER) leftPwr = H_MAX_POWER; // stay in range
    leftPwr = leftSign * leftPwr;

    // assert(buf[p] == ',');
    p++;

    if (buf[p] == '-') {
      rightSign = -1;
      p++;
    }
    int rightPwr = 0;
    while ('0' <= buf[p] && buf[p] <= '9') rightPwr = rightPwr * 10 + (buf[p++] - '0');
    if (rightPwr > H_MAX_POWER) rightPwr = H_MAX_POWER; // stay in range
    rightPwr = rightSign * rightPwr;

    // assert(buf[p] == 0); // the buffer is null-terminated
    p++;

    leftMotor.setPower((leftPwr * 16) / H_MAX_POWER);
    rightMotor.setPower((rightPwr * 16) / H_MAX_POWER);
    writeCRLF(out, "d");
  }
  public void move() {
    if (last_move_time == null || last_move_time.doubleValue() < world.time) {
      last_move_time = new Double(world.time);
      double max_dist, dist_right, dist_left, theta, x, y, dist_diff;
      double delta_theta, turn_radius, new_theta, new_x, new_y;
      Location location;
      Orientation orientation;

      orientation = orientation();
      location = location();

      max_dist = max_speed / world.ticks_per_second;
      dist_right = right_motor.output() * max_dist;
      dist_left = left_motor.output() * max_dist;
      theta = orientation.theta;
      x = location.x;
      y = location.y;
      old_location.x = x;
      old_location.y = y;
      dist_diff = dist_right - dist_left;

      //			System.out.println("dist_diff: " + dist_diff);

      delta_theta = dist_diff / wheel_base;
      if (Math.abs(dist_diff) < .0001) {
        turn_radius = 0.0;
      } else {
        turn_radius = (dist_right / delta_theta) - (wheel_base / 2);
      }

      //			System.out.println("turn_radius: " + turn_radius);

      new_theta = theta + delta_theta;
      if (turn_radius == 0.0) {

        //				System.out.println("turn_radius == 0");

        new_x = x + Math.cos(theta) * dist_left;
        new_y = y + Math.sin(theta) * dist_left;
      } else {

        //				System.out.println("new_theta= " + new_theta + " theta= " + theta);

        new_x = x + ((Math.sin(new_theta) - Math.sin(theta)) * turn_radius);
        new_y = y - ((Math.cos(new_theta) - Math.cos(theta)) * turn_radius);
      }
      orientation.theta = new_theta;
      location.x = new_x;
      location.y = new_y;

      maybe_fire_guns();
    }
  }
  // set status led state and wait a second
  public static void main(String[] args) throws IOException {
    char test = 0;
    int servo_position = 6;

    System.out.println("NanoVM - console echo demo");
    System.out.println("Please press some keys ...");

    // adc_value150 이상일경우 흰색 150이하 일경우 검은색
    ADC.setEnable(1, ADC.ENABLE);
    ADC.setEnable(2, ADC.ENABLE);
    ADC.setEnable(3, ADC.ENABLE);
    ADC.setEnable(4, ADC.ENABLE);

    Servo.release(0);

    int adc_value1, adc_value2, adc_value3, adc_value4;
    boolean enabled;
    while (true) {
      if (System.in.available() != 0) {
        test = ((char) System.in.read());
        System.out.print(test);
      } else {
        test = 0;
      }

      if (test == 'w') {
        Motor.setSpeed(0, 80);
      } else if (test == 's') {
        Motor.stop();
      }
      // Servo.setPosition(0,12);// 12
      enabled = ADC.getEnable(1);
      adc_value1 = ADC.getADC(1);
      System.out.println("adc1 " + adc_value1);

      enabled = ADC.getEnable(2);
      adc_value2 = ADC.getADC(2);
      System.out.println("adc2 " + adc_value2);

      enabled = ADC.getEnable(3);
      adc_value3 = ADC.getADC(3);
      System.out.println("adc3 " + adc_value3);

      enabled = ADC.getEnable(4);
      adc_value4 = ADC.getADC(4);
      System.out.println("adc4 " + adc_value4);

      if (adc_value2 < 150 || adc_value3 < 150) {
        Servo.setPosition(0, 6);
      } else if (adc_value1 < 150 && adc_value2 < 150) {
        Servo.setPosition(0, 9);
      } else if (adc_value4 < 150 && adc_value3 < 150) {
        Servo.setPosition(0, 3);
      } else if (adc_value1 < 150) {
        Servo.setPosition(0, 9);
      } else if (adc_value4 < 150) {
        Servo.setPosition(0, 3);
      }

      Clock.delay_ms(10);
    }
  }
  public void motorsOff(OutputStream out, byte[] buf, int len) {
    leftMotor.setPower(0);
    rightMotor.setPower(0);

    writeCRLF(out, "z");
  }