[MajkelAngelo]

Cześć, robię sobie robota na inżynierkę, co śmiga po pomieszczeniu i tworzy mapę jego ścian.

Działa to tak, robot jedzie wzdłuż ścian i co 3 impulsy z enkodera optycznego (MH-Sensor-Series Foto Interruptor; Tarcza 20 otworów -> 20 impulsów na obrót koła) wrzuca na KONIEC LISTY aktualny kąt i aktualną odległość od ściany. 

W czasie wykonywania pętli głównej na bieżąco przesyła dane z POCZĄTKU lity do mojego komputera i usuwa je, żeby zwolnić pamięć. W ten sposób dane są ładnie ustawione w kolejce. Te z przodu są przesyłane i usuwane, a z tyłu są dostawiane nowe.

I w sumie wszystko spoko, bo program działa poprawnie i spełnia swoje zadanie, ale jest dość obszerny i Arduino nie wyrabia się z wykonywaniem przerwań na bieżąco. Z tego co wyczytałem w tej sytuacji ustawiają się one w kolejkę.

Problem pojawia się, kiedy robot dojedzie do rogu ściany. W tym miejscu generuję sztucznie współrzędne punktu odpowiadającego rogowi ściany i wrzucam do kolejki. Niestety niewykonane wcześniej przerwania następują po tym i punkty, które powinny być przed lądują w kolejce później. To rozwala mapę, bo punkty nie są ustawione po kolei.

Próbowałem różnych sposobów, żeby interrupty  wykonały się do końca zanim przejdę dalej. Timerów, delay, for i tak dalej. Sprawdzam nawet czy w kolejce są jeszcze elementy

 while(ListaKaty.size() != 0 && ListaOdl.size() != 0) WyslijKatIOdl();

i to nie pomaga. Jak robot obróci się i pojedzie dalej, dopiero wtedy wcześniejsze punkty ( z zakolejkowanych przerwań) trafiają na listę i  są przesyłane.

Czy jest jakiś sposób, żeby sprawdzać czy są jeszcze przerwania oczekujące na wykonanie i żeby zostały wykonane? Lub jak zrobić, żeby się wykonywały, a program poczeka sobie np. 2 sekundy. Bo akurat czas nie jest tutaj problemem.

wiersze,  o których mówię za-komentowałem //PROBLEM

Jakby trzeba było coś jeszcze wyjaśnić to proszę pytajcie.

Z góry dzięki za wszelką pomoc,
Michał

Kod:

#include <NewPing.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Timers.h>
#include <Wire.h>
#include <MPU6050.h>
#include <LinkedList.h>
#include <math.h>

RF24 radio(7, 8); // CE, CSN
const byte addresses[][6] = {"00001", "00002"};
boolean buttonState = 0;

#define PrawyPrzod 10
#define PrawyTyl 9
#define LewyPrzod 5
#define LewyTyl 6
#define Lewe 0.47
#define Prawe  0.39
#define WspWzmocKata 0.005
#define PrzGoraEchoPin A3
#define PrzDolEchoPin A1
#define TylBokEchoPin A2
#define PrzBokEchoPin A0
#define OdlegloscPrzodOdSciany 25
#define WspWzmSciana 1.5
#define IloscProb 1
#define Dystans 250

Timer RozruchTimer;

NewPing Gora(PrzGoraEchoPin,PrzGoraEchoPin,OdlegloscPrzodOdSciany+5);
NewPing Przod(PrzBokEchoPin,PrzBokEchoPin,OdlegloscPrzodOdSciany+5);
NewPing Tyl(TylBokEchoPin,TylBokEchoPin,45);
NewPing Dol(PrzDolEchoPin,PrzDolEchoPin,45);

bool CzyRozruch;
volatile bool CzyJadePrzyScianie;
int OdlOdSciany;
double OdlegloscPrzod;
double OdlegloscTyl;
int ProbyGora;
int ProbyDol;
int Enkoder[Dystans];
int ZbiorOdleglosci[Dystans];
volatile int LicznikKrokow;
int LicznikWyslania;
int i;
volatile bool Licz;
volatile bool Obrot;
volatile int AktualnyKat;
volatile int AktualnaOdlOdSciany;
String inString = "";
char str2[10];
char str1[10];
char str[10];

LinkedList<volatile int> ListaKaty;
LinkedList<volatile int> ListaOdl;
void setup() {
 radio.begin();
 radio.openWritingPipe(addresses[1]); // 00001
 radio.openReadingPipe(1, addresses[0]); // 00002
 radio.setPALevel(RF24_PA_MAX);
 pinMode (LewyPrzod, OUTPUT);
 pinMode (LewyTyl, OUTPUT);
 pinMode (PrawyPrzod, OUTPUT);
 pinMode (PrawyTyl, OUTPUT);
 CzyRozruch = 1;
 CzyJadePrzyScianie = 0;
 ProbyGora = 0;
 ProbyDol = 0;
 LicznikKrokow = 0;
 LicznikWyslania = 0;
 AktualnyKat = 0;
 RozruchTimer.begin(60);
 Licz = 0;
 Obrot = 0;
attachInterrupt(digitalPinToInterrupt(2), ZliczKrok, FALLING);
Serial.begin(115200);
}

