LBotics.at

Soccer Bodensensor

Der KeplerBRAIN V4 Soccer Bodensensor besteht aus zwei Komponenten - der Sensorplatine und einer KeplerBRAIN V4 Stamp. Auf der Sensorplatine befinden sich die Sensoren, auf der KeplerBRAIN V4 Stamp werden die Sensordaten mit einem Mikrocontroller ermittelt und weiter verarbeitet.

Die Sensorplatine

Design und Layout by Janez Rotman 2019

Auf der Sensorplatine befinden sich auf der Unterseite 8 Reflexionssensoren. Gleichzeitig dient die Platine als Grundplatte für den mechanischen Aufbau eines Soccer Roboters. Auf der Oberseite kann über Steckleisten ein KeplerBRAIN V4 Stamp Board aufgesteckt werden. Zusätzlich sind dort zwei Taster und zwei LEDs platziert, welche spezielle Aufgaben in der Firmware übernehmen können.

Die von den Reflexionssensoren gemessenen Intensitäten von reflektiertem Licht werden als Spannungen an analogen Eingängen des Mikrocontrollers gemessen und in digitale Werte umgewandelt.

Die KeplerBRAIN V4 Stamp

Die KeplerBRAIN V4 Stamp ist ein Mikrocontrollerboard im Briefmarkenformat für die die Entwicklung eigener Sensoren innerhalb des KeplerBRAIN V4 Robotik-Systems. Darauf befindet sich der gleiche STM32F411 Mikrocontroller, wie auf dem Mainboard und allen anderen Sensoren. Von diesem Mikrocontroller sind IO-Pins, analoge Eingangspins und I2C-Pins herausgeführt, sodass damit umfangreiche Möglichkeiten für die Entwicklung bereitstehen.

Dieses Board wird mit dem KeplerBRAIN V4 Mainboard über eine der drei SPI Schnittstellen verbunden, über welche die Datenübertragung erfolgt und auch Versorgungspannungen von 3,3V und 5V zu Verfügung gestellt werden. Zusätzlich befindet sich auf dem Board ein sechspoliger Anschluss für den selben Programmieradapter mit dem auch der Mikrocontroller auf dem Mainboard programmiert werden kann.

 

 

Firmware für den Soccer Bodensensor

Der folgende Code ist quasi das Betriebssystem des Sensors. Dieser kann 1:1 so in der Arduino IDE kompilliert und auf das die KeplerBRAIN V4 Stamp übertragen werden. In der Folge ist es aber auch möglich die Firmware zu erweitern, um z. B. die Analyse und weitere Verarbeitung der Sensorwerte direkt am Sensor auszuführen.

Die Werte des von einem ADC (Analog Digital Converter) konvertierten Spannungen der Reflexionssensoren liegen im Bereich zwischen 0 und 1023 und werden im Array adcValues[] abgelegt. Diese werden dem Array spi_data[] zugewiesen und dabei durch 10 dividiert um Werte zu erhalten, die innerhalb des Wertebereichs eines Bytes (Werte zwischen 0 und 255) liegen.

Die Werte im Array spi_data[] werden ü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 zum 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 Soccer Bodensensor V 29.11.2024 ***
// *******************************************************************************
 
// PB1 F  Front
// PC2 FL Front Left
// PC5 FR Front Right
// PC0 L  Left
// PB0 R  Right
// PC3 BL Back Left
// PA1 BR Back Right
// PA0 B  Back
 
// PA15 Transistor LEDs Boden
// PB7 Led left
// PB8 Button left
// PB9 Led right
// PC13 Button right
 
#include "stm32f4xx_hal.h"
 
// ADC1 Handler
ADC_HandleTypeDef hadc1;       
 
// DMA Handler für ADC   
DMA_HandleTypeDef hdma_adc1;      
 
// Array zum Speichern der ADC-Werte
uint32_t adcValues[8];   
 
// SPI Data
volatile uint8_t spi_data[9] = {250,1,2,3,4,5,6,7,8};
volatile uint8_t tx_index = 0;
 
