diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 8df9100..618a317 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -99,13 +99,14 @@ int buzzuav_goto(buzzvm_t vm) { buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); float dx = buzzvm_stack_at(vm, 1)->f.value; float dy = buzzvm_stack_at(vm, 2)->f.value; + printf(" Vector for Goto: %f,%f\n",dx,dy); //goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value; double cur_pos_cartesian[3]; cartesian_coordinates(cur_pos,cur_pos_cartesian); double goto_spheric[3]; goto_spheric[0] = dx + cur_pos_cartesian[0]; goto_spheric[1] = dy + cur_pos_cartesian[1]; - goto_spheric[2] = 6371000 + cur_pos_cartesian[2]; + goto_spheric[2] = cur_pos_cartesian[2]; spherical_coordinates(goto_spheric, goto_pos); // goto_pos[0]=cur_pos[0]+dlat*180/M_PI; // goto_pos[1]=cur_pos[1]+dlon*180/M_PI; diff --git a/src/test1.bzz b/src/test1.bzz index ca6b7ab..55fd63a 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -34,7 +34,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.x, "; ", accum.y) uav_goto(accum.x, accum.y) }