Radarowy Sharky

Typ_projektu

ElektronikaPL

Zdjecie główne

Krótki opis projektu

Nasz robot skanuje okoliczny teren z użyciem dwóch obracających się sonarów, a odczytane dane wysyła do komputera przez port szeregowy.

Projekt zawiera dwie płytki - esp32 Cam oraz nanoch32v00x. Z powodu ograniczonej liczby pinów na esp32 ch32 jest używane jako narzędzie do odczytu pomiarów z dalmierzy, które następnie są wysyłane UARTem do esp32, które już może komunikować się z komputerem.

Niezbędne elementy

1. Płytka ESP32 Cam

2. Płytka nanoCH32V00X

3. Serwomechanizm x2

4. Dalmierz ultradźwiękowy x2

Sprzęt

Drukarka 3D

Klej na gorąco

Żyłka wędkarska bądź nitka

Opis projektu

Cel projektu

Głównym celem projektu jest skomunikowanie mikrokontrolerów nanoCH32v003 z ESP32CAM, uwzględniając ograniczenia związane z wykorzystaniem ESP do sterowania robotem Sharky, a następnie stworzenie z robota platformy rozwojowej, która za pomocą radarów zamontowanych na serwomechanizmach jest w stanie mapować otoczenie i potencjalnie wykorzystać to do nawigacji w terenie.

Architektura układu

Do wykonania działającego układu ruchomych radarów potrzeba:

2 radarów ultradźwiękowych HC-SR04

2 serwomotorów SG90

Płytki stykowej

Płytki nanoCH32v003

Kabelków m-m i m-ż

Schemat podłączenia elementów prezentuje się następująco:

schemat układu

Do zamontowania tworzonego układu na robocie warto wykorzystać załączoną w oddzielnym pliku podstawkę na serwomechanizmy, którą można wydrukować na drukarce 3D. Serwa do obudowy można przykleić klejem na gorąco. Gotowy układ na robocie może wyglądać np. tak:

obraz robota z przyklejonymi serwami

Komunikacja nanoCH32v003 – ESP32CAM

Ze względu na ograniczoną liczbę wolnych pinów w ESP32CAM (tylko dwa, reszta wykorzystywana jest do obsługi robota) mikrokontroler nanoCH32v003 działa jak czujnik pomiarowy – obsługuje oba czujniki HC-SR04, przelicza ich odczyty na centymetry a następnie przesyła te informacje do ESP32CAM. Komunikacja odbywa się za pomocą protokołu UART, przy czym, jako że jest ona jednostronna (tylko przesyłanie odczytu czujników, nie ma potrzeby wysyłania żadnych informacji w drugą stronę) wykorzystany jest tylko jeden kabel łączący pin TX na nanoCH32v003 z pinem RX na ESP32CAM. Od strony programu komunikacja odbywa się za pomocą komend Serial.print() i Serial.read() z tym, że w ESP32CAM dostępny pin RX należy do drugiego bloku UART (pierwszy służy do komunikacji z komputerem a jego piny nie są w tym przypadku dostępne) używane są komendy Serial2.print() i Serial2.read().

Realizacja skanowania terenu

Robot radzi sobie dobrze ze skanowaniem otoczenia w odległości nie większej niż 2 metry. Dane wypisuje do serial monitora, skąd można je skopiować i wyświetlić w formie graficznej w programie zewnętrznym. Na załączonych zdjęciach widać, że łatwo można powiedzieć, który z obiektów ze zdjęcia pokazuje się na obrazie wygenerowanym z pomiarów.

zdjęcie otoczenia robota mapa otoczenia na podstawie pomiaru

U góry zdjęcie skanowanego terenu, poniżej obraz utworzony ze sczytanych odległości i kątów w oddzielnym programie. Na biało punkty wykryte przez prawy czujnik, na czarno te wykryte przez lewy.

Możliwości rozwoju

W kolejnych etapach zachęcamy wykonującego do próby połączenia możliwości jezdnych robota z możliwością mapowania terenu. Nie będzie to proste zadanie gdyż uwzględnić trzeba będzie potrzeby prądowe silników kół oraz całego układu radarowego. Można popracować też nad algorytmami interpretującymi odczyty z radarów, które będą obrabiały otrzymywane wyniki (np. filtrowały szumy).

Zdjęcia

kod programu

// Kod do esp32:

float pingpong(float x, float a);

void setup() {
  // put your setup code here, to run once:

  //pinMode(15, OUTPUT);
  pinMode(14, OUTPUT);
  Serial.begin(4800);
  Serial2.begin(4800, SERIAL_8N1, 15, 16);
  pinMode(16, OUTPUT);
}

void loop() {

  // put your main code here, to run repeatedly:
  digitalWrite(14, 0);
  static uint32_t czasMem = millis();
  static uint32_t pwmStop = 0;

  static double t = 0;
  float increment = 0.02;

  //if (t >= 1) t = 0;
  int nasycenie = t * 1000 + 1000;
  //Serial.println(t);
  static int input = 1000;
  static int count = 0;
  static double kat = 0;
  static int odczytL = 0;
  static int odczytR = 0;

  char in1 = Serial2.read();
  if (in1 == 'L') {
    odczytL = 0;
    odczytR = 0;
    kat = pingpong(t,1)*180;
    while (1) {
      static char tryb = 'L';
      static int i = 2;
      if(in1 == 'R'){
        tryb = 'R';
        i = 2;
      }
      if(in1 == 'L'){
        tryb = 'L';
        i = 2;
      }

      if(in1 != 255 && in1 != -1) {
        //Serial.print(in1);
        if(in1 >= '0' && in1 <= '9'){
          if(tryb == 'L')odczytL += (in1-'0')*pow(10,i);
          if(tryb == 'R')odczytR += (in1-'0')*pow(10,i);
          i--;
        }
      }
      in1 = Serial2.read();
      if(in1 == '\n') break;
    }
    Serial.print(' ');
    Serial.print(odczytL);
    Serial.print(' ');
    Serial.print(odczytR);
    Serial.print(' ');
    Serial.println(kat);

    digitalWrite(16, 1);
    delayMicroseconds(pingpong(t, 1) * 2000 + 500);
    digitalWrite(16, 0);
    t += increment;
  }
}

