Problem z czujnikiem odległości, robota sterowanego pilotem

[JacekKarnowka]

Witam.

Programuję właśnie robota sterowanego pilotem, zapisałem to w osobnej funkcji, nie w "loop". Zasada działania ma być bardzo prosta. Robot jeździ zgodnie z poleceniami pilota, jednak po zbliżeniu się do przeszkody (odległość mniejsza niż 30 centymetrów), wykrytej przez czujnik odległości znajdujący się z przodu pojazdu, buzzer ma wydać dźwięk.

Siedzę już na tym trochę, nie umiem znaleźć błędu.

Dziękuję bardzo za pomoc.

#include <RC5.h>
#include <Servo.h>
#define PWM_MAX 165
#define L_PWM 5
#define L_DIR 4
#define R_PWM 6
#define R_DIR 9
#define Buzzer 10
#define TSOP_PIN 3
#define TrigPin 7
#define EchoPin 8
#define SerwoPin 11
Servo Serwo;

RC5 rc5(TSOP_PIN);
byte address;
byte command;
byte toggle;

void setup() {
 pinMode(L_PWM, OUTPUT);
 pinMode(L_DIR, OUTPUT);
 pinMode(R_PWM, OUTPUT);
 pinMode(R_DIR, OUTPUT);
 pinMode(Buzzer, OUTPUT);
 pinMode(TrigPin, OUTPUT);
 pinMode(EchoPin, INPUT);
 Serwo.attach(SerwoPin);
 digitalWrite(Buzzer, LOW);
}
void loop() {

 if (odleglosc() < 30) {
   digitalWrite(Buzzer, HIGH);
 }
 else {
   sterowanie();
 }
}

void sterowanie() {

 if (rc5.read(&toggle, &address, &command)) {

   switch (command) {

     case 2:
       leftMotor(30);
       rightMotor(30);
       break;

     case 8:
       leftMotor(-30);
       rightMotor(-30);
       break;

     case 5:
       stopMotors();
       break;

     case 4:
       leftMotor(-30);
       rightMotor(30);
       break;

     case 6:
       leftMotor(30);
       rightMotor(-30);
       break;

     case 1:
       leftMotor(20);
       rightMotor(30);
       break;

     case 3:
       leftMotor(30);
       rightMotor(20);
       break;

     case 7:
       leftMotor(-20);
       rightMotor(-30);
       break;

     case 9:
       leftMotor(-30);
       rightMotor(-20);
       break;

     case 150:
       for (int i = 0; i < 90; ++i) {
         Serwo.write(i);
         delay(20);
         digitalWrite(Buzzer, HIGH);

       }
       break;

     case 149:
       for (int i = 180; i > 90; --i) {
         Serwo.write(i);
         delay(20);
         digitalWrite(Buzzer, HIGH);
       }
   }
 }
}
void leftMotor(int V) {
 if (V > 0) {
   V = map(V, 0, 100, 0, PWM_MAX);
   digitalWrite(L_DIR, 0); //kierunek do przodu
   analogWrite(L_PWM, V);

 }
 else {
   V = abs(V);
   V = map(V, 0, 100, 0, PWM_MAX);
   digitalWrite(L_DIR, 1); //kierunek do tyłu
   analogWrite(L_PWM, V);

 }
}

void rightMotor(int V) {
 if (V > 0) {
   V = map(V, 0, 100, 0, PWM_MAX);
   digitalWrite(R_DIR, 0); //kierunek do przodu
   analogWrite(R_PWM, V);

 }
 else {
   V = abs(V);
   V = map(V, 0, 100, 0, PWM_MAX);
   digitalWrite(R_DIR, 1); //kierunek do tyłu
   analogWrite(R_PWM, V);

 }
}
void stopMotors() {
 analogWrite(L_PWM, 0);
 analogWrite(R_PWM, 0);
}


int odleglosc() {
 long czas;
 long dystans;

 digitalWrite(TrigPin, LOW);
 delayMicroseconds(2);
 digitalWrite(TrigPin, HIGH);
 delayMicroseconds(10);
 digitalWrite(TrigPin, LOW);

 czas = pulseIn(EchoPin, HIGH);
 dystans = czas / 58;
 if (dystans > 400) {
   dystans = 400;
 }

 return (dystans);

}

[Treker (Damian Szymański)]

JacekKarnowka, witam na forum. Umieść proszę kod w formie tekstowej w swojej wiadomości. Wtedy będzie znacznie łatwiej sprawdzić, co tam masz. Zrzuty ekranu nie nadają się do tego zbyt dobrze

[JacekKarnowka]

Poprawione )

[Lukaszm]

Sprawdzałeś czy pomiar odległości działa poprawnie?

W programie błędu nie widzę, więc pierwsze co bym sprawdził to czy 'komunikacja' z sonarem działa ok.

[JacekKarnowka]

Pomiar pokazuje poprawny, połączenia prawidłowe, sprawdzałem wielokrotnie. W takiej formie jak wkleiłem kod, to w ogóle nie działa na żadne polecenia, czy z pilota, czy z czujnika odległości.

[Treker (Damian Szymański)]

JacekKarnowka, możliwe, że biblioteki od serwa i RC5 się "gryzą". Spróbuj dla testu zakomentować wszystko, co jest związane z serwami i uruchom całość. Będzie wtedy wiadomo, czy tutaj leży problem