void loop() {
 digitalWrite( LewyPrzod,LOW);
 digitalWrite( PrawyPrzod,LOW);
 digitalWrite( LewyTyl,LOW);
 digitalWrite( PrawyTyl,LOW);
 for(int czekaj = 0; czekaj < 10; czekaj ++){
   delay(2200);
   OdbierzKat();
}
 while(1){
  OdbierzKat();
  WyslijKatIOdl();
  WyslijKatIOdl();
     double Gora = OdlegloscPrzGora();
     double Dol = OdlegloscPrzDol();
     if(Gora > OdlegloscPrzodOdSciany && Dol > OdlegloscPrzodOdSciany){
       if(CzyRozruch) Start();
       if(!CzyJadePrzyScianie) Jazda(0);
       else  JazdaPrzyScianie(0);  
     }
       else{
           Stop();                       //PROBLEM
           for(int k=0; k<200;k++){      //PROBLEM
             WyslijKatIOdl();            //PORBLEM
             delay(10);                  //PROBLEM
           }
           if(CzyJadePrzyScianie){
               while(ListaKaty.size() != 0 && ListaOdl.size() != 0) WyslijKatIOdl();
               double x = OdlegloscPrzodOdSciany;
               double y = AktualnaOdlOdSciany;
               double alfa = atan(x/y);
               ListaKaty.add(-(AktualnyKat+(alfa*180/3.14)));
               double Przekatna = x/sin(alfa);
               if (Przekatna >45) Przekatna = 45;
               ListaOdl.add(Przekatna);
           }
           CzyJadePrzyScianie = 1;
           ObrotPrawo();
           Licz = 1;
           Stop();
       }

 }
}


void Start(){
   digitalWrite(PrawyTyl, LOW);
   digitalWrite(LewyTyl, LOW);
   analogWrite(PrawyPrzod,100);
   analogWrite(LewyPrzod, 120);
   CzyRozruch=0;
   RozruchTimer.restart();
   while(!RozruchTimer.available()){
       if((OdlegloscPrzGora() < OdlegloscPrzodOdSciany) || (OdlegloscPrzDol()< OdlegloscPrzodOdSciany)){
           Stop();
           return;
       }
   }
   return;
}


void Jazda(double Offset){
 digitalWrite(PrawyTyl, LOW);
 digitalWrite(LewyTyl, LOW);
 analogWrite(PrawyPrzod,170*Prawe);
 analogWrite(LewyPrzod, 170*Lewe);
}

int OdlegloscPrzGora() {
 int  dystans ;
 dystans = Gora.ping_median(2)/58;
 if (dystans == 0) dystans = OdlegloscPrzodOdSciany+5;
 delay(5);
 return dystans;
}
int OdlegloscPrzDol() {
 int  dystans;
 dystans = Dol.ping_median(2)/58;
 if (dystans == 0) dystans = OdlegloscPrzodOdSciany+5;
 delay(5);
 return dystans;
}
int TylBokZmierzOdleglosc(int x) {
 Licz = 0;
 int  dystans;
 dystans = Tyl.ping_median(x)/58;
 if (dystans == 0) dystans = 45;
 Licz = 1;
 return dystans;
}

int PrzBokZmierzOdleglosc(int x) {
 int  dystans;
 Licz = 0;
 dystans = Przod.ping_median(x)/58;
 if (dystans == 0) dystans = 45;
 Licz = 1;
 return dystans;
}

void Stop(){
 digitalWrite( LewyPrzod,HIGH);
 digitalWrite( PrawyPrzod,HIGH);
 digitalWrite( LewyTyl,HIGH);
 digitalWrite( PrawyTyl,HIGH);
 WyslijKatIOdl();
 delay(500);
 digitalWrite( LewyPrzod,LOW);
 digitalWrite( PrawyPrzod,LOW);
 digitalWrite( LewyTyl,LOW);
 digitalWrite( PrawyTyl,LOW);
 WyslijKatIOdl();
 CzyRozruch = 1;
}

