Reduce Lag Time?

Hi there, I have an ultrasonic sensor mounted on a servo that rolls around on 4 wheels. The wheels are hooked up via L298N | PWM | H-Bridge. I have a joystick connected to control the wheels, however I’m experiencing a bit of lag between when I control using the joystick and when the wheels actually start up… Any suggestions?

//Servo and Echo Variables and such
#include <Servo.h> // include file for servo control

int pin_trig = 11; // trigger pin connected to ultrasonic sensor
int pin_echo = 9; // echo pin connected to ultrasonic sensor

Servo servo1;  // create servo object to control servo #1

int position = 25 ;    // state variable
boolean forward = false ;   // state variable
unsigned long previousMillis = 0;
unsigned long previousMillis2 = 0;
const long interval = 2000;
int trigState = LOW;
int printState = LOW;
const long interval2 = 1;
const long interval3 = 500;




//H-Bridge Variables and such

int motorSpeedA = 0;
int motorSpeedB = 0;
int Disable_forward = 0;

#define enA 2
#define in1 3
#define in2 4
#define enB 7
#define in3 5
#define in4 6




void setup() {

  //Servo Tower initialization stuff
  servo1.attach(10);  // connect pin 7 to servo #1
  
  pinMode(pin_trig, OUTPUT);  
  pinMode(pin_echo, INPUT); 
  Serial.begin(9600);
  
  //H-Brdige initialization Stuff
  
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
}

void loop() {

  //H-Bridge Controls
  
  int xAxis = analogRead(A0); // Read Joysticks X-axis
  int yAxis = analogRead(A1); // Read Joysticks Y-axis
 

   if (yAxis < 470) {
    // Set Motor A forward
    if(Disable_forward == 1){
      Serial.println("Forward is disabled");
    }
    else{
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    // Set Motor B backward
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
    // Convert the declining Y-axis readings for going backward from 470 to 0 into 0 to 255 value for the PWM signal for increasing the motor speed
    motorSpeedA = map(yAxis, 470, 0, 0, 255);
    motorSpeedB = map(yAxis, 470, 0, 0, 255);
  }
  }
  else if (yAxis > 550) {
    // Set Motor A backwards
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    // Set Motor B forward
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);
    // Convert the increasing Y-axis readings for going forward from 550 to 1023 into 0 to 255 value for the PWM signal for increasing the motor speed
    motorSpeedA = map(yAxis, 550, 1023, 0, 255);
    motorSpeedB = map(yAxis, 550, 1023, 0, 255);
  }
  
  // If joystick stays in middle the motors are not moving
  else {
    motorSpeedA = 0;
    motorSpeedB = 0;
  }


  // X-axis used for left and right control
  if (xAxis < 470) {
    // Convert the declining X-axis readings from 470 to 0 into increasing 0 to 255 value
    int xMapped = map(xAxis, 470, 0, 0, 255);
    // Move to right 
    motorSpeedA = motorSpeedA + xMapped;
    motorSpeedB = motorSpeedB - xMapped;
    // Confine the range from 0 to 255
    if (motorSpeedB < 0) {
      motorSpeedB = 0;
    }
    if (motorSpeedA > 255) {
      motorSpeedA = 255;
    }
  }
  if (xAxis > 550) {
    // Convert the increasing X-axis readings from 550 to 1023 into 0 to 255 value
    int xMapped = map(xAxis, 550, 1023, 0, 255);
    // Move left 
    motorSpeedA = motorSpeedA - xMapped;
    motorSpeedB = motorSpeedB + xMapped;
    // Confine the range from 0 to 255
    if (motorSpeedB > 255) {
      motorSpeedB = 255;
    }
    if (motorSpeedA < 0) {
      motorSpeedA = 0;
    }
  }
  // Prevent buzzing at low speeds 
  if (motorSpeedA < 70) {
    motorSpeedA = 0;
  }
  if (motorSpeedB < 70) {
    motorSpeedB = 0;
  }
  analogWrite(enA, motorSpeedA); // Send PWM signal to motor A
  analogWrite(enB, motorSpeedB); // Send PWM signal to motor B




