Может ли дифференциальный привод работать вместе с ультразвуковым датчиком?

Я написал скетч, чтобы управлять системой дифференциального привода, в частности, чтобы иметь возможность управлять strait. Это действительно работает.

Поэтому я пытаюсь добавить возможность останавливаться, когда путь заблокирован, используя датчик мкс, направленный в переднюю часть "автомобиля". Мне удается получить правильные показания, но моя "машина" больше не может двигаться.

Я попытался использовать функцию ping_timer() библиотеки NewPing, но это не решило проблему. Когда ультразвуковой код закомментирован, моя "машина" может ехать прямо.

Мой алгоритм прямого вождения, по сути, заключается в сравнении импульсов от двух поворотных датчиков, установленных на колесах, и, если один превышает другой более чем на один импульс, выключите соответствующий двигатель и включите другой двигатель (если он еще не был включен), чтобы у более медленного двигателя был шанс не отставать.

Интересно, есть ли в моем коде проблема, которую я не могу найти, я буду очень благодарен всем, кто сможет просмотреть мой скетч:

#include <Arduino.h>
// #include <Servo.h>
#include <TimedAction.h>
#include <NewPing.h>

#define RIGHT_MOTOR_ON (0 - int(pulseCountRight <= pulseCountLeft + 1))
#define LEFT_MOTOR_ON (0 - int(pulseCountLeft <= pulseCountRight + 1))

const int EN_RIGHT = 6;
const int IN1_RIGHT = 7;
const int IN2_RIGHT = 8;
const int ENCODER_RIGHT = 2;

const int EN_LEFT = 5;
const int IN1_LEFT = 10;
const int IN2_LEFT = 11;
const int ENCODER_LEFT = 3;

const int SPEED = 100;

const int BW_TRUE = -1;
const int BW_FALSE = 0;

// const int SERVO_PIN = 9;
// const int SERVO_MAX_RIGHT = 0;
// const int SERVO_MAX_LEFT = 125;
const int SERVO_SWEEP_DELAY = 150;
// const int SERVO_TURN_RIGHT = -25;
// const int SERVO_TURN_LEFT = 25;

const int ULTRASONIC_TOO_CLOSE = 20;
// Timeout is the maximum time it would take sound to do a roundtrip with length of "ULTRASONIC_TOO_CLOSE".
// Formula is: DIST(cm) x TIME(us per cm).
const int ULTRASONIC_TIMEOUT = ULTRASONIC_TOO_CLOSE * US_ROUNDTRIP_CM;
const int ULTRASONIC_TRIG_PIN = 13;
const int ULTRASONIC_ECHO_PIN = 12;

unsigned int pulseCountRight = 0;
unsigned int pulseCountLeft = 0;

int movingStrait = BW_TRUE;
int stopping = BW_TRUE;

double distance = 0.0;

void pingTimer();
void checkTimer();

NewPing sonar(ULTRASONIC_TRIG_PIN, ULTRASONIC_ECHO_PIN, 400);

TimedAction pingTimerAction(SERVO_SWEEP_DELAY, pingTimer);

// Servo myServo;

void countPulse(unsigned int &counter) {
  ++counter;

  char c[1000];
  sprintf(c, "Pulses (right,left): %d,%d, Motor on (right,left): %d,%d", pulseCountRight, pulseCountLeft, RIGHT_MOTOR_ON, LEFT_MOTOR_ON);
  Serial.println(c);

  analogWrite(EN_RIGHT, SPEED & (RIGHT_MOTOR_ON | ~movingStrait) & ~stopping);
  analogWrite(EN_LEFT, SPEED & (LEFT_MOTOR_ON | ~movingStrait) & ~stopping);
}

void forward() {
  movingStrait = BW_TRUE;
  stopping = BW_FALSE;
  pulseCountRight = 0;
  pulseCountLeft = 0;

  digitalWrite(IN1_RIGHT, LOW);
  digitalWrite(IN2_RIGHT, HIGH);

  digitalWrite(IN1_LEFT, LOW);
  digitalWrite(IN2_LEFT, HIGH);

  analogWrite(EN_RIGHT, SPEED);
  analogWrite(EN_LEFT, SPEED);
}

