diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 0618a72..503fa27 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -110,8 +110,8 @@ int buzzuav_goto(buzzvm_t vm) {
    goto_cartesian[1] = dy + cur_pos_cartesian[1];
    goto_cartesian[2] = cur_pos_cartesian[2];
    spherical_coordinates(goto_cartesian, goto_pos);
-//   goto_pos[0]=cur_pos[0]+dlat*180/M_PI;
-//   goto_pos[1]=cur_pos[1]+dlon*180/M_PI;
+   goto_pos[0]=dx;
+   goto_pos[1]=dy;
    goto_pos[2]=height;	// force a constant altitude
    cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
    printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f  \n",goto_pos[0],goto_pos[1],goto_pos[2]);
diff --git a/src/test1.bzz b/src/test1.bzz
index 3118d49..bbd8c4e 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -12,11 +12,11 @@ function updated_neigh(){
 neighbors.broadcast(updated, update_no)
 }
 
-TARGET_ALTITUDE = 10.1
+TARGET_ALTITUDE = 10.0
  
 # Lennard-Jones parameters
 TARGET     = 5.0	#0.000001001
-EPSILON    = 200.0	#0.001
+EPSILON    = 80.0	#0.001
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {