#+Title: Run and tumble + obstacle avoidance on the Crazyflie palm-sized drone #+Author: Nishant Elkunchwar, Krishna Balasubramanian, Jessica Noe

This repository contains the source code for a biology-inspired "run and tumble" source seeking behaviour coupled with obstacle avoidance on a palm-sized drone, Crazyflie 2.1.
#+html:

• Major highlights ** [[./crazyflie-firmware/src/deck/drivers/src][Deck drivers]]
  • [[./crazyflie-firmware/src/deck/drivers/src/bh1750deck.c][BH1750]] light sensor (if you are not using the continuous high resolution mode on the BH1750 sensor, please refer to the sensor datasheet and change the =conv_factor=, =CONT_HI_RES_MODE= in =i2cdevWriteByte()= and delay time in =vTaskDelayUntil()= accordingly)
  • [[./crazyflie-firmware/src/deck/drivers/src/hdc2010deck.c][HDC2010]] temperature sensor
  • [[./crazyflie-firmware/src/deck/drivers/src/nrfI2Cdeck.c][nRF52840]] dongle configured as an TWI device to measure bluetooth RSSI (note: this is possible using the on-board nRF chip too, but this method was used to quickly filter out bluetooth advertising packets from a particular MAC address by flashing the nRF dongle) ** A simulation environment for designing and testing the algorithm See [[./scripts/simulation.py][scripts/simulation.py]].

https://user-images.githubusercontent.com/14308382/135767027-af7cb36c-4d9a-480d-a9c5-4e3e0c6c232d.mov