// Funktion zur Initialisierung des ADC mit DMA
void ADC1_Init(void)              
{
  // Takt für ADC1 aktivieren
  __HAL_RCC_ADC1_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();
  __HAL_RCC_GPIOC_CLK_ENABLE();
  __HAL_RCC_DMA2_CLK_ENABLE();  // DMA2 für ADC1
 
  // ADC-Pins initialisieren
  GPIO_InitTypeDef GPIO_InitStruct = {0};
 
  // PC2, PC0, PC3, PB1, PC5, PB0, PA1, PA0 als Analog-Eingänge konfigurieren
  GPIO_InitStruct.Pin = GPIO_PIN_2 | GPIO_PIN_0 | GPIO_PIN_3 | GPIO_PIN_5;  // PC-Pins
  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
 
  GPIO_InitStruct.Pin = GPIO_PIN_1 | GPIO_PIN_0;  // PB-Pins
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
 
  GPIO_InitStruct.Pin = GPIO_PIN_1 | GPIO_PIN_0;  // PA-Pins
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
  // ADC1 konfigurieren
  hadc1.Instance = ADC1;
  hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;  // Prescaler für geringere Taktfrequenz
  hadc1.Init.Resolution = ADC_RESOLUTION_8B;  // 12-Bit-Auflösung
  hadc1.Init.ScanConvMode = ENABLE;  // Scan-Modus aktivieren (mehrere Kanäle)
  hadc1.Init.ContinuousConvMode = ENABLE;  // Kontinuierliche Konvertierung
  hadc1.Init.DiscontinuousConvMode = DISABLE;  // Kein diskontinuierlicher Modus
  hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;  // Keine externe Auslösung
  hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;  // Daten rechtsbündig
  hadc1.Init.NbrOfConversion = 8;  // Anzahl der Kanäle
  hadc1.Init.DMAContinuousRequests = ENABLE;  // DMA-Anforderungen kontinuierlich
  hadc1.Init.EOCSelection = ADC_EOC_SEQ_CONV;  // End-of-Conversion-Sequenz
 
  if (HAL_ADC_Init(&hadc1) != HAL_OK)
  {
    // Fehler bei der Initialisierung
  }
 
  // Konfigurieren der ADC-Kanäle
  ADC_ChannelConfTypeDef sConfig = {0};
 
  sConfig.Channel = ADC_CHANNEL_9;  // PB1 front
  sConfig.Rank = 1;
  sConfig.SamplingTime = ADC_SAMPLETIME_15CYCLES;
  HAL_ADC_ConfigChannel(&hadc1, &sConfig);
 
  sConfig.Channel = ADC_CHANNEL_12;  // PC2 front left
  sConfig.Rank = 2;
  HAL_ADC_ConfigChannel(&hadc1, &sConfig);
 
  sConfig.Channel = ADC_CHANNEL_15;  // PC5 front right
  sConfig.Rank = 3;
  HAL_ADC_ConfigChannel(&hadc1, &sConfig);
 
  sConfig.Channel = ADC_CHANNEL_10;  // PC0 left
  sConfig.Rank = 4;
  HAL_ADC_ConfigChannel(&hadc1, &sConfig);
 
  sConfig.Channel = ADC_CHANNEL_8;  // PB0 right
  sConfig.Rank = 5;
  HAL_ADC_ConfigChannel(&hadc1, &sConfig);
 
  sConfig.Channel = ADC_CHANNEL_13;  // PC3 back left
  sConfig.Rank = 6;
  HAL_ADC_ConfigChannel(&hadc1, &sConfig);
 
  sConfig.Channel = ADC_CHANNEL_1;  // PA1 back right
  sConfig.Rank = 7;
  HAL_ADC_ConfigChannel(&hadc1, &sConfig);
 
  sConfig.Channel = ADC_CHANNEL_0;  // PA0 back
  sConfig.Rank = 8;
  HAL_ADC_ConfigChannel(&hadc1, &sConfig);
 
  // DMA für ADC konfigurieren
  hdma_adc1.Instance = DMA2_Stream0;  // STM32F411RE verwendet DMA2 Stream 0 für ADC1
  hdma_adc1.Init.Channel = DMA_CHANNEL_0;
  hdma_adc1.Init.Direction = DMA_PERIPH_TO_MEMORY;
  hdma_adc1.Init.PeriphInc = DMA_PINC_DISABLE;
  hdma_adc1.Init.MemInc = DMA_MINC_ENABLE;
  hdma_adc1.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;
  hdma_adc1.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;
  hdma_adc1.Init.Mode = DMA_CIRCULAR;  // Zirkulärer Modus für kontinuierliche Konvertierung
  hdma_adc1.Init.Priority = DMA_PRIORITY_HIGH;
  hdma_adc1.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
 
  if (HAL_DMA_Init(&hdma_adc1) != HAL_OK)
  {
    // Fehler bei der DMA-Initialisierung
  }
 
  // Verknüpfen des DMA-Handles mit ADC
  __HAL_LINKDMA(&hadc1, DMA_Handle, hdma_adc1);
 
  // Start des ADC mit DMA
  HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adcValues, 8);
}
 