void backwards() {
  movingStrait = BW_TRUE;
  stopping = BW_FALSE;
  pulseCountRight = 0;
  pulseCountLeft = 0;

  digitalWrite(IN1_RIGHT, HIGH);
  digitalWrite(IN2_RIGHT, LOW);

  digitalWrite(IN1_LEFT, HIGH);
  digitalWrite(IN2_LEFT, LOW);

  analogWrite(EN_RIGHT, SPEED);
  analogWrite(EN_LEFT, SPEED);
}

void right() {
  movingStrait = BW_FALSE;
  stopping = BW_FALSE;

  digitalWrite(IN1_RIGHT, LOW);
  digitalWrite(IN2_RIGHT, HIGH);

  digitalWrite(IN1_LEFT, HIGH);
  digitalWrite(IN2_LEFT, LOW);

  analogWrite(EN_RIGHT, SPEED);
  analogWrite(EN_LEFT, SPEED);
}

void left() {
  movingStrait = BW_FALSE;
  stopping = BW_FALSE;

  digitalWrite(IN1_RIGHT, HIGH);
  digitalWrite(IN2_RIGHT, LOW);

  digitalWrite(IN1_LEFT, LOW);
  digitalWrite(IN2_LEFT, HIGH);

  analogWrite(EN_RIGHT, SPEED);
  analogWrite(EN_LEFT, SPEED);
}

void stop() {
  movingStrait = BW_FALSE;
  stopping = BW_TRUE;

  digitalWrite(IN1_RIGHT, LOW);
  digitalWrite(IN2_RIGHT, LOW);

  digitalWrite(IN1_LEFT, LOW);
  digitalWrite(IN2_LEFT, LOW);
}

void setup() {
  Serial.begin(9600);
  Serial.println("DifferentialDrive2");

  pinMode(EN_RIGHT, OUTPUT);
  pinMode(EN_LEFT, OUTPUT);
  pinMode(IN1_RIGHT, OUTPUT);
  pinMode(IN2_RIGHT, OUTPUT);
  pinMode(IN1_LEFT, OUTPUT);
  pinMode(IN2_LEFT, OUTPUT);

  pinMode(ENCODER_RIGHT, INPUT_PULLUP);
  pinMode(ENCODER_LEFT, INPUT_PULLUP);

  // pinMode(SERVO_PIN, OUTPUT);

  pinMode(ULTRASONIC_TRIG_PIN, OUTPUT);
  pinMode(ULTRASONIC_ECHO_PIN, INPUT);

  attachInterrupt(digitalPinToInterrupt(ENCODER_RIGHT), [] { countPulse(pulseCountRight); }, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_LEFT), [] { countPulse(pulseCountLeft); }, CHANGE);

  digitalWrite(IN1_RIGHT, LOW);
  digitalWrite(IN2_RIGHT, HIGH);

  digitalWrite(IN1_LEFT, LOW);
  digitalWrite(IN2_LEFT, HIGH);

  analogWrite(EN_RIGHT, SPEED);
  analogWrite(EN_LEFT, SPEED);

  // myServo.attach(SERVO_PIN);
  // myServo.write((SERVO_MAX_RIGHT + SERVO_MAX_LEFT) / 2);

  forward();
}

unsigned long start_time = 0;

void pingTimer() {
  sonar.timer_stop();
  start_time = micros();
  sonar.ping_timer(checkTimer);
  // NewPing::timer_us(ULTRASONIC_TIMEOUT * 4, checkTimer);
}

void checkTimer() {
  // static int angle = SERVO_MAX_RIGHT;
  // static int direction = SERVO_TURN_LEFT;
  if (micros() - ULTRASONIC_TIMEOUT >= start_time && sonar.check_timer()) {
    distance = ((float) sonar.ping_result / (float) US_ROUNDTRIP_CM) / 2.0;
    if (distance < ULTRASONIC_TOO_CLOSE) {
      stop();
      Serial.println("stoppeing");
    }
  }
  // sonar.timer_stop();
}

