Merge pull request buzz-lang#16 from ulrichdah/master

New behaviors for the paper experiments (crazyflies)

Author: beltrame

src/cmake/Crazyflie.cmake

@@ -159,7 +159,7 @@ set(PROCESSOR "-mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16")
 
 set(DEFS "-DCRAZYFLIE_FW -DARM_MATH_CM4 -D__FPU_PRESENT=1 -D__TARGET_FPU_VFP -DSTM32F4XX -DSTM32F40_41xxx -DHSE_VALUE=8000000 -DUSE_STDPERIPH_DRIVER -DUSE_RADIOLINK_CRTP -DENABLE_UART -DBOARD_REV_${REV} -DESTIMATOR_NAME=${ESTIMATOR}Estimator -DCONTROLLER_NAME=ControllerType${CONTROLLER} -DPOWER_DISTRIBUTION_TYPE_${POWER_DISTRIBUTION}")
 
-set(SENSOR_CONFIG "-DSENSORS_IGNORE_BAROMETER_FAIL -DSENSOR_INCLUDED_MPU9250_LPS25H -DSENSORS_FORCE=SensorImplementation_mpu9250_lps25h -DSENSOR_INCLUDED_BMI088_BMP388")
+set(SENSOR_CONFIG "-DSENSORS_IGNORE_BAROMETER_FAIL -DSENSOR_INCLUDED_MPU9250_LPS25H -DSENSOR_INCLUDED_BMI088_BMP388")
 #-DSENSOR_INCLUDED_BMI088_BMP388 -DSENSOR_INCLUDED_MPU9250_LPS25H
 
 #set(FORCEDEVICE "-DDEVICE_TYPE_STRING_FORCE=CF20 -DDECK_FORCE=bcDWM1000")

src/crazyflie/behaviors/buzz_scripts/lostPackets.bst

@@ -0,0 +1,41 @@
+# EXPECTED BEHAVIOR :
+#
+# 
+
+experiment = 2
+counter = 0
+received_packets = 0
+NB_STEPS = 20
+
+function init() {
+    neighbors.listen("toto",
+        function(vid, value, rid) {
+            received_packets = received_packets + 1
+        }
+    )
+    neighbors.listen("rina",
+        function(vid, value, rid) {
+            received_packets = received_packets + 1
+        }
+    )
+}
+
+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)
+        }
+        if (experiment == 0){
+            delay(1000)
+        } else if (experiment == 1) {
+            delay(500)
+        } else {
+            
+        }
+    } else {
+        show(received_packets)
+    }
+}

src/crazyflie/behaviors/buzz_scripts/lostPackets.bzz

@@ -0,0 +1,6 @@
+delay
+takeoff
+land
+spin
+hover
+goTo

src/crazyflie/behaviors/buzz_scripts/neighborsAvoidance.bst

@@ -0,0 +1,77 @@
+# EXPECTED BEHAVIOR :
+#
+# Crazyflies with id:
+# > 0: Takeoff on start and hover at 300 mm (with obstacles avoidance) until it receives a landing message
+# = 0: Takeoff on start, spin 5 times (with obstacles avoidance), broadcats a landing message and land itself.
+#
+# To work properly, the crazyflies need to be equiped with a flow deck and a multiranger (necessary for the obstacles avoidance)
+
+offset_x = 0
+offset_y = 0
+stop = 0
+done = 0
+HEIGHT = 600
+s_hover = swarm.create(0)
+s_explore = swarm.create(1)
+
+function init() {
+    neighbors.listen("pos",
+        function(vid, value, rid) {
+            stop = 0
+            neighbors.foreach(function(r_id, data) {
+                if (neighbors.get(rid).distance < 500) {
+                    HEIGHT = HEIGHT + 200
+                }
+            })
+        }
+    )
+    neighbors.listen("land",
+        function(vid, value, rid) {
+            land(HEIGHT, 10)
+            done = 1
+        }
+    )
+    s_explore.select(id >= 10)
+    s_hover.select(id < 10)
+    if (id >= 10) {
+        neighbors.ignore("land")
+        stop = 1
+    } else {
+        neighbors.ignore("pos")
+    }
+    takeoff(HEIGHT, 10)
+}
+
+function step() {
+    if (done == 0) {
+        s_hover.exec(hover_behavior)
+        s_explore.exec(move_behavior)
+    }
+}
+
+# ========================================
+# =         CLOSURE DEFINITIONS          =
+# ========================================
+
+function hover_behavior() {
+    hover(HEIGHT)
+    neighbors.broadcast("pos", 1)
+}
+
+function move_behavior() {
+    if (stop == 0) {
+        goTo(3000 + 100 * offset_x, 2000 + 200 * offset_y, HEIGHT)
+        offset_x = offset_x + 1
+        if (offset_x % 2 == 0) {
+            offset_y = offset_y + 1
+        }
+        if (offset_x == 7) {
+            neighbors.broadcast("land", 1)
+            land(HEIGHT, 10)
+            stop = 1
+            done = 1
+        }
+    } else {
+        hover(HEIGHT)
+    }
+}

src/crazyflie/behaviors/buzz_scripts/neighborsAvoidance.bzz

@@ -0,0 +1,33 @@
+# EXPECTED BEHAVIOR :
+#
+# 
+
+experiment = 1
+counter = 0
+NB_STEPS = 10
+
+function init() {
+    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
+        if (val == 42) {
+            show(42)
+        } else if (val == 20) {
+            show(20)
+        } else {
+            show(0)
+        }
+    } else {
+        show(90)
+    }
+}

src/crazyflie/behaviors/buzz_scripts/stigmergyTime.bst

@@ -0,0 +1,50 @@
+#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"
+
+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;
+    ledSet(color?LINK_LED:LINK_DOWN_LED, value);
+#endif
+    bbzvm_ret0();
+}
+
+void bbz_delay() {
+    bbzvm_assert_lnum(1);
+#ifndef DEBUG
+    uint16_t d = (uint16_t)bbzheap_obj_at(bbzvm_locals_at(1))->i.value;
+    vTaskDelay(M2T(d));
+#endif
+    bbzvm_ret0();
+}
+
+void bbz_print() {
+    bbzvm_assert_lnum(1);
+#ifndef DEBUG
+    uint16_t d = (uint16_t)bbzheap_obj_at(bbzvm_locals_at(1))->i.value;
+    DEBUG_PRINT("PRINT: %d\n", d);
+#endif
+    bbzvm_ret0();
+}
+
+void setup() {
+    // bbzvm_function_register(BBZSTRING_ID(led), bbz_led);
+    bbzvm_function_register(BBZSTRING_ID(delay), bbz_delay);
+    bbzvm_function_register(BBZSTRING_ID(show), bbz_print);
+}
+
+int main() {
+    bbz_init(setup);
+
+    return 0;
+}