17-01-2019, 15:22
(Ten post był ostatnio modyfikowany: 17-01-2019, 16:22 przez 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ł
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;
}