Problem przy programowaniu platformy jeżdżącej - Jacek1878 - 26-09-2016
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ę.
|