Update stepping period for Crazyflie and new experiments scripts

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)

src/crazyflie/behaviors/bidding.c

@@ -0,0 +1,66 @@
+#include <bbzsymbols.h>
+#include <bittybuzz/bbzutil.h>
+#include <bittybuzz/util/bbzstring.h>
+#include <bittybuzz/bbzvm.h>
+#include <bbzcrazyflie.h>
+#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;
+}

src/crazyflie/behaviors/buzz_scripts/bidding.bst

@@ -0,0 +1,2 @@
+led
+print

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

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

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