1

I've written a sketch to drive a differential drive system, specifically - to be able to drive strait. This does work.

So I am trying to add the ability to stop when the path is blocked using US sensor directed towards the front of the "car". I manage to get correct readings, but my "car" cannot move strait anymore.

I tried using the ping_timer() functionality of NewPing library, but it did not solve the problem. When the ultrasonic code is commented out, my "car" can go strait.

My algorithm for strait driving is essentially to compare the pulses from two rotary encoders fitted to the wheels, and if one exceeds the other by more than one pulse, shut down the relevant motor and turn on the other motor (if it wasn't on already) so the slower motor has a chance to keep-up.

I wonder if there is a problem in my code that I cannot find, I will be very thankful for anyone who can review my sketch:

#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();
  }
}

Here is a printout of the pulse counts and the status of the motors for a run:

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

What bugs me about this output is that I am expecting the slower motor not to open such large gaps, and I also know that in reality it did not open such large gaps. I suspect some pulses don't get counted for some reason, perhaps I am putting too much work on the micro-controller.

If the problem is not in my code, I wonder if this problem can be solved on a single Arduino device and I just have a weak algorithm, or perhaps it requires a separate device for the ultrasonic and a separate device for the differential drive.

Thanks in advance for any helpers.

PS. I am using this motor shield: https://osoyoo.com/2020/08/06/osoyoo-motor-shield/

2
  • The NewPing library uses a hardware timer for doing the background measurement, so does analogWrite. I suspect that there is a conflict for a hardware timer in your code. I currently cannot verify, but you should make sure, that these don't want to use the same timer
    – chrisl
    Oct 20, 2021 at 21:13
  • Thanks, chrisl. indeed I am using pins 2 and 3 for rotary encoders, and I see that pin 3 belongs to Timer2 which is used by NewPing for UNO boards. I understand that only pins 2 and 3 are usable for interrupts on UNO, so I guess I cannot use the ping_timer function and also attachIntterup on pin 3 on the same board? Oct 22, 2021 at 7:31

1 Answer 1

0

Thanks to @chrisl, here is the answer:

The NewPing library uses Timer2 on UNO, which means pin 3 (and 11) is not available for use. I have been using pin 3 for interrupts from a rotary encoder, which is probably the reason for the sluggish performance.

The bad news are there are only two interrupt pins on UNO - 2 and 3, so if I want to attach two rotary encoders to my two drive wheels, I cannot use interrupts to count the pulses. There may be a workaround, I couldn't think of one, but I think IR proximity sensors may be a better choice than my ultrasonic sensor because judging from code examples they are neither blocking nor do they need timers, but I didn't really dive deep into them yet so I may be wrong on this.

2
  • If you use the PinChangeInterrupt you can connect the rotary encoders do other pins. I think there are libraries available to handle that kind of interrupt easily. You can search for "Arduino library pin change interrupt". Though changing the design of your project in favor of IR proximity sensors is also a way of handling this.
    – chrisl
    Oct 22, 2021 at 9:18
  • Thanks! this is very useful. I didn't know this. Oct 24, 2021 at 12:19

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.