//Servo + Echo Controls

//Servo Section

  unsigned long currentMillis = millis();
  unsigned long currentMillis2 = millis();

  int analog_input0; // analog input for pin A0
  float voltage0; // voltage input for pin A0

//servo
  if (currentMillis - previousMillis >= interval){
  previousMillis += interval;

  if (forward){
    servo1.write (position = position - 15);
    if (position ==25)
    forward = false;
  }else{
    servo1.write (position = position +15);
    if (position == 70)
    forward = true;
   
  }
  }




//Ultrasonic Sensor Section

  if (currentMillis2-previousMillis2 >= interval2) { 
    previousMillis2 = currentMillis2;
    if (trigState == LOW){ //if no pulse then pulse
      (trigState = HIGH);
    }
    else { //if pulsed, then stop pulse
      (trigState = LOW); 
    }
  }
  // printing if statement
  if (currentMillis2-previousMillis2 >= interval3) { 
    previousMillis2 = currentMillis2;
    if (printState == LOW){
      (printState = HIGH);
    }
    else {
      (printState = LOW);
    }
  }
  
  digitalWrite(pin_trig,trigState);
  
  float duration; 
  float distance; 
  float c = 343; //speed of sounds (m/s)
  duration = pulseIn(pin_echo,HIGH);//pulseIn records time between high and low.
  
  duration *= 1.0e-6;  //Convert to seconds
  distance = c*100* duration/2 ; // distance in cm
  
  if (printState = HIGH){
    if (distance != 0){
    Serial.print ("\ndistance (cm) = ");
    Serial.print(distance);
  }
  }
  
 
  if (distance <= 35.0 && distance >=1){
    Disable_forward = 1;
  }
  else if (distance >= 35){
    Disable_forward = 0;
  }
  else{
    Serial.print("");
  }
  



 }

Well done! No delays!

But pulseIn() can block while it is waiting for a pulse. You might consider using an interrupt to receive the pulse instead of waiting for up to a whole second.

I think you also have a problem with previousMillis2. You use it with both interval2 and interval3. interval2 "wins" and interval3 will never occur.

You don't need more than one currentMillis. Delete the second one.

Any time you have numbers in your variable names, you are doing something wrong. In this case, give them more descriptive names like previousPrintTime.

Better yet, don't use any "currentMillis" variables, just put millis() in the timing equations, save RAM and be more actually on-time.

if ( millis() - timeStart >= timeWait )

Also, make ALL of your time variables unsigned long rather than a mix of unsigned and signed that generate more code. Mixed-type math is something to avoid. If necessary, do it but know you're doing it and cast the values to all the same type.

MorganS:
Well done! No delays!

But pulseIn() can block while it is waiting for a pulse. You might consider using an interrupt to receive the pulse instead of waiting for up to a whole second.

Alternately, pulseIn() defaults to one second for timeout, but you can explicitly specify a timeout value.

For a HC-SR04 ultrasonic sensor, the effective maximum range is less than 4 meters (8 meters round trip) so if you haven't got a return in 30 milliseconds there's not much point in waiting any longer.

Thus "duration = pulseIn(pin_echo,HIGH,30000); // Measure pulse duration, timeout if not received in 30e3 microseconds."

MorganS:
But pulseIn() can block while it is waiting for a pulse. You might consider using an interrupt to receive the pulse instead of waiting for up to a whole second.

pulseIn() can take a timeout as third parameter. Very useful as a 5m round trip (10m total distance) takes some 30 ms, if no echo in that period there won't be one. For most applications it's quite OK to block for up to 30 ms a few times a second.

duration = pulseIn(pin_echo, HIGH, 30000); // pulseIn records time between high and low, timing out after 30,000 µs (30 ms).

LOL MrMark beat me to it :slight_smile:

Thank you guys so much, I took your considerations in, and the easiest fix with best results was the change duration = pulseIn(pin_echo, HIGH, 30000);
Again thank you! You rlly saved me!