float pingpong(float x, float a) {
  int count = int(x / a);
  x = x - count * a;
  if (count % 2) x = a - x;
  return x;
}

// Kod do ch32:

#include <Arduino.h>
//#include <Serial.h>

extern "C" {
  #include "ch32v00x.h"
}

#define LED_PIN PD6

#define SERWO1_PIN PD4
#define SERWO2_PIN PD6

#define OLED_ADDR 0x3C
#define SCL_PIN PC2
#define SDA_PIN PC1

// HC-SR04
#define TRIG_PORT GPIOD
#define TRIG_GPIO GPIO_Pin_2   // PD2 / D2

#define ECHO_PORT GPIOD
#define ECHO_GPIO GPIO_Pin_3   // PD3 / D3

#define TRIG_GPIO2 GPIO_Pin_0  // PD0 / D0

#define ECHO_GPIO2 GPIO_Pin_1   // PD1 / D1

// Korekcja pomiaru.
#define DIVIDER 2

void HCSR04_Config(void) {
  GPIO_InitTypeDef GPIO_InitStructure = {0};
  GPIO_InitTypeDef GPIO_InitStructure2 = {0};

  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE);

  // TRIG jako wyjście
  GPIO_InitStructure.GPIO_Pin = TRIG_GPIO;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(TRIG_PORT, &GPIO_InitStructure);

  GPIO_InitStructure2.GPIO_Pin = TRIG_GPIO2;
  GPIO_InitStructure2.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_InitStructure2.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(TRIG_PORT, &GPIO_InitStructure2);

  GPIO_ResetBits(TRIG_PORT, TRIG_GPIO);
  GPIO_ResetBits(TRIG_PORT, TRIG_GPIO2);

  // ECHO jako wejście pływające
  GPIO_InitStructure.GPIO_Pin = ECHO_GPIO;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(ECHO_PORT, &GPIO_InitStructure);

  GPIO_InitStructure2.GPIO_Pin = ECHO_GPIO2;
  GPIO_InitStructure2.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(ECHO_PORT, &GPIO_InitStructure2);
}

uint16_t HCSR04_ReadDistance_cm(uint16_t trig, uint16_t echo) {
  uint16_t high_count = 0;
  uint8_t echo_started = 0;

  // TRIG LOW
  GPIO_ResetBits(TRIG_PORT, trig);
  delayMicroseconds(5);

  // Impuls TRIG
  GPIO_SetBits(TRIG_PORT, trig);
  delayMicroseconds(15);
  GPIO_ResetBits(TRIG_PORT, trig);

  // Czekamy na impuls ECHO i mierzymy jego długość próbkami.
  // Pętla trwa ok. 60 ms.
  for (uint16_t i = 0; i < 6000; i++) {
    if (GPIO_ReadInputDataBit(ECHO_PORT, echo) == SET) {
      echo_started = 1;

      if (high_count < 999) {
        high_count++;
      }
    } else {
      // Jeżeli impuls już był i się skończył, kończymy pomiar
      if (echo_started) {
        break;
      }
    }

    delayMicroseconds(10);
  }

  if (high_count == 0) {
    return 0;
  }

  // high_count jest proporcjonalny do czasu trwania ECHO.
  // DIVIDER trzeba dobrać praktycznie dla tej płytki/core.
  return high_count / DIVIDER;
}
void ustawSerwo(char serwo, double kat){
  if (kat > 135)kat=135;
  if(kat < -45)kat= -45;
  if(serwo == 'L'){
    digitalWrite(SERWO1_PIN, 1);
    delayMicroseconds(500 + (kat+45)/180*2000);
    digitalWrite(SERWO1_PIN, 0);
  }
  if(serwo == 'R'){
    digitalWrite(SERWO2_PIN, 1);
    delayMicroseconds(500 + (kat+45)/180*2000);
    digitalWrite(SERWO2_PIN, 0);
  }
}

void setup() {
  pinMode(LED_PIN, OUTPUT);

  Serial.begin(4800);

  HCSR04_Config();

}

void loop() {
  static uint32_t czasMem = millis();

  if(millis()-czasMem > 20){
  uint16_t distance_cm = HCSR04_ReadDistance_cm(TRIG_GPIO, ECHO_GPIO);
  uint16_t distance_cm2 = HCSR04_ReadDistance_cm(TRIG_GPIO2, ECHO_GPIO2);
  char msg[10];
  msg[0] = 'L';
  msg[1] = '0' + distance_cm/100;
  msg[2] = '0' + (distance_cm%100)/10;
  msg[3] = '0' + (distance_cm)%10;
  msg[4] = 'R';
  msg[5] = '0' + distance_cm2/100;
  msg[6] = '0' + (distance_cm2%100)/10;
  msg[7] = '0' + (distance_cm2)%10;
  msg[8] = '\n';
  for(int i = 0; i < 9; i++)
  Serial.write(msg[i]);