Might also be of interest: [[https://github.com/thecountoftuscany/unicycle-navigation][unicycle-navigation]], a repository that simulates go-to-goal (with known robot and goal positions) and obstacle avoidance behaviours in a finite state machine. That repository is more educational in purpose, but this simulation builds upon that one and adds simulated sensors, etc ** Light source seeking See [[./scripts/run-and-tumble-alg_light.py][scripts/run-and-tumble-alg_light.py]].

https://user-images.githubusercontent.com/14308382/135767338-6554afb8-b1ab-46d6-83b2-ec6a89107e88.mov

** Temperature source seeking See [[./scripts/run-and-tumble-alg_temp.py][scripts/run-and-tumble-alg_temp.py]].

https://user-images.githubusercontent.com/14308382/135767356-2353b9a0-04e4-4f12-8626-ac466eadc21b.mov

https://user-images.githubusercontent.com/14308382/135767396-848630aa-a9d7-439e-b01a-12c716a38bdc.mov

** Keyboard-based crazyflie control, live plotting and logging See [[./scripts/cflibController.py][scripts/cflibController.py]]. This script can be invoked to manually control the crazyflie. Also contains functionality to live plot drone position estimate, log various measurements (eg. position, temperature, light intensity, multiranger ranges) to a csv file. Please use the =-h= flag for usage

• Using this repository

• Patch a recently cloned copy of the [[https://github.com/bitcraze/crazyflie-firmware][crazyflie-firmware]] with the contents from [[./crazyflie-firmware/src/deck/drivers/src]]
• Modify the =crazyflie-firmware/Makefile= to include any of the new drivers from this repository being used (eg. =bh1750deck.c= in the example below): #+begin_src

Decks

PROJ_OBJ += bh1750deck.o

PROJ_OBJ += hdc2010deck.o

PROJ_OBJ += nrfI2Cdeck.o

#+end_src

• Copy =crazyflie-firmware/tools/make/config.mk.example= to =crazyflie-firmware/tools/make/config.mk= and force loading of any new drivers (eg. =bh1750deck= driver in the example below): #+begin_src

bh1750 deck

CFLAGS += -DDECK_FORCE=bh1750Deck DEBUG=1

hdc2010 deck

CFLAGS += -DDECK_FORCE=hdc2010Deck

nrf I2C deck

CFLAGS += -DDECK_FORCE=nrfI2CDeck

#+end_src

• Compile and flash the firmware to a crazyflie. Instructions [[https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/building-and-flashing/build/][here]].
• Use the relevant script from [[./scripts][scripts]].

• Description of repository contents

• [[./crazyflie-firmware/src/deck/drivers/src][crazyflie-firmware/src/deck/drivers/src]]: the deck drivers for the three devices used for signal strength detection
• [[./data][data]]: data recorded from all experiments. See the [[Results][Results]] section in this readme for details
• [[./docs][docs]]: relevant datasheets etc
• [[./resources][resources]]: resources used in this README
• [[./scripts][scripts]]: all scripts
  • [[./scripts/plots][scripts/plots]]: scripts used for generating all plots from [[./data][data]]. See the [[Results][Results]] section in the readme for details
  • [[./scripts/cflibController.py][scripts/cflibController.py]]: manual control and live plotting of the crazyflie along with logging measurements to a csv file
  • [[./scripts/run-and-tumble-alg_light.py][scripts/run-and-tumble-alg_light.py]] and [[./scripts/run-and-tumble-alg_temp.py][scripts/run-and-tumble-alg_temp.py]]: the algorithms used for each source seeking method. The only major difference between the two at the high level is that the temperature seeking algorithm has a stop command for a few seconds to let the temperature sensor stabilize at a measurement. See the respective files for more details
  • [[./scripts/simulation.py][scripts/simulation.py]]: runs a simulation of a robot seeking a source with an inverse square potential in the presence of obstacles. Use the =-h= flag for details

• Requirements

• The following python packages (obtain using =pip=, =conda=, your OS distribution package manager or any other preferred means):
  • =numpy=
  • =pygame=
  • [[https://github.com/bitcraze/crazyflie-lib-python][cflib]]
  • =argparse=
  • =matplotlib=
• [[https://github.com/bitcraze/crazyflie-firmware][crazyflie-firmware]] (just clone the repository)

• Method ** Source characterization #+html:

  

  (left) Light intensity distribution of an indoor light source using drone's position estimate. [top] with 2D position [bottom] along a straight line path towards the source, (middle) Temperature distribution for a fire using ground truth distance [top], and drone position estimate [bottom], (right) Bluetooth source packet detection rate distribution with ground truth distance

We can see that for the light source the intensity distribution qualitatively follows an [[https://en.wikipedia.org/wiki/Inverse-square_law#Light_and_other_electromagnetic_radiation][inverse-squared distribution]] as expected from a point light source. Hence this choice in the simulation was reasonable. For the fire we see from the top plot that it takes a while for the temperature sensor to reach a steady state (since it is not an ideal blackbody) hence we add a stop command for a few seconds after every run to ensure the sensor measurements have stabilized. Future work will focus on extending this to Radio Frequency (RF) sources as it can be observed that bluetooth packet rate also follows a roughly monotonic trend. ** Simulation We used ~PyGame~ to create the simulation environment to for rapid testing of the algorithm before deploying it on the actual drone. The code in [[./scripts/simulation.py][simulation.py]] is pretty self-explanatory to understand what is going on. ** Algorithm The algorithm is implemented as the following finite state machine: #+html:

• run: move forward with a constant velocity
• tumble: turn left or right to a random angle
• avoid-obstacle: move directly away from the closest obstacle along the direction of the distance sensor, and change heading away slightly

The exact implementation details can be seen in [[./scripts/run-and-tumble-alg_light.py][scripts/run-and-tumble-alg_light.py]] and [[./scripts/run-and-tumble-alg_temp.py][scripts/run-and-tumble-alg_temp.py]].

Note: The last intensity is taken to be the average of last 10 recorded intensity values to minimize errors due to sensor noise.

• Results ** Distance to source always decreases For example, various runs from the simulation: #+html:

  

  (left) The blue circles are randomly placed obstacles. Multiple trajectories (different colors) with different starting points (red dots) at the same distance from the source (black diamond) (right) Variation of distance of the robot from the light source for the trajectories on the left ** Increasing obstacle avoidance distance threshold increases source seeking time For example, various runs from the simulation: |------------------+-------------------------------| | =obst_threshold= | average seek time for 10 runs | |------------------+-------------------------------| | 20 px | 6.54 s | | 40 px | 11.10 s | | 80 px | 13.74 s | | 100 px | 22.88 s | |------------------+-------------------------------| ** Light source seeking experiments #+html:

  

  ** Heat source seeking experiments #+html:

  

• Authors In case more details are needed, please contact one of the authors listed below:

• [[https://www.linkedin.com/in/nishant-elkunchwar][Nishant Elkunchwar]]
• [[https://homes.cs.washington.edu/~vsiyer/][Vikram Iyer]]
• [[https://www.biology.washington.edu/people/profile/melanie-anderson][Melanie Anderson]]
• [[https://www.linkedin.com/in/krishna-balasubramanian-79199289/][Krishna Balasubramanian]]
• [[https://www.linkedin.com/in/jessica-noe-a7a77313/][Jessica Noe]]
• [[https://www.linkedin.com/in/yashtalwekar][Yash Talwekar]]
• [[https://faculty.washington.edu/minster/][Sawyer B. Fuller]]

• Acknowledgements The authors would like to thank [[https://yogeshchukewad.com/][Yogesh Chukewad]] for insightful suggestions as well as [[https://staff.washington.edu/jmjames/][Johannes James]] for assistance with the fire seeking experiments. The authors would also like to thank [[https://www.afrl.af.mil/AFOSR/][The Air Force Office of Scientific Research (AFOSR)]], grant no. FA9550-14-1-0398 by [[http://nifti.washington.edu/][The Air Force Center of Excellence on Nature-Inspired Flight Technologies and Ideas (NIFTI)]] for funding the equipment used in this research.

Note: This project started as a course project for the course [[https://faculty.washington.edu/minster/bio_inspired_robotics/][biology-inspired robotics]] taken by [[https://faculty.washington.edu/minster/][Prof. Sawyer B. Fuller]] at the University of Washington during Winter 2020. The original goal was light source seeking ([[https://www.youtube.com/watch?v=fgn8WjtvQ8k][video here]]). [[https://www.bitcraze.io/2020/07/intro-to-autonomous-robotics-with-the-crazyflie/][This blog post]] on the Bitcraze blog provides a good introduction. However the information contained in that blog post (and previous versions of this README) is outdated considering the current contents of this repository because the project has been in continued development after the course completion (at [[https://depts.washington.edu/airlab/][AIRlab]]) and it has increased in scope and some implementation details have been changed. The original project poster, proposal etc can be accessed from [[https://github.com/thecountoftuscany/crazyflie-run-and-tumble/tree/ca5551c61cfac40ec27befd05dfa0bb7c93e0499/documents][this commit]]. The initial approach was based on using the ROS ecosystem and the [[https://github.com/JGSuw/rospy_crazyflie][rospy_crazyflie]] library (a wrapper around [[https://github.com/bitcraze/crazyflie-lib-python][cflib]]) used in the [[https://depts.washington.edu/airlab/files/anderson_2019.pdf][smellicopter]] paper. We currently directly use cflib to be Python3 compliant. The ros-based scripts and other deprecated files can be accessed from [[https://github.com/thecountoftuscany/crazyflie-run-and-tumble/tree/ae6a137a5c590964edecda536cc7a88f88fce83f][this commit]]. The current version also does not use a virtual machine.

• Todos

• [ ] The crazyflie now has an [[https://www.bitcraze.io/documentation/repository/crazyflie-firmware/2020.02/app_layer/][app layer]]. Implement this algorithm on-board and remove the dependency on an external computer