// Initialisierung der Systemuhr für STM32F411RE
void SystemClock_Config(void)
{
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
 
  // Konfigurieren des Oszillators
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLM = 16;
  RCC_OscInitStruct.PLL.PLLN = 336;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4;
  RCC_OscInitStruct.PLL.PLLQ = 4;
 
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    // Fehler bei der Oszillator-Konfiguration
    while (1);
  }
 
  // Initialisieren des Taktsystems
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
 
  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    // Fehler bei der Taktkonfiguration
    while (1);
  }
}
 
 
void setup()
{
  SPI1_Init();                  // Initialize SPI1 as Slave with interrupt    
  NVIC_EnableIRQ(SPI1_IRQn);    // Enable the SPI1 interrupt in NVIC
  HAL_Init();                   // HAL Initialisierung
  SystemClock_Config();         // Systemuhr konfigurieren
  ADC1_Init();                  // ADC und DMA initialisieren
 
  pinMode(PA15,OUTPUT);   // Transistor LEDs Boden
  digitalWrite(PA15,LOW); // LEDs Boden LOW:ein HIGH:aus
  pinMode(PB7,OUTPUT);    // LED links
  digitalWrite(PB7, LOW); // LED links aus
  pinMode(PB9,OUTPUT);    // LED rechts
  pinMode(PB8,INPUT);     // Taster links
  pinMode(PC13,INPUT);    // Taster rechts
 
  // User Code
  //Serial.begin(115200);
}
 
void loop() 
{
  spi_data[1] = adcValues[0] / 10;
  spi_data[2] = adcValues[1] / 10;
  spi_data[3] = adcValues[2] / 10;
  spi_data[4] = adcValues[3] / 10;
  spi_data[5] = adcValues[4] / 10;
  spi_data[6] = adcValues[5] / 10;
  spi_data[7] = adcValues[6] / 10;
  spi_data[8] = adcValues[7] / 10;
 
  // User Code
  /*
  Serial.print(adcValues[0]);Serial.print(" ");
  Serial.print(adcValues[1]);Serial.print(" ");
  Serial.print(adcValues[2]);Serial.print(" ");
  Serial.print(adcValues[3]);Serial.print(" ");
  Serial.print(adcValues[4]);Serial.print(" ");
  Serial.print(adcValues[5]);Serial.print(" ");
  Serial.print(adcValues[6]);Serial.print(" ");
  Serial.println(adcValues[7]);
  */
}
 
// Initialize SPI1 in slave mode with interrupt
void SPI1_Init() 
{
  // Enable clocks for GPIOA and SPI1
  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
  RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
 
  // Configure PA4 (NSS), PA5 (SCK), PA6 (MISO), PA7 (MOSI) as alternate function
  GPIOA->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 slave
  SPI1->CR1 = 0;                 // Reset CR1
  SPI1->CR1 &= ~SPI_CR1_MSTR;    // Set as slave
  SPI1->CR1 &= ~SPI_CR1_SSM;     // Disable software slave management
  SPI1->CR1 |= SPI_CR1_SPE;      // Enable SPI
  SPI1->CR2 |= SPI_CR2_RXNEIE;   // Enable RXNE interrupt
  SPI1->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 first
    if (SPI1->SR & SPI_SR_RXNE)
    {
      uint8_t rx_value = SPI1->DR;  
      //Serial.println(rx_value);
    }
      // Check if TXE (Transmit Empty) flag is set, but only if not busy
    if (SPI1->SR & SPI_SR_TXE)
    {
        // Send data from tx_buffer if in "send mode"
        if (tx_index < 9)
        {
          // Write data to SPI
          SPI1->DR = spi_data[tx_index];  
          tx_index++;
        }
        else
        {
          // Reset buffer index
          tx_index = 0;                      
        }
    }
}
Programmbeispiel - Einlesen von Werten des Soccer Bodensensors

Der Bodensensor wird an die SPI1 Schnittstelle eines KeplerBRAIN V4 Mainboards angeschlossen. Am Display sollen laufend die aktuellen, gemessenen Werte der 8 Reflexionssensoren 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 Soccer Bodensensor
 
  digitalWrite(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 Soccer Bodensensor
 
  WRITE_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 Soccer Bodensensor 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.