Time-of-Flight (ToF) Abstandssensoren messen die Distanz zu einem Objekt, indem sie die Zeit messen, die ein Lichtimpuls benötigt, um vom Sensor zum Objekt und zurück zu gelangen. Sensoren vom Typ VL53L0X können Entfernungen bis zu zwei Metern messen. Diese kommunizieren mit einem Mikrocontroller über I2C.
Der 8 Kanal ToF Sensor des KeplerBRAIN V4 Robotik-Systems besteht aus einem Sonsorboard an das bis zu 8 einzelne ToF Sensoren angeschlossen werden können. Das Sensorboard verfügt über einen STM32F441RE Mikrocontroller mit dem die LV53L0X Sensoren über I2C kommunizieren.
An den Buchsen auf der rechten Seite wird die benötigte Anzahl an einzelnen ToF Sensoren angeschlossen. Dabei ist zu beachten, dass immer bei der Buchse Sensor 1 begonnen werden muss und die nächsten Sensoren direkt daneben angesteckt werden. Die nicht benötigten Buchsen bleiben einfach leer. Die mittlere, 7-polige Buchse dient dazu, um das Sensorboard an eine der beiden SPI Schnittstellen des OpenBOT Mainboards anzuschließen.
Firmware für das KeplerBRAIN V4 VL53L0X Sensorboard
Der folgende Code ist quasi das Betriebssystem des Sensors. Dieser kann 1:1 so in der Arduino IDE kompilliert und auf das Sensorboard übertragen werden. Dazu muss in der Arduino IDE zuvor aber die Bibliothek VL53L0X von Pololu importiert werden.
Installation der VL53L0X Bibliothek in der Arduino IDE
1. Tools > Manage Libraries ...
2. Eingabe von VL53L0X in das Suchfeld
3. Klick auf den Button INSTALL bei VL53L0X by Pololu
Funktionsumfang und Aufgabe der Firmware
Die Software für den STM32F411RE Mikrocontroller des Sensorboard deckt zwei Aufgabenbereiche ab. Zum einen vergibt sie beim Start den einzelnen VL53L0X Sensoren automatisch unterschiedliche Adressen und liest im Anschluss laufend die aktuellen Werte der angeschlossenen Sensoren ein und zum anderen verarbeitet sie Anfragen eines anderen Mikrocontrollers über die SPI Schnittstelle und sendet daraufhin Werte zu diesem zurück.
Diese Messwerte der TOF-Sensoren werden im Array spi_data[] abgelegt und dann über die SPI-Schnittstelle in einem durchgehenden Stream von 9 Werten zum Mikrocontoller eines KeplerBRAIN Mainboards geschickt. Der erste Wert eines solchen Datenpakets ist jeweils die Zahl 250, mit der der Beginn eines Datenpakets vom empfangenden Mikrocontroller identifiziert werden kann. Dabei ist darauf zu achten, dass keiner der folgenden 8 Zahlen den Wert 250 annehmen darf.
Erweiterung der Firmware
Es ist möglich, bereits auf dem Sensorboard eine Analyse der gemessenen Werte vorzunehmen oder Berechnungen mit den Werten durchzuführen und dann nur die Ergebnisse an den Mikrocontroller auf dem Mainboard zu übertragen. Dazu sind im folgenden Code in den Funktionen setup() und loop() zwei Bereiche mit dem Kommentar // User Code vorgesehen, in denen eigener Code platziert werden kann.
Auch ist es möglich über den Programmieradapter direkt Werte im Serial Monitor der Arduino IDE zum Testen des eigenen Codes anzeigen zu lassen. Siehe dazu Arduino > Arduino C C++ > Serial Monitor
// ***********************************************************// *** KeplerBRAIN V4 Sensor STAMP SPI Client TOF Sensor ***// ***********************************************************#include <VL53L0X.h>// TOF SensorVL53L0X sensor1;VL53L0X sensor2;VL53L0X sensor3;VL53L0X sensor4;VL53L0X sensor5;VL53L0X sensor6;VL53L0X sensor7;VL53L0X sensor8;uint16_t s1;uint16_t s2;uint16_t s3;uint16_t s4;uint16_t s5;uint16_t s6;uint16_t s7;uint16_t s8;// SPI Datavolatile uint8_t spi_data[9] = {250,1,2,3,4,5,6,7,8};volatile uint8_t tx_index = 0;void setup(){// Initialize SPI1 as Slave with interruptSPI1_Init();// Enable the SPI1 interrupt in NVICNVIC_EnableIRQ(SPI1_IRQn);// Initialize TOFpinMode(PB12, OUTPUT);pinMode(PB13, OUTPUT);pinMode(PB14, OUTPUT);pinMode(PC3, OUTPUT);pinMode(PB2, OUTPUT);pinMode(PB1, OUTPUT);pinMode(PB0, OUTPUT);pinMode(PC5, OUTPUT);digitalWrite(PB12, LOW);digitalWrite(PB13, LOW);digitalWrite(PB14, LOW);digitalWrite(PC3, LOW);digitalWrite(PB2, LOW);digitalWrite(PB1, LOW);digitalWrite(PB0, LOW);digitalWrite(PC5, LOW);Wire.setSDA(PB4);Wire.setSCL(PA8);Wire.begin();digitalWrite(PB12, HIGH);delay(150);sensor1.init(true);delay(100);sensor1.setAddress((uint8_t)00);digitalWrite(PB13, HIGH);delay(150);sensor2.init(true);delay(100);sensor2.setAddress((uint8_t)01);digitalWrite(PB14, HIGH);delay(150);sensor3.init(true);delay(100);sensor3.setAddress((uint8_t)02);digitalWrite(PC3, HIGH);delay(150);sensor4.init(true);delay(100);sensor4.setAddress((uint8_t)03);digitalWrite(PB2, HIGH);delay(150);sensor5.init(true);delay(100);sensor5.setAddress((uint8_t)04);digitalWrite(PB1, HIGH);delay(150);sensor6.init(true);delay(100);sensor6.setAddress((uint8_t)05);digitalWrite(PB0, HIGH);delay(150);sensor7.init(true);delay(100);sensor7.setAddress((uint8_t)06);digitalWrite(PC5, HIGH);delay(150);sensor8.init(true);delay(100);sensor8.setAddress((uint8_t)07);sensor1.startContinuous();sensor2.startContinuous();sensor3.startContinuous();sensor4.startContinuous();sensor5.startContinuous();sensor6.startContinuous();sensor7.startContinuous();sensor8.startContinuous();// User Code}void loop(){s1 = sensor1.readRangeContinuousMillimeters() / 10;s2 = sensor2.readRangeContinuousMillimeters() / 10;s3 = sensor3.readRangeContinuousMillimeters() / 10;s4 = sensor4.readRangeContinuousMillimeters() / 10;s5 = sensor5.readRangeContinuousMillimeters() / 10;s6 = sensor6.readRangeContinuousMillimeters() / 10;s7 = sensor7.readRangeContinuousMillimeters() / 10;s8 = sensor8.readRangeContinuousMillimeters() / 10;if (s1<125) spi_data[1] = s1; else spi_data[1] = 0;if (s2<125) spi_data[2] = s2; else spi_data[2] = 0;if (s3<125) spi_data[3] = s3; else spi_data[3] = 0;if (s4<125) spi_data[4] = s4; else spi_data[4] = 0;if (s5<125) spi_data[5] = s5; else spi_data[5] = 0;if (s6<125) spi_data[6] = s6; else spi_data[6] = 0;if (s7<125) spi_data[7] = s7; else spi_data[7] = 0;if (s8<125) spi_data[8] = s8; else spi_data[8] = 0;// User Code}// Initialize SPI1 in slave mode with interruptvoid SPI1_Init(){// Enable clocks for GPIOA and SPI1RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;// Configure PA4 (NSS), PA5 (SCK), PA6 (MISO), PA7 (MOSI) as alternate functionGPIOA->MODER &= ~(GPIO_MODER_MODER4 | GPIO_MODER_MODER5 | GPIO_MODER_MODER6 | GPIO_MODER_MODER7);GPIOA->MODER |= (GPIO_MODER_MODER4_1 | GPIO_MODER_MODER5_1 | GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1);GPIOA->AFR[0] |= (5 << (4 * 4)) | (5 << (5 * 4)) | (5 << (6 * 4)) | (5 << (7 * 4)); // Set AF5 for SPI1// Configure SPI1 as a slaveSPI1->CR1 = 0; // Reset CR1SPI1->CR1 &= ~SPI_CR1_MSTR; // Set as slaveSPI1->CR1 &= ~SPI_CR1_SSM; // Disable software slave managementSPI1->CR1 |= SPI_CR1_SPE; // Enable SPISPI1->CR2 |= SPI_CR2_RXNEIE; // Enable RXNE interruptSPI1->CR2 |= SPI_CR2_TXEIE; // Enable TXE interrupt}extern "C" void SPI1_IRQHandler(void){// Check if RXNE (Receive Not Empty) flag is set and handle RX firstif (SPI1->SR & SPI_SR_RXNE){uint8_t rx_value = SPI1->DR;}// Check if TXE (Transmit Empty) flag is set, but only if not busyif (SPI1->SR & SPI_SR_TXE){// Send data from tx_buffer if in "send mode"if (tx_index < 9){// Write data to SPISPI1->DR = spi_data[tx_index];tx_index++;}else{// Reset buffer indextx_index = 0;}}}
Programmbeispiel - Einlesen von Werten des 8 Channel ToF Abstandssensors
Das Sensorboard wird an die SPI1 Schnittstelle des OpenBOT Mainboards angeschlossen. Am Sensorboard werden 4 ToF Sensoren an die Buchsen Sensor 1 bis Sensor 4 angeschlossen. Am Display sollen laufend die aktuellen, gemessenen Abstandswerte der 4 Sensoren angezeigt werden.
Dazu muss eine SPI Datenübertragung initiiert werden. Dies geschieht dadurch, dass die CS-Leitung (Chip Select) der SPI-Verbindung auf LOW gesetzt wird. Mit der Funktion spi.transfer(0xFF) wird ein Byte aus dem Datenstream ausgelesen. Kommt in diesem die Zahl 250 vor, so ist der erste Wert des Datenpakets aus 9 Werten identifiziert und die nächsten 8 Werte werden in den Variablen value_1, ..., value_8 abgelegt. Am Ende der SPI Kommunikation muss die CS-Leitung wieder auf HIGH gesetzt werden.
#include "KeplerBRAIN_V4.h"uint8_t value_1;uint8_t value_2;uint8_t value_3;uint8_t value_4;uint8_t value_5;uint8_t value_6;uint8_t value_7;uint8_t value_8;void setup(){KEPLERBRAIN_INIT();WRITE_LCD_TEXT(1,1," ");WRITE_LCD_TEXT(1,2," ");}void loop(){// read 8 Bytes from TOF Sensorboard BEGINdigitalWrite(SPI1, LOW);if(spi.transfer(0XFF) == 250){value_1 = spi.transfer(0XFF);value_2 = spi.transfer(0XFF);value_3 = spi.transfer(0XFF);value_4 = spi.transfer(0XFF);value_5 = spi.transfer(0XFF);value_6 = spi.transfer(0XFF);value_7 = spi.transfer(0XFF);value_8 = spi.transfer(0XFF);}digitalWrite(SPI1, HIGH);// read 8 Bytes from TOF Sensorboard ENDWRITE_LCD_TEXT(1, 1, String(value_1)+" "+String(value_2)+" "+String(value_3)+" "+String(value_4));WRITE_LCD_TEXT(1, 2, String(value_5)+" "+String(value_6)+" "+String(value_7)+" "+String(value_8));}
Erklärungen zu diesem Programmbeispiel
Zeile 3 - 10: uint8_t value_1;
Zunächst werden 8 Variablen vom Typ uint8_t definiert, in welchen die vom Sensorboard abgefragten Zahlen abgelegt werden.
Zeile 21 - 38: // read 8 Bytes from TOF Sensorboard BEGIN - END
In diesem Code-Abschnitt sind grundsätzlich keine Änderungen vorzunehmen. Laufend werden über die SPI-Schnittstelle Zahlen von der OpenMV Cam abgefragt. Entspricht in diesem Daten-Stream eine Zahl dem Wert 250, so wird diese als erste Zahl der Neunergruppe identifiziert und alle weiteren werden in den Variablen value1, value2, ...abgelegt.
Zeile 41,42: WRITE_LCD_TEXT(1, 1, String(value_1)+" "+String(value_2)+" "+String(value_3)+" "+String(value_4));
Mit diesen Code-Zeilen werden die Zahlen der Variablen value1, value2, ... am Display angezeigt.