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(); } }