Spiri
/
ROSBuzz_MISTLab
13
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

goto test

This commit is contained in:
David St-Onge 2017-01-26 23:37:08 -05:00
parent 4027de8262
commit 597ce839c4
2 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/buzzuav_closures.cpp
View File

@ -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;

2
src/test1.bzz
View File

@ -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)
}