Skocz do zawartości

Arduino Mega 2560 + HC-06. Problem z HC-06.


Bazuka1988

Pomocna odpowiedź

Dzień dobry Wszystkim!

Tak w skrócie, mój robot ma się poruszać po wyznaczonej ścieżce i robić mapę otoczenia wraz z pokazywaniem swojej pozycji. Mam problem z komunikacją, HC-06 mam podłączony do Arduino Mega 2560 przez RX,TX. Wysyłam dane z czujników optycznych odnośnie pozycji (pozycja już wyliczana w Arduino). Przesyłam 2 zmienne po sobie x1, y1. Arduino łączy się w 1-2 sekundy z moim telefonem, na telefonie używam Serial Bluetooth Terminal i odczyty z Arduino sa odbierane bez problemu (wysyłam około 4-6 danych/sekundę). Podczas łączenia się z komputerem trwa to 30sekund, a czasem 1-2minuty. Po stronie PC mam Bluetootha 4.0 Asusa USB-BT400 (pisze że jest kompatybilny z wersjami 2.0, 2.1, 3.0 no i 4.0... jest BLE (Low Energy). Używam do tego skryptu napisanego w Matlabie.

Ta część odpowiada za samo połączenie. Działa gdyż w ustawieniach windowsa w sekcji bluetooth widzę HC-06 i mam sparowane i połączone.

delete(instrfind) %zamkniecie otwartych portow w Matlabie
clear all
instrhwinfo('Bluetooth','HC-06')
bt = Bluetooth('HC-06', 1)
fopen(bt);

Kolejna część kodu zajmuje się odczytem przesyłanych danych i prostym rysowaniem.
 

tic;
figure(1);
x=0;
xn=0;
y=0;
yn=0;

while (true)
x=fscanf(bt,'%f');   %odczyt impulsow z lewego kola
y=fscanf(bt,'%f');   %odczyt impulsow z prawego kola

xn=xn+x;
yn=yn+y;

plot(xn,yn,'black*')
axis auto
hold on
end

 

Dodam że czasem działa, ale głownie mam błąd z przekroczeniem czasu. Co może być przyczyną że jest taki problem z połączeniem? Poniżej screen z 2 kółek... Dzisiaj udało mi się nawiązać tylko 2 razy połączenie, a z telefonem śmiga. Tylko że na telefonie nie mam takich możliwości z odbieraniem danych i rysowaniem ich. A może jest jakaś apka na androida co by mi rysowała położenie mojego robota?
1756788764_Beztytuu.thumb.jpg.f53beb3727ea98e43eabb1f799f8ed70.jpg

Na koniec kod z Arduino. Trzymanie się lini i sterowanie silnikami wykonuje na niezależnym Arduino Nano. Próbowałem wykonywać i śledzenie linii, PID'a i odczyt z enkoderów, a także obsługę 3 czujników HC-SR04 na jednej platformie Arduino MEga2560, ale nie ogarniał (a może to mój nie zoptymalizowany kod?). Robiłem to przerwaniach, wątkach itd. Stanęło na 2 Arduino jak teraz, choć zastanawiam się nad platformą ESP32, nie wiem czy by nie było łatwiej. Proszę o pomoc.

//czujniki lini
#define L2_LINE_SENSOR A0
#define R2_LINE_SENSOR A1

//czujniki szczelinowe
#define L_SZCZ 2
#define P_SZCZ 3

//silniki
#define LMOTOR 9
#define RMOTOR 8
#define IN1 4
#define IN2 5
#define IN3 6
#define IN4 7

//czujniki ultradzwiekowe
#define SechoHSCR04 51
#define StrigHSCR04 49
#define PechoHSCR04 50
#define PtrigHSCR04 48
#define LechoHSCR04 42
#define LtrigHSCR04 40

#include <Stream.h>
#include <SoftwareSerial.h>
SoftwareSerial mySerial(17, 16); // RX, TX
#include <Wire.h>
#include <Timers.h>//watki
Timers<1>akcja;//watki

volatile float impL=0, impP=0, vx1=0, vy1=0;   //"impL","impP"-impulsy z enkoderow;
float x1=0, y1=0, r=3.25, teta=0, tetaKat=0, tetaV=0, d=6.5, l=0, L=13.30, dystansL=0, dystansP=0, dystansS=0;  //"r"-promien kola; "L"-odleglosc pomiedzy kolami mierzona od osi kola
int licznik=0;
int blad=0, g=200, vstart=40, pochodna=0, poprzedni=0, calka=0, PID=0; //"g"-granica widzialnosci czarnej lini; "vstart"-predkosc stala
float Kp=8, Kd=0.1, Ki=0.4; //nastawy regulatora PID
float Ldystans=0, Sdystans=0, Pdystans=0;
float doMetra=0, prawo90=0; 
unsigned long s=0, start=0, time=0, aktualnyCzas=0, zapamietanyCzas=0, roznicaCzasu=0;//timery
String inString = "";

void setup() {
  
//Zasilanie +++
  pinMode (47, OUTPUT);//ultradzwiekowy srodek
  digitalWrite(47, HIGH);
  pinMode (46, OUTPUT);//ultradzwiekowy prawy
  digitalWrite(46, HIGH);
  pinMode (38, OUTPUT);//ultradzwiekowy lewy
  digitalWrite(38, HIGH);
  pinMode (22, OUTPUT);//szczelinowy prawy
  digitalWrite(22, HIGH);
  pinMode (23, OUTPUT);//szczelinowy lewy
  digitalWrite(23, HIGH);
  
//Masa ---
  pinMode (53, OUTPUT);//ultradzwiekowy srodek
  digitalWrite(53, LOW);
  pinMode (52, OUTPUT);//ultradzwiekowy prawy
  digitalWrite(52, LOW);
  pinMode (44, OUTPUT);//ultradzwiekowy lewy
  digitalWrite(44, LOW);

//--------------Czujniki optyczne
  pinMode(L_SZCZ, INPUT);
  pinMode(P_SZCZ, INPUT);
  l=d*PI;
  
//Silniki
  pinMode (LMOTOR, OUTPUT);//lewy
  pinMode (IN1, OUTPUT);//przod
  pinMode (IN2, OUTPUT);//tyl
  pinMode (RMOTOR, OUTPUT);//prawy
  pinMode (IN3, OUTPUT);//przod
  pinMode (IN4, OUTPUT);//tyl
  analogWrite(LMOTOR, 0); //Ustawienie predkosci  
  digitalWrite(IN1, HIGH); //Kierunek: do przodu
  digitalWrite(IN2, LOW); //Kierunek: do tylu
  analogWrite(RMOTOR, 0); //Ustawienie predkosci
  digitalWrite(IN3, HIGH); //Kierunek: do przodu
  digitalWrite(IN4, LOW); //Kierunek: do tylu
  
//Czujniki odbiciowe
  pinMode(R2_LINE_SENSOR, INPUT);
  pinMode(L2_LINE_SENSOR, INPUT);

//Czujniki ultradzwiekowe
  pinMode(SechoHSCR04, INPUT);
  pinMode(StrigHSCR04, OUTPUT);
  pinMode(LechoHSCR04, INPUT);
  pinMode(LtrigHSCR04, OUTPUT);
  pinMode(PechoHSCR04, INPUT);
  pinMode(PtrigHSCR04, OUTPUT);

    mySerial.begin(9600);
    Serial.begin(9600);
    s=millis();
    start=millis();
    time = millis();
//------------Watki------------

    //akcja.attach(0, 50, wysylanie);
    akcja.attach(0, 250, pozycja);
    //akcja.attach(1, 980, hcsr04lewy);
    //akcja.attach(2, 990, hcsr04srodek);
    //akcja.attach(3, 1000, hcsr04prawy);
    //akcja.attach(4, 2000, stopMotors);


//--------------Przerwania na enkoderach kol---------------------
    attachInterrupt(digitalPinToInterrupt(2), enkoderL, LOW); 
    attachInterrupt(digitalPinToInterrupt(3), enkoderP, LOW);
}

//********************************************
//*****************FUNKCJE********************
//********************************************

//-----------------Wysylanie pozycji z arduinoNano przez BT 
void wysylanie()
  {
     if (Serial.available() > 0) {
    int inChar = Serial.read();

    if (inChar != '\n') {
      inString += (char)inChar;
    }
    else {
      Serial.println(inString.toFloat());
      inString = "";
    }
  }
  }

//----------------Enkodery kol
void enkoderL()
{
 if ((millis() - time) > 5)
 impL++;

 time = millis();
}
 
void enkoderP()
{
 if ((millis() - time) > 5)
 impP++;
   
 time = millis();
}

//----------------Pozycja pojazdu
void pozycja() {
  
  //droga przejechana przez kazde z kol
  dystansL=(impL/20)*l; 
  dystansP=(impP/20)*l;

  //kata o jaki obrocil sie robot
  tetaKat=tetaKat+((dystansP-dystansL)/L);

  //pozycja
  dystansS=(dystansP+dystansL)/2;
  x1=dystansS*(cos(tetaKat));
  y1=dystansS*(sin(tetaKat));
  //Serial.print("x1: ");
  mySerial.println(x1);
  //Serial.print("y1: ");
  mySerial.println(y1);
  //reset licznikow
  impL=0;
  impP=0; 
  
}

//---------------Jazda prosto 1 metr
void jedenMetr () {
    if (doMetra<100) {
  dystansL=(impL/20)*l; 
  dystansP=(impP/20)*l;
  dystansS=(dystansP+dystansL)/2;
          analogWrite(RMOTOR, 70); //Ustawienie predkosci
          digitalWrite(IN3, HIGH); //Kierunek: do przodu
          analogWrite(LMOTOR, 70); //Ustawienie predkosci 
          digitalWrite(IN1, HIGH); //Kierunek: do przodu

          doMetra=dystansS+doMetra;
            //reset licznikow
  impL=0;
  impP=0;
  Serial.println(doMetra);
  } else if (doMetra>=100) {
    
          analogWrite(RMOTOR, 0); //Ustawienie predkosci
          digitalWrite(IN3, LOW); //Kierunek: do przodu
          analogWrite(LMOTOR, 0); //Ustawienie predkosci 
          digitalWrite(IN1, LOW); //Kierunek: do przodu
  }

}
//---------------Skręt w prawo 90 stopni
void wPrawo90 () {
    if (prawo90<20.88) {
  dystansL=(impL/20)*l; 
          analogWrite(RMOTOR, 0); //Ustawienie predkosci
          digitalWrite(IN3, LOW); //Kierunek: do przodu
          analogWrite(LMOTOR, 70); //Ustawienie predkosci 
          digitalWrite(IN1, HIGH); //Kierunek: do przodu

          prawo90=dystansL+prawo90;
            //reset licznikow
  impL=0;
  impP=0;
  Serial.println(prawo90);
  } else if (prawo90>=20.88) {
    
  analogWrite(LMOTOR, 0); //Ustawienie predkosci
  analogWrite(RMOTOR, 0); //Ustawienie predkosci
  digitalWrite(IN1, LOW);
  digitalWrite(IN3, LOW);
  }

}
//---------------Pomiar odleglosci czujnika srodkowego
void hcsr04srodek () {
    float czas;
    digitalWrite(StrigHSCR04, LOW);
    delayMicroseconds(2);
    digitalWrite(StrigHSCR04, HIGH);
    delayMicroseconds(10);
    digitalWrite(StrigHSCR04, LOW);
    czas = pulseIn(SechoHSCR04, HIGH);
    Sdystans = (czas / 58);
    //delay(1000);
    //Serial.print("\nOdleglosc-srodek: ");
    mySerial.println(Sdystans);
}

//---------------Pomiar odleglosci czujnika lewego
void hcsr04lewy () {
    float czas;
    digitalWrite(LtrigHSCR04, LOW);
    delayMicroseconds(2);
    digitalWrite(LtrigHSCR04, HIGH);
    delayMicroseconds(10);
    digitalWrite(LtrigHSCR04, LOW);
    czas = pulseIn(LechoHSCR04, HIGH);
    Ldystans = (czas / 58);
    //delay(1000);
    //Serial.print("\nOdleglosc-lewy: ");
    mySerial.println(Ldystans);

}

//---------------Pomiar odleglosci czujnika prawego
void hcsr04prawy () {
    float czas;
    digitalWrite(PtrigHSCR04, LOW);
    delayMicroseconds(2);
    digitalWrite(PtrigHSCR04, HIGH);
    delayMicroseconds(10);
    digitalWrite(PtrigHSCR04, LOW);
    czas = pulseIn(PechoHSCR04, HIGH);
    Pdystans = (czas / 58);
    //delay(1000);
    //Serial.print("\nOdleglosc-prawy: ");
    mySerial.println(Pdystans);
}

//---------------------Wyliczenie bledu
 int error () {
   if (analogRead(L2_LINE_SENSOR) < g && analogRead(R2_LINE_SENSOR) < g) { //nie widza lini
    blad=1;
  } 
    else if (analogRead(L2_LINE_SENSOR) > g && analogRead(R2_LINE_SENSOR) < g) { //lewy widzi linie
    blad=2; 
  } else if (analogRead(L2_LINE_SENSOR) < g && analogRead(R2_LINE_SENSOR) > g) { //prawy widzi linie
    blad=-2;
  }
  //Serial.print("blad: ");
  //Serial.println(blad);
  return blad;
  } 

//-----------------------------PID
void regPID () {
   pochodna=blad-poprzedni;
   poprzedni=blad-poprzedni;
   calka=constrain(calka+blad, -50, 50);
   PID=round(abs((Kp*blad)+(Kd*pochodna)+(Ki*calka)));
   //Serial.print("PID: ");
   //Serial.println(PID);
    if (blad == 1) {//jazda prosto
    leftMotor(vstart); 
    rightMotor(vstart);
    
  } else if (blad == 2) { //lewy widzi linie
    leftMotor(vstart-PID); 
    rightMotor(vstart+PID); 
    
  } else if (blad == -2) {  //prawy widzi linie
    leftMotor(vstart+PID); 
    rightMotor(vstart-PID);
     
  }
 }
 
//-------------------Sterowanie silnikami
void leftMotor(int V) {
  if (V>0) {
  //V=constrain(V, 0, 256);
  analogWrite(LMOTOR, V); //Ustawienie predkosci
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  } else {  //stop
    //V=abs(V);
    analogWrite(LMOTOR, 0); //Ustawienie predkosci
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
  }
}

void rightMotor(int V) {
  if (V>0) { 
  //V=constrain(V, 0, 256);
  analogWrite(RMOTOR, V); //Ustawienie predkosci
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
 
  } else {   //stop
    //V=abs(V);
    analogWrite(RMOTOR, 0); //Ustawienie predkosci
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
  }
}

void stopMotors() {
  analogWrite(LMOTOR, 0); //Ustawienie predkosci
  analogWrite(RMOTOR, 0); //Ustawienie predkosci
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  delay(1000);
}

//--------------PETLA GLOWNA--------------
void loop() {

  //jedenMetr();
  //wPrawo90();

  
   //akcja.process();
   //aktualnyCzas = millis();
   //roznicaCzasu = aktualnyCzas - zapamietanyCzas;
    
    //error();
    //regPID();
    delay(200);
    pozycja();
  //if (roznicaCzasu >= 250UL) {
   //zapamietanyCzas = aktualnyCzas;
  //pozycja();
  //hcsr04lewy();
  //hcsr04srodek();
  //hcsr04prawy();
  //}
 
  /*
  float czas=millis();
  Serial.println(analogRead(R2_LINE_SENSOR));
  delay(1000);
  Serial.print("  ,  ");
  Serial.println(czas/1000);
  */

}

 

Link do komentarza
Share on other sites

23 godziny temu, Bazuka1988 napisał:

A może jest jakaś apka na androida co by mi rysowała położenie mojego robota?

Możesz ją sobie sam napisać np. w Qt - Android - https://forbot.pl/blog/kurs-qt-3-pierwsza-aplikacja-mobilna-na-androida-id35602, Bluetooth - https://forbot.pl/blog/kurs-qt-4-aplikacja-mobilna-lacznosc-bluetooth-z-arduino-id35603 a do wykresów możesz wykorzystać: QtCharts https://doc.qt.io/qt-5/qtcharts-index.html albo np. QCustomPlot - https://www.qcustomplot.com/index.php/introduction pozwoli Ci to na przesyłanie znacznie częściej danych niż 4-6x na sekundę.

 

  • Lubię! 1
Link do komentarza
Share on other sites

Dołącz do dyskusji, napisz odpowiedź!

Jeśli masz już konto to zaloguj się teraz, aby opublikować wiadomość jako Ty. Możesz też napisać teraz i zarejestrować się później.
Uwaga: wgrywanie zdjęć i załączników dostępne jest po zalogowaniu!

Anonim
Dołącz do dyskusji! Kliknij i zacznij pisać...

×   Wklejony jako tekst z formatowaniem.   Przywróć formatowanie

  Dozwolonych jest tylko 75 emoji.

×   Twój link będzie automatycznie osadzony.   Wyświetlać jako link

×   Twoja poprzednia zawartość została przywrócona.   Wyczyść edytor

×   Nie możesz wkleić zdjęć bezpośrednio. Prześlij lub wstaw obrazy z adresu URL.

×
×
  • Utwórz nowe...

Ważne informacje

Ta strona używa ciasteczek (cookies), dzięki którym może działać lepiej. Więcej na ten temat znajdziesz w Polityce Prywatności.