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

lj test

This commit is contained in:
David St-Onge 2017-01-31 02:18:29 -05:00
parent fe33806904
commit af6602c649
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/buzzuav_closures.cpp
View File

@ -111,8 +111,10 @@ int buzzuav_goto(buzzvm_t vm) {
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); //buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
float d = buzzvm_stack_at(vm, 1)->f.value; float dx = buzzvm_stack_at(vm, 1)->f.value;
float b = buzzvm_stack_at(vm, 2)->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); printf(" Vector for Goto: %.7f,%.7f\n",d,b);
//goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value; //goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
//double cur_pos_cartesian[3]; //double cur_pos_cartesian[3];

5
src/test1.bzz
View File

@ -41,7 +41,7 @@ function hexagon() {
if(neighbors.count() > 0) if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count()) math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector # 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) uav_goto(accum.x, accum.y)
} }
@ -102,7 +102,7 @@ neighbors.listen("cmd",
} }
function takeoff() { 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_set(ROBOTS,hexagon)
barrier_ready() barrier_ready()
} }
@ -139,7 +139,6 @@ function step() {
statef=land statef=land
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
} }
statef() statef()
} }