diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index aaac839..ad15d3d 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -111,8 +111,10 @@ int buzzuav_goto(buzzvm_t vm) {
    //buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
    buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
    buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
-   float d = buzzvm_stack_at(vm, 1)->f.value;
-   float b  = buzzvm_stack_at(vm, 2)->f.value;
+   float dx = buzzvm_stack_at(vm, 1)->f.value;
+   float dy = buzzvm_stack_at(vm, 2)->f.value;
+   float d = sqrt(dx*dx+dy*dy);
+   float b = atan2(dy,dx);
    printf(" Vector for Goto: %.7f,%.7f\n",d,b);
    //goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
    //double cur_pos_cartesian[3];
diff --git a/src/test1.bzz b/src/test1.bzz
index d6a5bcc..671e0cc 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -41,7 +41,7 @@ function hexagon() {
   if(neighbors.count() > 0)
     math.vec2.scale(accum, 1.0 / neighbors.count())
   # Move according to vector
-#  print("Robot ", id, "must push ",accum.x, "; ", accum.y)
+  print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
   uav_goto(accum.x, accum.y)
 }
  
@@ -102,7 +102,7 @@ neighbors.listen("cmd",
 }
 
 function takeoff() {
-	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/50.0) {
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
 		barrier_set(ROBOTS,hexagon)
 		barrier_ready()
 	}
@@ -139,7 +139,6 @@ function step() {
 		statef=land
 		neighbors.broadcast("cmd", 21)
 	}
-
 statef()
 }