• Witaj na Forum Arduino Polska! Zapraszamy do rejestracji!
  • Znajdziesz tutaj wiele informacji na temat hardware / software.
Witaj! Logowanie Rejestracja


Ocena wątku:
  • 0 głosów - średnia: 0
  • 1
  • 2
  • 3
  • 4
  • 5
Problem przy programowaniu platformy jeżdżącej
#1
Witam,
Arduino zainteresowałem się dokładnie tydzień temu. Jestem teraz na etapie robienia własnej platformy jeżdżącej, jednak mam trochę kłopoty, podejrzewam, że z kodem, bo robot zachowuje się nie tak jak trzeba.

Niestety nie mam pomysłu, co może być źle. Moim zamiarem jest, aby robot poruszał się cały czas do przodu, w momencie gdy przeszkoda znajdzie się w odległości 20 cm lub mniej, powinien się zatrzymać rozejrzeć w trzy strony i pojechać w kierunku gdzie odległość jest największa. Kod wygląda tak:

Kod:
#include <Servo.h>
#include <NewPing.h>

#define buzz 2

//  Silnik A
#define kierunekA 12
#define predkoscA 3

//  Silnik B
#define kierunekB 13
#define predkoscB 11

// Diody z przodu
#define diodaPrzodBiala 4

// Fotorezystor
#define fotorezystor A2

// Odbiornik IR
#define IR 5

// Odleglosciomierz
#define TRIGGER 7  //wejscie
#define ECHO 6      //wyjscie
#define MaxDystans 400

Servo mojeServo;
NewPing sonar(TRIGGER, ECHO, MaxDystans);

void setup() {
 mojeServo.attach(10);
 mojeServo.write(90);
 pinMode(diodaPrzodBiala, OUTPUT);
 pinMode(ECHO, OUTPUT);
 pinMode(TRIGGER, INPUT);
}

int jakaOdleglosc() {
 unsigned int uS = sonar.ping(); //wyslij i odbierz ping (mikrosekundy)
 unsigned int odleglosc = (uS / US_ROUNDTRIP_CM);

 return odleglosc;
}

void stopMaszyna() {
 analogWrite(predkoscA, 0);
 analogWrite(predkoscB, 0);
}

void doPrzodu() {
 digitalWrite(kierunekA, LOW);
 digitalWrite(kierunekB, LOW);
 analogWrite(predkoscA, 245);
 analogWrite(predkoscB, 255);
}

void doTylu() {
 digitalWrite(kierunekA, HIGH);
 digitalWrite(kierunekB, HIGH);
 analogWrite(predkoscA, 180);
 analogWrite(predkoscB, 190);
}

void wLewo() {
 digitalWrite(kierunekA, HIGH);
 digitalWrite(kierunekB, LOW);
 analogWrite(predkoscA, 180);
 analogWrite(predkoscB, 190);
 delay(550);
}

void wPrawo() {
 digitalWrite(kierunekA, LOW);
 digitalWrite(kierunekB, HIGH);
 analogWrite(predkoscA, 180);
 analogWrite(predkoscB, 190);
 delay(550);
}

void doObrotu() {
 digitalWrite(kierunekA, HIGH);
 digitalWrite(kierunekB, HIGH);
 analogWrite(predkoscA, 180);
 analogWrite(predkoscB, 190);
 delay(500);
 digitalWrite(kierunekA, LOW);
 digitalWrite(kierunekB, HIGH);
 analogWrite(predkoscA, 180);
 analogWrite(predkoscB, 190);
 delay(1000);
}

int gdzieMamJechac () {
 int a;
 int b;
 int c;
 int najwieksza;

 mojeServo.write(10);
 delay(200);
 a = jakaOdleglosc();
 delay(100);
 mojeServo.write(90);
 delay(200);
 b = jakaOdleglosc();
 delay(100);
 mojeServo.write(170);
 delay(200);
 c = jakaOdleglosc();
 delay(100);

 if (a > b)
 {
   if (a > c)
   {
     najwieksza = 3;
   } else {
     najwieksza = 1;
   }
 } else {
   if (b > c)
   {
     najwieksza = 2;
   } else {
     najwieksza = 1;
   }
 }
 return najwieksza;
}

void loop() {

 int odczytSwiatla = analogRead(fotorezystor);

 if (odczytSwiatla < 50)
 {
   digitalWrite(diodaPrzodBiala, HIGH);
 }
 else
 {
   digitalWrite(diodaPrzodBiala, LOW);
 }

 mojeServo.write(90);
 delay(200);
 int  ileJeszcze = jakaOdleglosc();

 if (ileJeszcze >= 20) {
   doPrzodu();
 }
 else {
   stopMaszyna();
   int wu = gdzieMamJechac();
   switch (wu)
   {
     case 1:
       wLewo();
     case 2:
       doObrotu();
     case 3:
       wPrawo();
   }
 }
}

Prosiłbym o pomoc i z góry dziękuję. Smile
 
Odpowiedź
  


Skocz do:


Przeglądający: 1 gości