From 3c9882089f8906cec653bd7cfbe83d1d0460b5a0 Mon Sep 17 00:00:00 2001 From: Ulrich Dah Date: Thu, 3 Feb 2022 16:42:59 -0500 Subject: [PATCH 1/7] Update stigmergy performance evaluation script --- .../behaviors/buzz_scripts/stigmergyTime.bzz | 41 +++++++++++++------ src/crazyflie/behaviors/stigmergyTime.c | 13 +++++- 2 files changed, 40 insertions(+), 14 deletions(-) diff --git a/src/crazyflie/behaviors/buzz_scripts/stigmergyTime.bzz b/src/crazyflie/behaviors/buzz_scripts/stigmergyTime.bzz index 66d86473..36889a8f 100644 --- a/src/crazyflie/behaviors/buzz_scripts/stigmergyTime.bzz +++ b/src/crazyflie/behaviors/buzz_scripts/stigmergyTime.bzz @@ -2,32 +2,49 @@ # # -experiment = 1 -counter = 0 -NB_STEPS = 10 +go = 0 +val = 0 +consensus = 0 +NB_STEPS = 100 function init() { + if (id != 0) { + neighbors.listen("start", + function(vid, value, rid) { + start() + go = 1 + neighbors.ignore("start") + } + ) + } stig = stigmergy.create(0) if (id == 0) { stig.put("a", 40) stig.put("a", 42) - } else if (experiment == 1) { - stig.put("a", 20) } } function step() { - if (counter < NB_STEPS) { - val = stig.get("a") - counter = counter + 1 + # Robot 0 starts the experiment + if (id == 0) { + if (go == 0) { + neighbors.broadcast("start", 1) + go = 2 + } else { + led(1, 1) # GREEN ON + } + } + # Log whether we have consensus or not + if (go == 1) { + if (consensus == 0){ + val = stig.get("a") + } if (val == 42) { + consensus = 1 show(42) - } else if (val == 20) { - show(20) + led(1, 1) # GREEN ON } else { show(0) } - } else { - show(90) } } diff --git a/src/crazyflie/behaviors/stigmergyTime.c b/src/crazyflie/behaviors/stigmergyTime.c index d9c75ea9..f706c003 100644 --- a/src/crazyflie/behaviors/stigmergyTime.c +++ b/src/crazyflie/behaviors/stigmergyTime.c @@ -35,7 +35,7 @@ void bbz_print() { #ifndef DEBUG uint16_t d = (uint16_t)bbzheap_obj_at(bbzvm_locals_at(1))->i.value; if (t == NULL) { - t = xTaskGetTickCount(); + DEBUG_PRINT("ERROR\n"); } else if(d == 42 && done == 0) { t = xTaskGetTickCount() - t; DEBUG_PRINT("CONSENSUS: %d\n", t); @@ -48,10 +48,19 @@ void bbz_print() { bbzvm_ret0(); } +void bbz_go() { + if (t == NULL) { + DEBUG_PRINT("START\n"); + t = xTaskGetTickCount(); + } + bbzvm_ret0(); +} + void setup() { - // bbzvm_function_register(BBZSTRING_ID(led), bbz_led); + bbzvm_function_register(BBZSTRING_ID(led), bbz_led); // bbzvm_function_register(BBZSTRING_ID(delay), bbz_delay); bbzvm_function_register(BBZSTRING_ID(show), bbz_print); + bbzvm_function_register(BBZSTRING_ID(start), bbz_go); } int main() { From c253ceaa11b3d063759eb77b2b9f4513a7799e60 Mon Sep 17 00:00:00 2001 From: Ulrich Dah Date: Thu, 3 Feb 2022 16:44:11 -0500 Subject: [PATCH 2/7] Add script for bidding experiments --- src/crazyflie/behaviors/bidding.c | 66 ++++++++ .../behaviors/buzz_scripts/bidding.bst | 2 + .../behaviors/buzz_scripts/bidding.bzz | 143 ++++++++++++++++++ 3 files changed, 211 insertions(+) create mode 100644 src/crazyflie/behaviors/bidding.c create mode 100644 src/crazyflie/behaviors/buzz_scripts/bidding.bst create mode 100644 src/crazyflie/behaviors/buzz_scripts/bidding.bzz diff --git a/src/crazyflie/behaviors/bidding.c b/src/crazyflie/behaviors/bidding.c new file mode 100644 index 00000000..d99b6a7b --- /dev/null +++ b/src/crazyflie/behaviors/bidding.c @@ -0,0 +1,66 @@ +#include +#include +#include +#include +#include +#include "led.h" +#include "FreeRTOS.h" +#include "task.h" +#include "FreeRTOSConfig.h" +#include "debug.h" + +TickType_t t = NULL; +void bbz_led() { + bbzvm_assert_lnum(2); +#ifndef DEBUG + uint8_t color = (uint8_t)bbzheap_obj_at(bbzvm_locals_at(1))->i.value; + uint8_t value = (uint8_t)bbzheap_obj_at(bbzvm_locals_at(2))->i.value; + DEBUG_PRINT("color: %d\n", color); + ledSet(color?LINK_LED:LINK_DOWN_LED, value); +#endif + bbzvm_ret0(); +} + +void bbz_print() { + bbzvm_assert_lnum(1); + int val = (int)bbzheap_obj_at(bbzvm_locals_at(1))->i.value; + DEBUG_PRINT("WInner: %d\n", val); + bbzvm_ret0(); +} + +void bbz_show() { +#ifndef DEBUG + uint16_t d = (uint16_t)bbzheap_obj_at(bbzvm_locals_at(1))->i.value; + if (t == NULL) { + DEBUG_PRINT("ERROR\n"); + } else if (d == 1) { + t = xTaskGetTickCount() - t; + DEBUG_PRINT("CONSENSUS: %d\n", t); + } else { + DEBUG_PRINT("CONSENSUS: %d\n", t); + } + DEBUG_PRINT("SHOW\n"); +#endif + bbzvm_ret0(); +} + +void bbz_go() { + // if (t == NULL) { + DEBUG_PRINT("START\n"); + t = xTaskGetTickCount(); + // } + bbzvm_ret0(); +} + +void setup() { + bbzvm_function_register(BBZSTRING_ID(led), bbz_led); + bbzvm_function_register(BBZSTRING_ID(print), bbz_print); + bbzvm_function_register(BBZSTRING_ID(show), bbz_show); + bbzvm_function_register(BBZSTRING_ID(go), bbz_go); +} + +int main() { + bbz_init(setup); + + return 0; +} \ No newline at end of file diff --git a/src/crazyflie/behaviors/buzz_scripts/bidding.bst b/src/crazyflie/behaviors/buzz_scripts/bidding.bst new file mode 100644 index 00000000..6cdea613 --- /dev/null +++ b/src/crazyflie/behaviors/buzz_scripts/bidding.bst @@ -0,0 +1,2 @@ +led +print \ No newline at end of file diff --git a/src/crazyflie/behaviors/buzz_scripts/bidding.bzz b/src/crazyflie/behaviors/buzz_scripts/bidding.bzz new file mode 100644 index 00000000..df9a2e06 --- /dev/null +++ b/src/crazyflie/behaviors/buzz_scripts/bidding.bzz @@ -0,0 +1,143 @@ +# # EXPECTED BEHAVIOR : +# # +# # + +# STATE = "IDLE" +# auction_info = {} +# cur_best_bid = {.id_bidder = -1, .value = -1} +# nb_steps = 0 + +# function init() { +# if (id == 0) { +# STATE = "AUCTIONEER" +# listen_to_bid_value() +# } else { +# STATE = "IDLE" +# listen_to_auction() +# listen_to_bid_winner() +# } +# } + +# function step() { +# if (STATE == "AUCTIONEER") { +# led(0, 1) # RED ON +# if (nb_steps > 10 and nb_steps < 30) { +# neighbors.broadcast("auction", 1) +# } else if (nb_steps > 30 and nb_steps < 40) { +# print(cur_best_bid.id_bidder) +# neighbors.broadcast("bid_winner", cur_best_bid.id_bidder) +# } +# } else if (STATE == "BID") { +# if (nb_steps % 2 == 0) { +# neighbors.broadcast("bid_value", id) +# } +# } else if (STATE == "WINNER") { +# led(1, 1) # GREEN ON +# } +# nb_steps = nb_steps + 1 +# } + +# function listen_to_auction() { +# neighbors.listen("auction", +# function(vid, value, rid) { +# auction_info = value +# STATE = "BID" +# } +# ) +# } + +# function listen_to_bid_value() { +# neighbors.listen("bid_value", +# function(vid, value, rid) { +# if (cur_best_bid.value < value) { +# cur_best_bid.id_bidder = rid +# cur_best_bid.value = value +# } +# } +# ) +# } + +# function listen_to_bid_winner() { +# neighbors.listen("bid_winner", +# function(vid, value, rid) { +# if (value == id) { +# STATE = "WINNER" +# } else { +# STATE = "IDLE" +# } +# } +# ) +# } +STATE = "IDLE" +NB_BIDDERS = 6 +auction_info = {} +stop_bid = -1 +cur_best_bid = {.id_bidder = id, .value = id} +received_ids = {} +nb_bid_received = 0 +nb_steps = 0 + +function init() { + listen_to_bid_value() + if (id == 0) { + STATE = "AUCTIONEER" + } else { + STATE = "IDLE" + listen_to_auction() + } +} + +function step() { + if (STATE == "AUCTIONEER") { + led(0, 1) # RED ON + if (nb_steps == 5) { + neighbors.broadcast("auction", 1) + } # else if (nb_steps >= 15) { + # STATE = "BID" + # stop_bid = nb_steps + 15 + # led(0, 0) + # go() # Start bid timer + # } + } else if (STATE == "BID") { + if (nb_steps % 2 == 0) { + neighbors.broadcast("bid_value", id) + } + if(nb_bid_received == NB_BIDDERS - 1) { + STATE = "END" + show(1) # Mark bid end time + } + } else if (STATE == "END") { + show(0) # Show bid duration + if (cur_best_bid.value == id) { # Winner + led(1, 1) # GREEN ON + } + } + nb_steps = nb_steps + 1 +} + +function listen_to_auction() { + neighbors.listen("auction", + function(vid, value, rid) { + auction_info = value + stop_bid = nb_steps + 20 + STATE = "BID" + go() # Start bid timer + } + ) +} + +function listen_to_bid_value() { + neighbors.listen("bid_value", + function(vid, value, rid) { + if (received_ids[rid] == nil) { + received_ids[rid] = 1 + nb_bid_received = nb_bid_received + 1 + } + if (cur_best_bid.value < value) { + cur_best_bid.id_bidder = rid + cur_best_bid.value = value + } + } + ) +} + From 52c5ba4caa78d16faa4d2e43b0d0bce8df850bc5 Mon Sep 17 00:00:00 2001 From: Ulrich Dah Date: Thu, 3 Feb 2022 16:45:18 -0500 Subject: [PATCH 3/7] Update packet loss performance evaluation script --- .../behaviors/buzz_scripts/lostPackets.bzz | 24 +++++++------------ 1 file changed, 9 insertions(+), 15 deletions(-) diff --git a/src/crazyflie/behaviors/buzz_scripts/lostPackets.bzz b/src/crazyflie/behaviors/buzz_scripts/lostPackets.bzz index 4bf271d5..faadd08c 100644 --- a/src/crazyflie/behaviors/buzz_scripts/lostPackets.bzz +++ b/src/crazyflie/behaviors/buzz_scripts/lostPackets.bzz @@ -1,19 +1,14 @@ # EXPECTED BEHAVIOR : -# -# +# Robot 0 broadcast NB_STEPS messages +# The other robot(s) counts the number of packets received and logs it experiment = 2 counter = 0 received_packets = 0 -NB_STEPS = 20 +NB_STEPS = 200 function init() { - neighbors.listen("toto", - function(vid, value, rid) { - received_packets = received_packets + 1 - } - ) - neighbors.listen("rina", + neighbors.listen("packet", function(vid, value, rid) { received_packets = received_packets + 1 } @@ -23,11 +18,7 @@ function init() { function step() { if (counter < NB_STEPS and id == 0) { counter = counter + 1 - if (counter % 2 == 0) { - neighbors.broadcast("rina", counter) - } else { - neighbors.broadcast("toto", counter) - } + neighbors.broadcast("packet", counter) if (experiment == 0){ delay(1000) } else if (experiment == 1) { @@ -35,7 +26,10 @@ function step() { } else { } - } else { + } else if (id == 0) { + show(counter) + } + else { show(received_packets) } } From dc5e5517f1a130c752afdde501e7e14ef9b11440 Mon Sep 17 00:00:00 2001 From: Ulrich Dah Date: Thu, 3 Feb 2022 16:53:59 -0500 Subject: [PATCH 4/7] Add log when error occurs in VM and set execution period to 50ms --- src/crazyflie/lib/src/bbzcrazyflie.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/crazyflie/lib/src/bbzcrazyflie.c b/src/crazyflie/lib/src/bbzcrazyflie.c index 69ad098a..10e1e47c 100644 --- a/src/crazyflie/lib/src/bbzcrazyflie.c +++ b/src/crazyflie/lib/src/bbzcrazyflie.c @@ -137,7 +137,7 @@ void bbzcrazyflie_func_call(uint16_t strid) { while(blockptr < vm->blockptr) { if(vm->state != BBZVM_STATE_READY) return; bbzvm_step(); - DEBUG_PRINT("VM: Stepping\n"); + // DEBUG_PRINT("VM: Stepping\n"); handleOutgoingRadioMessage(); } } @@ -284,11 +284,11 @@ void bbzTask(void * param) uint8_t init_done = 0; while (1) { - vTaskDelayUntil(&lastWakeTime, F2T(1)); //delay some time get next data. Setting to 1 gives max delay. + vTaskDelayUntil(&lastWakeTime, F2T(20)); //delay some time get next data. Setting to 1 gives max delay. if (!init_done) { if (vm->state == BBZVM_STATE_READY) { - DEBUG_PRINT("VM: stepping now.\n"); + // DEBUG_PRINT("VM: stepping now.\n"); bbzvm_step(); } else { @@ -311,6 +311,8 @@ void bbzTask(void * param) bbzcrazyflie_func_call(__BBZSTRID_step); DEBUG_PRINT("VM: bbzcrazyflie_func_call(__BBZSTRID_ step) called.\n"); bbzvm_process_outmsgs(); + } else { + DEBUG_PRINT("VM: VM error code (%d).\n", vm->error); } // checkRadio(); // checkTouch(); From ffbba07e783f3c03ea8b10a7ea0e2ab82188c1ad Mon Sep 17 00:00:00 2001 From: Ulrich Dah Date: Thu, 3 Feb 2022 16:54:43 -0500 Subject: [PATCH 5/7] Increased stack size for crazyflie --- src/cmake/Crazyflie.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/cmake/Crazyflie.cmake b/src/cmake/Crazyflie.cmake index ca0cf642..162d6873 100644 --- a/src/cmake/Crazyflie.cmake +++ b/src/cmake/Crazyflie.cmake @@ -228,7 +228,7 @@ option(BBZ_NEIGHBORS_USE_FLOATS "Whether to use floats for the neighbor's range option(BBZ_ENABLE_FLOAT_OPERATIONS "Whether to enable floats operations" ON) option(BBZ_BYTEWISE_ASSIGNMENT "Whether to make assignment byte per byte or directly. (used to ensure compatibility with Cortex-M0)" OFF) #Turned ON for Cortex-M0. CF uses Cortex-M4 set(BBZHEAP_SIZE 3500) -set(BBZSTACK_SIZE 128) +set(BBZSTACK_SIZE 440) # message("BBZHEAP_SIZE := ${BBZHEAP_SIZE}") set(BBZHEAP_GCMARK_DEPTH 16) set(BBZNEIGHBORS_CAP 10) From 239b4312395674b0fbc894e34ddfe1122b196b7d Mon Sep 17 00:00:00 2001 From: Ulrich Dah Date: Thu, 10 Feb 2022 16:11:45 -0500 Subject: [PATCH 6/7] Add exploration experiment script --- .../behaviors/buzz_scripts/exploration.bst | 11 + .../behaviors/buzz_scripts/exploration.bzz | 116 ++++++++++ src/crazyflie/behaviors/exploration.c | 211 ++++++++++++++++++ 3 files changed, 338 insertions(+) create mode 100644 src/crazyflie/behaviors/buzz_scripts/exploration.bst create mode 100644 src/crazyflie/behaviors/buzz_scripts/exploration.bzz create mode 100644 src/crazyflie/behaviors/exploration.c diff --git a/src/crazyflie/behaviors/buzz_scripts/exploration.bst b/src/crazyflie/behaviors/buzz_scripts/exploration.bst new file mode 100644 index 00000000..85968c33 --- /dev/null +++ b/src/crazyflie/behaviors/buzz_scripts/exploration.bst @@ -0,0 +1,11 @@ +get_up +print +get_left +get_right +get_front +get_back +get_z +takeoff +land +move_toward +hover \ No newline at end of file diff --git a/src/crazyflie/behaviors/buzz_scripts/exploration.bzz b/src/crazyflie/behaviors/buzz_scripts/exploration.bzz new file mode 100644 index 00000000..16791cd7 --- /dev/null +++ b/src/crazyflie/behaviors/buzz_scripts/exploration.bzz @@ -0,0 +1,116 @@ +# INFO needed from C++ code: +# - distances from multiranger Y +# +# - random function +# - progress in a direction Y +# - takeoff Y +# - land Y +# - spin Y +# ---------------------------------------- +# States: +IDLE = 0 +EXPLORING = 1 +HOMING = 2 + +EXPLORATION_HEIGHT = 300 +GAP_WALL = 400 +DELTA = 50 + +state = IDLE +counter = 0 +ranger = {.up = 0, .left = 0, .right = 0, .front = 0, .rear = 0, .z = 0 } +sample_dir = {.x = 50, .y = 0} +s_start = swarm.create(0) +s_explore = swarm.create(1) + +function init() { + s_explore.select(id >= 10) + s_start.select(id < 10) + if (id >= 10) { + listen_exploration_start() + stig_end = stigmergy.create(0) + stig_end.put("end", 0) + } + state = IDLE +} + +function step() { + s_start.exec(start_behavior) + s_explore.exec(explore_behavior) +} + +function start_behavior() { + if (counter >= 20 and counter <= 22) { + neighbors.broadcast("start", 1) + } + counter = counter + 1 +} + +function explore_behavior() { + update_sensors() + if (state == EXPLORING and ranger.z > (EXPLORATION_HEIGHT - DELTA)) { + end = stig_end.get("end") + if (end == 1) { + state = HOMING + } + # hover(EXPLORATION_HEIGHT) + if (counter == 0) { + # Sample new direction + sample_dir.x = rand_bbz() + sample_dir.y = rand_bbz() + # Init counter to 10 + counter = 100 + } + goto(sample_dir) + counter = counter - 1 + + if (ranger.up < 1000) { + state = HOMING + stig_end.put("end", 1) + } + } else if (state == HOMING) { + land() + state = IDLE + } +} + +function update_sensors() { + ranger.up = get_up() + ranger.left = get_left() + ranger.right = get_right() + ranger.front = get_front() + ranger.rear = get_back() + ranger.z = get_z() +} + +function goto(direction) { + update_dir = {.x = direction.x, .y = direction.y} + if (ranger.front <= GAP_WALL + DELTA and ranger.front > 0) { + update_dir.x = update_dir.x - 100 + } + if (ranger.rear <= GAP_WALL + DELTA and ranger.rear > 0) { + update_dir.x = update_dir.x + 100 + } + if (ranger.left <= GAP_WALL + DELTA and ranger.left > 0) { + update_dir.y = update_dir.y - 100 + } + if (ranger.right <= GAP_WALL + DELTA and ranger.right > 0) { + update_dir.y = update_dir.y + 100 + } + move_toward(update_dir.x, update_dir.y) +} + +function listen_exploration_start() { + neighbors.listen("start", + function(vid, value, rid) { + # Takeoff + takeoff(EXPLORATION_HEIGHT) + state = EXPLORING + + # Sample a random direction + sample_dir.x = rand_bbz() + sample_dir.y = rand_bbz() + counter = 100 + } + ) +} \ No newline at end of file diff --git a/src/crazyflie/behaviors/exploration.c b/src/crazyflie/behaviors/exploration.c new file mode 100644 index 00000000..51f66975 --- /dev/null +++ b/src/crazyflie/behaviors/exploration.c @@ -0,0 +1,211 @@ +#include +#include +#include +#include +#include +#include "motors.h" +#include "FreeRTOS.h" +#include "task.h" +#include "FreeRTOSConfig.h" +#include "stabilizer_types.h" +#include "commander.h" +#include "string.h" +#include "debug.h" +#include "log.h" +#include +#include +#include +#include +#include "crtp_commander_high_level.h" + +uint16_t idRange = 0; +uint16_t range = 0; + +float VELOCITY = 0.5f; +uint16_t PROXIMITY_LIMIT = 300; +float TAKEOFF_HEIGHT = 0; + +void bbz_get_up() { + bbzvm_assert_lnum(0); + idRange = logGetVarId("range", "up"); + range = logGetUint(idRange); + bbzvm_pushi(range); + bbzvm_ret1(); +} + +void bbz_get_right() { + bbzvm_assert_lnum(0); + idRange = logGetVarId("range", "right"); + range = logGetUint(idRange); + bbzvm_pushi(range); + bbzvm_ret1(); +} + +void bbz_get_left() { + bbzvm_assert_lnum(0); + idRange = logGetVarId("range", "left"); + range = logGetUint(idRange); + bbzvm_pushi(range); + bbzvm_ret1(); +} + +void bbz_get_front() { + bbzvm_assert_lnum(0); + idRange = logGetVarId("range", "front"); + range = logGetUint(idRange); + bbzvm_pushi(range); + bbzvm_ret1(); +} + +void bbz_get_back() { + bbzvm_assert_lnum(0); + idRange = logGetVarId("range", "back"); + range = logGetUint(idRange); + bbzvm_pushi(range); + bbzvm_ret1(); +} + +void bbz_get_z() { + bbzvm_assert_lnum(0); + idRange = logGetVarId("range", "zrange"); + range = logGetUint(idRange); + bbzvm_pushi(range); + bbzvm_ret1(); +} + +void bbz_print() { + bbzvm_assert_lnum(1); + int val = (int)bbzheap_obj_at(bbzvm_locals_at(1))->i.value; + DEBUG_PRINT("%d\n", val); + bbzvm_ret0(); +} + +void bbz_takeoff() { + bbzvm_assert_lnum(1); + TAKEOFF_HEIGHT = (int)bbzheap_obj_at(bbzvm_locals_at(1))->i.value / 1000.0; + setpoint_t *setpoint = malloc(sizeof(sizeof(setpoint_t))); + memset(setpoint, 0, sizeof(setpoint_t)); + + setpoint-> mode.z = modeAbs; + setpoint->position.z = TAKEOFF_HEIGHT; + + setpoint->mode.yaw = modeVelocity; + setpoint->attitudeRate.yaw = 0.0; + + setpoint->mode.x = modeVelocity; + setpoint->mode.y = modeVelocity; + setpoint->velocity.x = 0.0; + setpoint->velocity.y = 0.0; + setpoint->velocity_body = true; + + commanderSetSetpoint(setpoint, 3); + + free(setpoint); + bbzvm_ret0(); +} + +void bbz_land() { + bbzvm_assert_lnum(0); + setpoint_t *setpoint = malloc(sizeof(sizeof(setpoint_t))); + memset(setpoint, 0, sizeof(setpoint_t)); + + setpoint-> mode.z = modeAbs; + setpoint->position.z = 0.0; + + setpoint->mode.yaw = modeVelocity; + setpoint->attitudeRate.yaw = 0; + + setpoint->mode.x = modeVelocity; + setpoint->mode.y = modeVelocity; + setpoint->velocity.x = 0.0; + setpoint->velocity.y = 0.0; + setpoint->velocity_body = true; + + commanderSetSetpoint(setpoint, 3); + + motorsBeep(MOTOR_M1, false, G6, 0); + motorsBeep(MOTOR_M2, false, G6, 0); + motorsBeep(MOTOR_M3, false, G6, 0); + motorsBeep(MOTOR_M4, false, G6, 0); + free(setpoint); + bbzvm_ret0(); +} + +void bbz_move_toward() { + bbzvm_assert_lnum(2); + float vx = (int)bbzheap_obj_at(bbzvm_locals_at(1))->i.value / 1000.0; + float vy = (int)bbzheap_obj_at(bbzvm_locals_at(2))->i.value / 1000.0; + setpoint_t *setpoint = malloc(sizeof(sizeof(setpoint_t))); + memset(setpoint, 0, sizeof(setpoint_t)); + + setpoint-> mode.z = modeAbs; + setpoint->position.z = TAKEOFF_HEIGHT; + + setpoint->mode.yaw = modeVelocity; + setpoint->attitudeRate.yaw = 0.0; + + setpoint->mode.x = modeVelocity; + setpoint->mode.y = modeVelocity; + setpoint->velocity.x = vx; + setpoint->velocity.y = vy; + setpoint->velocity_body = true; + + commanderSetSetpoint(setpoint, 3); + + free(setpoint); + bbzvm_ret0(); +} + +void bbz_hover() { +#ifndef DEBUG + bbzvm_assert_lnum(1); + float height = (int)bbzheap_obj_at(bbzvm_locals_at(1))->i.value / 1000.0; + setpoint_t *setpoint = malloc(sizeof(sizeof(setpoint_t))); + memset(setpoint, 0, sizeof(setpoint_t)); + + setpoint-> mode.z = modeAbs; + setpoint->position.z = height; + + setpoint->mode.yaw = modeVelocity; + setpoint->attitudeRate.yaw = 0.0; + + setpoint->mode.x = modeVelocity; + setpoint->mode.y = modeVelocity; + setpoint->velocity.x = 0.0; + setpoint->velocity.y = 0.0; + setpoint->velocity_body = true; + commanderSetSetpoint(setpoint, 3); + + free(setpoint); +#endif + bbzvm_ret0(); +} +void bbz_rand() { + bbzvm_assert_lnum(0); + int r = (int)((float)rand() * ((float)300 / (float)RAND_MAX)) - 150; + bbzvm_pushi(r); + bbzvm_ret1(); +} + +void setup() { + bbzvm_function_register(BBZSTRING_ID(rand_bbz), bbz_rand); + bbzvm_function_register(BBZSTRING_ID(get_up), bbz_get_up); + bbzvm_function_register(BBZSTRING_ID(get_left), bbz_get_left); + bbzvm_function_register(BBZSTRING_ID(get_right), bbz_get_right); + bbzvm_function_register(BBZSTRING_ID(get_front), bbz_get_front); + bbzvm_function_register(BBZSTRING_ID(get_back), bbz_get_back); + bbzvm_function_register(BBZSTRING_ID(get_z), bbz_get_z); + bbzvm_function_register(BBZSTRING_ID(print), bbz_print); + bbzvm_function_register(BBZSTRING_ID(takeoff), bbz_takeoff); + bbzvm_function_register(BBZSTRING_ID(land), bbz_land); + bbzvm_function_register(BBZSTRING_ID(move_toward), bbz_move_toward); + + bbzvm_function_register(BBZSTRING_ID(hover), bbz_hover); +} + +int main() { + srand(time(NULL)); + bbz_init(setup); + + return 0; +} \ No newline at end of file From 50904d5da34d82cd3c630fb16beac09957aa39f8 Mon Sep 17 00:00:00 2001 From: Ulrich Dah Date: Thu, 10 Feb 2022 16:14:32 -0500 Subject: [PATCH 7/7] Change delay function used for the stepping period --- src/crazyflie/lib/src/bbzcrazyflie.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/crazyflie/lib/src/bbzcrazyflie.c b/src/crazyflie/lib/src/bbzcrazyflie.c index 10e1e47c..74103516 100644 --- a/src/crazyflie/lib/src/bbzcrazyflie.c +++ b/src/crazyflie/lib/src/bbzcrazyflie.c @@ -284,7 +284,7 @@ void bbzTask(void * param) uint8_t init_done = 0; while (1) { - vTaskDelayUntil(&lastWakeTime, F2T(20)); //delay some time get next data. Setting to 1 gives max delay. + vTaskDelay(F2T(20)); //delay some time get next data. Setting to 1 gives max delay. if (!init_done) { if (vm->state == BBZVM_STATE_READY) {