void ObrotPrawo(){
 
 digitalWrite(PrawyPrzod, LOW);
 digitalWrite(LewyTyl, LOW);
 if((OdlegloscPrzGora() < 15 ) || (OdlegloscPrzDol()< 15)) Cofnij();
 analogWrite (PrawyTyl, 90);
 analogWrite (LewyPrzod,110);
 delay(150);
 analogWrite (PrawyTyl, 50);
 analogWrite (LewyPrzod,70);
 Obrot =1;
 int LicznikObrotu =LicznikKrokow;
 double PrzodOdl;
 double TylOdl;
 double Roznica;
 while(LicznikKrokow-LicznikObrotu <=6);
 do{
 PrzodOdl = PrzBokZmierzOdleglosc(1);
 TylOdl = TylBokZmierzOdleglosc(1);
 Roznica = PrzodOdl - TylOdl;
 Roznica = abs(Roznica);
 }while(2 >= Roznica && PrzodOdl<OdlegloscPrzodOdSciany + 3 && TylOdl<OdlegloscPrzodOdSciany + 3);
 
 Stop();
 Obrot  = 0;
 OdlOdSciany = OdlegloscPrzodOdSciany;
 AktualnaOdlOdSciany = (TylOdl + PrzodOdl)/2;
 LicznikKrokow = 0;
 return;
}





void WyslijDane(int Value){
 
 radio.stopListening();
 radio.write(&Value, sizeof(Value));

}

void JazdaPrzyScianie(double Offset){
 digitalWrite(PrawyTyl, LOW);
 digitalWrite(LewyTyl, LOW);
 double WspOdl = 1;
 double WspRownoleg = 1.1;
 int Predkosc = 170;
 double OdlegloscPrzod = PrzBokZmierzOdleglosc(1);
 double OdlegloscTyl = TylBokZmierzOdleglosc(1);
 if((OdlegloscPrzod+OdlegloscTyl)/2 != 45) AktualnaOdlOdSciany = (OdlegloscPrzod+OdlegloscTyl)/2;
 double KorektaOdleglosci = (OdlOdSciany-AktualnaOdlOdSciany)*WspOdl;
 double KorektaRownolegla = (OdlegloscPrzod-OdlegloscTyl)*WspRownoleg;
 double P = Predkosc*Prawe - KorektaOdleglosci+ KorektaRownolegla;
 if (P<Predkosc*Prawe-10) P=Predkosc*Prawe-10;
 if (P>Predkosc*Prawe+12) P=Predkosc*Prawe+12;
 double L = Predkosc*Lewe + KorektaOdleglosci- KorektaRownolegla;
 if (L<Predkosc*Lewe-12) L=Predkosc*Lewe-12;
 if (L>Predkosc*Lewe+10) L=Predkosc*Lewe+10;
 analogWrite(PrawyPrzod,P);
 analogWrite(LewyPrzod, L);
 
}

void Cofnij(){
   Licz  = 0;
   digitalWrite(PrawyPrzod, LOW);
   digitalWrite(LewyPrzod, LOW);
   if((OdlegloscPrzGora() > OdlegloscPrzodOdSciany) && (OdlegloscPrzDol()> OdlegloscPrzodOdSciany)) return;
   analogWrite(PrawyTyl,75);
   analogWrite(LewyTyl, 95);
   delay(500);
   Stop();
   Licz = 1;
   return;
     
}

void ZliczKrok(){
 if(Licz && CzyJadePrzyScianie) LicznikKrokow++;
 if (LicznikKrokow >= 3 && !Obrot) {
   ListaKaty.add(AktualnyKat);
   ListaOdl.add(AktualnaOdlOdSciany);
   LicznikKrokow = 0;
  }
}

void OdbierzKat(){
 Licz =0;
   Serial.println('$');
   while (Serial.available() > 0) {
     int inChar = Serial.read();
     if (isDigit(inChar)) {
       // convert the incoming byte to a char and add it to the string:
       inString += (char)inChar;
     }
     // if you get a newline, print the string, then the string's value:
     if (inChar == '\n') {
       //Serial.print("Value:");
       //Serial.println(inString.toInt());
       AktualnyKat = inString.toInt();
       
       // clear the string for new input:
       inString = "";
       break;
     }
   }
   Licz = 1;
 return;
}

void WyslijKatIOdl(){
         Licz=0;
         if(ListaKaty.size() != 0 && ListaOdl.size() != 0){
           if(ListaKaty.get(0)<0){
             sprintf(str,"%d",0);
             sprintf(str1, "%d", -ListaKaty.shift());
             sprintf(str2,"%d", ListaOdl.shift());
           }
           else{
             sprintf(str,"%d",1);
             sprintf(str1, "%d", ListaKaty.shift());
             sprintf(str2,"%d", ListaOdl.shift());
           }
           strncat (str,str1,6);
           strncat (str, "   ", 6);
           strncat (str,str2,6);
           delayMicroseconds(10);
           radio.stopListening();
           radio.write(&str, sizeof(str));
         }
     Licz=1;
     return;
       
}