void loop() {
  pingTimerAction.check();
  if (distance == 0 || distance > ULTRASONIC_TOO_CLOSE) {
    // forward();
  }
}

Вот распечатка количества импульсов и состояния двигателей для запуска:

DifferentialDrive2
Pulses (right,left): 1,0, Motor on (right,left): -1,-1
Pulses (right,left): 1,1, Motor on (right,left): -1,-1
Pulses (right,left): 2,1, Motor on (right,left): -1,-1
Pulses (right,left): 3,1, Motor on (right,left): 0,-1
Pulses (right,left): 4,1, Motor on (right,left): 0,-1
Pulses (right,left): 5,1, Motor on (right,left): 0,-1
Pulses (right,left): 6,1, Motor on (right,left): 0,-1
Pulses (right,left): 7,1, Motor on (right,left): 0,-1
Pulses (right,left): 8,1, Motor on (right,left): 0,-1
Pulses (right,left): 9,1, Motor on (right,left): 0,-1
Pulses (right,left): 9,2, Motor on (right,left): 0,-1
Pulses (right,left): 9,3, Motor on (right,left): 0,-1
Pulses (right,left): 9,4, Motor on (right,left): 0,-1
Pulses (right,left): 9,5, Motor on (right,left): 0,-1

Pulses (right,left): 9,6, Motor on (right,left): 0,-1
Pulses (right,left): 9,7, Motor on (right,left): 0,-1
Pulses (right,left): 9,8, Motor on (right,left): -1,-1
Pulses (right,left): 9,9, Motor on (right,left): -1,-1
Pulses (right,left): 10,9, Motor on (right,left): -1,-1
Pulses (right,left): 11,9, Motor on (right,left): 0,-1
Pulses (right,left): 12,9, Motor on (right,left): 0,-1
Pulses (right,left): 13,9, Motor on (right,left): 0,-1
Pulses (right,left): 14,9, Motor on (right,left): 0,-1
Pulses (right,left): 15,9, Motor on (right,left): 0,-1
Pulses (right,left): 16,9, Motor on (right,left): 0,-1
Pulses (right,left): 17,9, Motor on (right,left): 0,-1
Pulses (right,left): 17,10, Motor on (right,left): 0,-1
Pulses (right,left): 17,11, Motor on (right,left): 0,-1
Pulses (right,left): 17,12, Motor on (right,left): 0,-1
Pulses (right,left): 17,13, Motor on (right,left): 0,-1
Pulses (right,left): 17,14, Motor on (right,left): 0,-1
Pulses (right,left): 17,15, Motor on (right,left): 0,-1
Pulses (right,left): 17,16, Motor on (right,left): -1,-1
Pulses (right,left): 17,17, Motor on (right,left): -1,-1
Pulses (right,left): 18,17, Motor on (right,left): -1,-1
Pulses (right,left): 19,17, Motor on (right,left): 0,-1
Pulses (right,left): 20,17, Motor on (right,left): 0,-1
Pulses (right,left): 21,17, Motor on (right,left): 0,-1
Pulses (right,left): 22,17, Motor on (right,left): 0,-1
Pulses (right,left): 23,17, Motor on (right,left): 0,-1
Pulses (right,left): 24,17, Motor on (right,left): 0,-1
Pulses (right,left): 24,18, Motor on (right,left): 0,-1
Pulses (right,left): 24,19, Motor on (right,left): 0,-1
Pulses (right,left): 24,20, Motor on (right,left): 0,-1

Что меня беспокоит в этом выходе, так это то, что я ожидаю, что более медленный двигатель не откроет таких больших зазоров, и я также знаю, что на самом деле он не открывал таких больших зазоров. Я подозреваю, что некоторые импульсы по какой-то причине не учитываются, возможно, я слишком много работаю с микроконтроллером.

Если проблема не в моем коде, мне интересно, может ли эта проблема быть решена на одном устройстве Arduino, и у меня просто слабый алгоритм, или, возможно, для этого требуется отдельное устройство для ультразвукового и отдельное устройство для дифференциального привода.

Заранее спасибо за любых помощников.

пс. Я использую этот моторный щит: https://osoyoo.com/2020/08/06/osoyoo-motor-shield/