Autonomous vehicles, putting it all together

With all the testing done, it’s time to put all the gathered knowledge together, and build the final product: a self-driving-obstacle-avoiding-smart-robot-car, with display and illuminated encoder button. If it works as great as it sounds, it is an awesome machine.

Autonomous vehicles, HC-SR04 sensors

Step 1: Detecting objects with HC-SR04
Step 2: Multiple HC-SR04 with Arduino using interrupts
Step 2b: Multiple HC-SR04 with Arduino using only 1 interrupt pin
Step 3: Choosing the right vehicle
Step 4: Automatic steer correction with HC-SR04
Step 5a: Vehicle accessories – SSD1306 status display
Step 5b: Vehicle accessories – Rotary encoder illuminated button
Step 6: Autonomous vehicles, putting it all together
Alternative 1: Steer correction with HC-SR04: Single sensor, stop and scan
Alternative 2: Steer correction with HC-SR04: Servo controlled car

Components

For the autonomous vehicle in this project, the following components are used

  • 3-wheel smart car chassis (with 2 DC motors)
  • Seeeduino v4.2 (alternative to Arduino Uno)
  • Cytron Maker Drive (to control 2 DC motors)
  • 3x HC-SR04 sensors (requirement for this project)
  • HC-SR04 holders (for easily mounting the sensors)
  • SSD1306 OLED 128×32 pixel display (optional for feedback)
  • Rotary encoder with RGB illuminated push button
    (optional, for start/stop the vehicle and max. speed setting)
  • 4x diodes (1N4001 will do fine)
  • 1x 10K resistor
  • 1x 100E resistor
  • 2x 150E resistor
  • 2x bread board, 400 holes
  • 5V power regulator (5A output, 2A will suffice)
  • LiPo battery (7.2V / 800mAh)
  • Bunch of jumper wires (bread board friendly)

Buildup

Vehicle chassis

The three wheel smart car chassis has 2 DC motors which makes it a perfect chassis for simple controlling the vehicle.

Three wheel smart car chassis
Three wheel smart car chassis

Motor control

As an alternative to the L298N, the Cytron Maker Drive is ideal to control the 2 DC motors in both direction and speed. It has an additional coasting mode.

Object detection

Three HC-SR04 sensors, with 1 in front and 2 on the side (angled) make for a wider scanner angle. The multi trigger, 1 echo (on an interrupt pin) method is used.

Three front sensors
Three front sensors

An optimal result is achieved if the side sensors are angled at about 60°. To achieve this, remove the outer screw, and turn the sensor holder so the hole lines up with the long slot.

HC-SR04 montage with 45° angle
HC-SR04 montage with 45° angle

Display

The SSD1306 128×32 pixels OLED is used to show which action the vehicle is taking, as well as the distances measured by the sensors. This is optional and can be turned of in the code.

Speed control and start/stop

An encoder with RGB LED and push button can control the vehicle’s speed and start/stop mode. This too is optional and can be turned off in the configuration part of the code.

The breakout board is 1 row to long for the breadboard. By placing it on the side and letting it hang over the chassis, it can be connected properly to the board.

The final result should look (more or less) like this

3 wheel autonomous vehicle
3 wheel autonomous vehicle

5VDC regulator

Voltage regulator (XL4015) 5A

The motor controller, sensors and display draw quite some power, more than the Arduino Uno / Seeduino v4.2 (or any other alternative) can provide. Therefore, an external 5VDC power supply is required.

Electrical connections

Safety diode

The motor driver, sensors and display can draw quite some power from the 5V pin from the Arduino board (or Seeeduino in this case). This could damage the voltage regulator on the board. Therefore, a diode is placed between the 5V pin of the board and the rest of the circuit, so that when a USB cable is connected to download the program, the rest of the components remain unpowered. This also prevents the vehicle from suddenly starting to drive around.

Power supply

In order to provide sufficient power to the vehicle, a battery and voltage regulator that can provide a stable 5VDC and 2A (peak) is strongly recommended. Anything less may fail to provide sufficient power and several issues can appear like bad measurements, slow motors, rebooting boards, …

Schematic

For the voltage regulator, the LM7805 is drawn in the schematic to keep it simple. Using a step-down is much more power efficient.

Keep in mind that it may be necessary to reverse the direction of one of the motors.

Autonomous vehicle electrical connections
Autonomous vehicle electrical connections

The schematics for the complete autonomous vehicle can be downloaded in pdf format below.

Code for 3 wheel vehicle

Logic

After a short boot cycle (gives you time to properly attach the battery), the vehicle will go into paused state. From there it depends if the encoder button is used.
With encoder button: the vehicle will remain in paused state until the encoder button is pressed.
Without encoder button: the vehicle will remain paused for a certain time, then move to the next state automatically.

Once the driving state is initialised, the vehicle will measure the distance on all three sensors. When the left or right sensor detect an object within a certain range (e.g. 50cm), it will make a soft steer left or right, away from the obstacle.

If an obstacle is detected very close by (e.g. 20cm), the vehcile will stop, make a hard turn left or right (wherever is the most room left), and resume driving.

Display (optional)

The display (if enabled) can be set to big font or normal font mode, by pressing the encoder button for 1 second (called long press in the code).
In big font mode, only the robot state and speed setting are displayed in big fonts. In normal font mode, the measured distances, the vehicle’s current state and the max. set speed for the motors is displayed.

Display big font mode
Display big font mode
Display normal font mode
Display normal font mode

Illuminated encoder button

Illuminated encoder button
Illuminated encoder button

With the optional encoder button, the robot can be switched from paused state to running state (and back) by pressing the button shortly.

By turning the encoder button clockwise or counter clockwise, the speed of the robot can be adjusted.

Long press will swap the display between big font and small font state.

Configuration

If you do not want to use the encoder and button, set the useEncoder flag in the configuration section to false. Don’t forget to set the fixed speeds for the motors. Set halfSpeed to about 2/3 of the fullSpeed, for the best results (unless different drivers and/or motors are used).

// Encoder
#define useEncoder  true   // If set to false, speed is fixed, car is started automatically

// Maker Drive - Motor control
#define fullSpeed   150    // 0-255 PWM signal for highest speed (forward drive, soft turn)
#define halfSpeed   100    // 0-255 PWM signal for reduced speed (hard turn, reverse)

To disable the display and send the feedback information to the serial monitor in stead, change the displayPresent flag to false.

By using big font, the display is easier to read when the robot is moving, but not all data will be displayed. If the encoder button is not used to swap between the big font and normal font mode, the flag useBigFont can be set to true font using big fonts (less info) or false for using normal fonts (more info).

// Use display ?
bool displayPresent = true;      // SD1306 display present
bool useBigFont = true;          // Large text, less info

The rest of the configuration is straight forward, and all commented in the code itself.

Not an Uno compatible

If a different board or interrupt pin is used for the HC-SR04 sensors and/or the encoder, the code in ISR function ISR_ECHO() and/or ISR_ENC() needs to be updated.

Either un-comment the line for digitalRead and change the pin number in the configuration section, and comment the port register lines

void ISR_ECHO()
{
  // bool pinRead = digitalRead(pinEcho);  // Use faster port registers for HC-SR04 instead
...

// Encoder pin
void ISR_ENC()
{
  // bool pinRead = digitalRead(pinEncB);  // Use faster port registers for encoder instead
...

Or update the port register lines

  // For ATmega328(P) chips, INT0 as at pin PD2
  bool pinRead = bool(PIND >> 2 & B0001);  // Read state of interrupt pin

  // For ATmega328(P) chips, INT1 as at pin PD3 (encoder signal A is connected here)
  // and encoder signal B is connected at pin 4 (which can be any pin)
  bool pinRead = bool(PIND >> 4 & B0001);  // Read state of Encoder B pin (pin 4)

Arduino code

To handle the button press, the class_button.h library is used.

The final full code for the Arduino Uno / Seeeduino v4.2 (and compatible boards) is

/****************************************************************
      To be used as a local class
****************************************************************/
class BUTTON
{
  private:
    unsigned long storeMillis = millis();
    unsigned long tmrTON = millis();
    unsigned long tmrTOF = millis();
    bool REpassed = false;  // Remember Rising Edge
    bool FEpassed = false;  // Remember Falling Edge

  public:
    bool state = false;     // State of the button to check
    int TON_delay = 1000;   // Debounce/Delay time for ON/Rising Trigger detection
    int TOF_delay = 1000;   // Debounce/Delay time for OFF/Falling Trigger detection
    bool Q = false;         // Debounce/Delay time passed and button still high
    bool RE = false;        // Rising Edge detected (remains high for only 1 cycle)
    int FE = false;         // Falling Edge detected (remains high for only 1 cycle)

    // Call this every cycle
    void refresh()
    {
      // First check the timers
      if (!this->state)
      {  this->tmrTON = millis(); }
      else  // (this->state)
      {  this->tmrTOF = millis(); }

      // How long is the button PRESSED ?
      if (this->state && (this->tmrTON + this->TON_delay < millis()))
      {
        // Button was continuously pressed for TON_delay time
        this->Q = true;           // Set Q to HIGH state
        this->FE = false;         // Falling Edge low (small change it's still high)
        this->FEpassed = false;   // Falling Edge detection help var low

        // Rising Edge detected ?
        if(!this->REpassed)
        {
          this->RE = true;        // Not yet -> Set Rising Edge detected HIGH
          this->REpassed = true;  // Help var for Rising Edge
        }
        else
        {
          this->RE = false;       // Already detected -> Set Rising Edge detected LOW
        }
      }

      // How long is the button RELEASED ?
      if (!this->state && (this->tmrTOF + this->TOF_delay < millis()))
      {
        // Button was continuously released for TOF_delay time
        this->Q = false;          // Set Q to LOW state
        this->RE = false;         // Rising Edge low (small change it's still high)
        this->REpassed = false;   // Rising Edge detection help var low

        // Falling Edge detected ?
        if(!this->FEpassed)
        {
          this->FE = true;        // Not yet -> Set Falling Edge detected HIGH
          this->FEpassed = true;  // Help var for Falling Edge
        }
        else
        {
          this->FE = false;       // Already detected -> Set Falling Edge detected LOW
        }
      }
    };
};
/****************************************************************
      Create button control
****************************************************************/
#include "class_button.h"
BUTTON btnShortPress;  // Detect short press on encoder button
BUTTON btnLongPress;   // Detect long press on encoder button


/****************************************************************
      SSD1306 display (128x32 px)
****************************************************************/
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

/****************************************************************
      Configuration
****************************************************************/
// HC-SR04 - sensors
#define tSENS 3            // Total number of sensors (3: Left, center, right)
#define pinEcho 2          // HC-SR04: common pin for echo (must be interrupt pin - also check port register in ISR !)
#define soundSpeed 343     // Speed of sound in m/s (343m/s at 20°C with dry air) to calculate distance
//#define pinTriggers[tSENS] {A0,A1,A2}    // HC-SR04: trigger pin of each sensor
const int pinTriggers[tSENS] = {A0,A1,A2};    // HC-SR04: trigger pin of each sensor (0=left, 1=center, 2=right)

// Encoder
#define pinEncA 3          // Encoder A signal (must be interrupt pin)
#define pinEncB 4          // Encoder B signal (any pin is fine)
#define pinEncButton 7     // Encoder push button
#define pinEncLED_R 8      // Encoder RGB LED, Red
#define pinEncLED_G 11     // Encoder RGB LED, Green
#define pinEncLED_B 12     // Encoder RGB LED, Blue
#define useEncoder  true   // If set to false, speed is fixed, car is started automatically

// Sensor delay
#define sensorDelay 50     // Check sensors every x ms (keep at least > 5ms)

// Maker Drive - Motor control
#define pinDriveM1A 5      // Cytron Maker Drive: Pin for M1A (PWM pin) - 980Hz
#define pinDriveM1B 9      // Cytron Maker Drive: Pin for M1B (PWM pin) - 490Hz
#define pinDriveM2A 6      // Cytron Maker Drive: Pin for M2A (PWM pin) - 980Hz
#define pinDriveM2B 10     // Cytron Maker Drive: Pin for M2B (PWM pin) - 490Hz
#define fullSpeed   150    // 0-255 PWM signal for highest speed (forward drive, soft turn)
#define halfSpeed   100    // 0-255 PWM signal for reduced speed (hard turn, reverse)

// Time between decisions (keep bigger than sensor delay)
#define robotBrainDelay 100   // Brain will call drive control and display refresh

// Use display ?
bool displayPresent = true;      // SD1306 display present
bool useBigFont = true;          // Large text, less info
/*****   END OF CONFIGUTATION    *****/

volatile unsigned long startEcho;     // Place to store start time (interrupt)
volatile unsigned long stopEcho;      // Place to store stop time (interrupt)
volatile byte encoderCounter;         // Encoder counter for speed setting
float distances[tSENS];               // HC-SR04 last measured distance per sensor (in cm)

// Robot state ENUM
enum ENUM_robotState {
  paused,
  booting,
  driving,
  sensing_block,
  soft_left,
  soft_right,
  hard_left,
  hard_right,
  stopping,
  stopped,
  reversing
};
ENUM_robotState robotState = booting;

// Poll timers
unsigned long lastSensorMillis;
unsigned long lastRobotBrainMillis;
unsigned long stateStartTime;

/****************************************************************
      SETUP
****************************************************************/
void setup()
{
  Serial.begin(115200);
  while (!Serial) {
    ; // Wait for Serial
  }
  Serial.println("--- Serial monitor started ---");

  // INIT SSD1306 if present
  if (displayPresent) {
    // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x32
      Serial.println("SSD1306 allocation failed");
      displayPresent = false;
    }
    else {
      updateDisplay();
    }
  }
  
  // Set pins as input
  pinMode(pinEcho, INPUT);	       // Echo pin
  pinMode(pinEncA, INPUT_PULLUP);  // Encoder signal A (C is GND !)
  pinMode(pinEncB, INPUT_PULLUP);  // Encoder signal B (C is GND !)
  pinMode(pinEncButton, INPUT);    // Encoder push button (common anode, no pull-up !)

  // Set pins as output
  for (int s = 0; s < tSENS; s++) {
    pinMode(pinTriggers[s], OUTPUT);   // Set trigger pin x as OUTPUT
  }
  pinMode(pinDriveM1A, OUTPUT);  // Motor 1A (PWM)
  pinMode(pinDriveM1B, OUTPUT);  // Motor 1B (PWM)
  pinMode(pinDriveM2A, OUTPUT);  // Motor 2A (PWM)
  pinMode(pinDriveM2B, OUTPUT);  // Motor 2B (PWM)
  pinMode(pinEncLED_R, OUTPUT);  // Encoder RGB led, Red
  pinMode(pinEncLED_G, OUTPUT);  // Encoder RGB led, Green
  pinMode(pinEncLED_B, OUTPUT);  // Encoder RGB led, Blue

  // Set interrupts
  attachInterrupt(0, ISR_ECHO, CHANGE );    // Set interrupt call for echo pin
  attachInterrupt(1, ISR_ENC, CHANGE);      // Set interrupt call for encoder

  // Set encoder button press timing (and de-bounce)
  btnShortPress.TON_delay = 25;  // 25ms stable high is considered ON (de-bounce)
  btnShortPress.TOF_delay = 25;  // 25ms stable low is considered OFF (de-bounce)
  btnLongPress.TON_delay = 1000; // 1000ms stable high is considered ON long press (de-bounce)
  btnLongPress.TOF_delay = 25;   // 25ms stable low is considered OFF (de-bounce)

  // Start encoder counter to defined full speed
  encoderCounter = fullSpeed;

  // Set timers
  lastSensorMillis = millis();
  lastRobotBrainMillis = millis();
  
  // Set robot state to booting
  robotState = booting;
  stateStartTime = millis();  // Remember when booting state started
}

/****************************************************************
      LOOP
****************************************************************/
void loop()
{
  if (useEncoder) {
    // Encoder button check
    // bool buttonState = digitalRead(pinEncButton); // Read state
    bool buttonState = bool(PIND >> 7 & 1);   // Faster (bit D7 on Uno)
    btnShortPress.state = buttonState;
    btnLongPress.state = buttonState;
    btnShortPress.refresh(); // Update button
    btnLongPress.refresh();

    // Short/Long button press ?
    if (btnLongPress.RE) {
      // Long press -> toggle normal/big font
      if (not(robotState == booting)) { // Supress false first trigger
        useBigFont = not(useBigFont);
      }
    }
    else if (btnShortPress.FE and not(btnLongPress.FE)) {
       // Short press -> swap state ON/OFF
      if (robotState == paused) {
        stateStartTime = millis();  // Remember when booting state started
        robotState = driving;
      }
      else if( robotState != booting && robotState != paused ) {
        stateStartTime = millis();  // Remember when booting state started
        robotState = paused;
      }
    }
    updateRGBLEDs();
  } // useEncoder
  
  // Read sensors
  if (millis() - lastSensorMillis >= sensorDelay)
  {
    checkHCSR04(15);    // Detecting objects (L/C/R)
    lastSensorMillis = millis();
  }

  // Make logic decision
  if (millis() - lastRobotBrainMillis >= robotBrainDelay)
  {
    robotBrain(); // Making decisions
    lastRobotBrainMillis = millis();
  }
}

/****************************************************************
      Robot brain
****************************************************************/
void robotBrain() {
  // 
  float steerDistance = 50; // in cm ; steer away but keep driving
  float brakeDistance = 20; // in cm ; brake, stop and act

  // Make code easier to read
  float distanceLeft = distances[0];
  float distanceCenter = distances[1];
  float distanceRight = distances[2];


  // Check current state and make decision
  switch (robotState) {
    // If booting, apply brakes and stay in this state for a while
	  // so the battery can be properly attached
    case booting:
      makerDrive(B0000);
      if (stateStartTime + 2000 < millis()) {
        robotState = paused;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Paused state activated: brake and wait
    case paused:
      makerDrive(B0000);
      if (useEncoder) {
        // Option encoder button used = TRUE -> 
        ; // wait untill button is pushed
      }
      else {
        if (stateStartTime + 2000 < millis()) {
         // Option encoder button used = FALSE -> auto resume after x milliseconds
          robotState = stopped;
          stateStartTime = millis();  // Remember when switching states started
        }
      }
      break;
      

    // Driving = move forward untill object detected
    case driving:
      makerDrive(B1010);
      // Distance < brake distance on any sensor
      if (not(checkDistance(brakeDistance))) {
        robotState = sensing_block;
        stateStartTime = millis();  // Remember when switching states started
      }

      // Distance > brake distance but steer correction required
      if (distanceLeft < steerDistance || distanceRight < steerDistance) {
        if (distanceLeft < distanceRight) {
          // correction to right
          robotState = soft_right;
          stateStartTime = millis();  // Remember when switching states started
        }
        else if(distanceRight < distanceLeft) {
          // correction to left
          robotState = soft_left;
          stateStartTime = millis();  // Remember when switching states started
        }
        // else just keep heading straight
      }
      break;

	  // Sensing object = keep driving until sensing object exceeds timers
	  case sensing_block:
  	  makerDrive(B1010);
  	  if (checkDistance(brakeDistance)) {
  		  robotState = driving;
  		  stateStartTime = millis();  // Remember when switching states started
  	  }
  	  else if (stateStartTime + 150 < millis()) {
  		  robotState = stopping;
  		  stateStartTime = millis();  // Remember when switching states started
  	  }
  	  break;

    // Coast stop and assume time it takes to standstill
    case stopping:
      makerDrive(B1111);
      if (stateStartTime + 500 < millis()) {
        robotState = stopped;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Stopped = apply brakes and wait for road to be cleared
    case stopped:
      makerDrive(B0000);  // Brakes on
      // Obstacle cleared -> resume driving
      if (checkDistance(brakeDistance)) {
        robotState = driving;
      }
      // Wait a small amount of time, then make decision
      if (stateStartTime + 500 < millis()) {
        if (distances[0] > distances[2]) {
          // More space on the left, turn left
          robotState = hard_left;
        }
        else if (distances[0] < distances[2]) {
          // more space on the right, turn right
          robotState = hard_right;
        }
        else {
          // Equal distance? What's the chance, drive reverse
          robotState = reversing;
        }
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Turning hard left
    case hard_left:
      makerDrive(B0110);
      // Turning left = depending on speed, so adjust for 90° turn or use encoders
      if (stateStartTime + 500 < millis()) {
        robotState = stopping;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Turning hard right
    case hard_right:
      makerDrive(B1001);
      // Turning right = depending on speed, so adjust for 90° turn or use encoders
      if (stateStartTime + 500 < millis()) {
        robotState = stopping;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Reversing should only take a little time
    case reversing:
      makerDrive(B0101);
      if (stateStartTime + 500 < millis()) {
        robotState = stopping;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Steering soft left (continue driving)
    case soft_left:
      makerDrive(B1110);  // coast left motor
      if (distanceRight > steerDistance) {
        robotState = driving;
        stateStartTime = millis();  // Remember when switching states started
      }
      // If object become to close -> brake
      if (not(checkDistance(brakeDistance))) {
        robotState = stopping;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Steering soft right (continue driving)
    case soft_right:
      makerDrive(B1011); // coast right motor
      if (distanceLeft > steerDistance) {
        robotState = driving;
        stateStartTime = millis();  // Remember when switching states started
      }
      // If object become to close -> brake
      if (not(checkDistance(brakeDistance))) {
        robotState = stopping;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Something went wrong ?
    default:
      robotState = booting;
      stateStartTime = millis();  // Remember when switching states started
      break;

  }  // end switch

  // Refresh the display
  updateDisplay();
}

/****************************************************************
      All sensors detect free route ?
****************************************************************/
bool checkDistance(int brakeDistance) {
  // Check if any of the sensors detects an object that is too close
  bool distOK = true;  
  for (int s=0; s<tSENS; s++) {
    if (distances[s] < brakeDistance) {
      distOK = false;
    }
  }
  return distOK;
}

/****************************************************************
      Handle HC-SR04 ensors
****************************************************************/
void checkHCSR04(unsigned long timeout)
{
  unsigned long startWait;

  // Loop trough all sensors
  for (int s = 0; s < tSENS; s++)
  {
    startEcho = 0;  // Reset start time
    stopEcho = 0;   // Reset stop time

    // Initiate next trigger
    // digitalWrite(triggerPins[s], LOW);  // rest of loop already takes > 2µs
    // delayMicroseconds(2);
    digitalWrite(pinTriggers[s], HIGH);    // HIGH pulse for at least 10µs
    delayMicroseconds(10);
    digitalWrite(pinTriggers[s], LOW);    // Set LOW again

    startWait = millis();   // remember starting to wait for the echo signal
    while (startEcho == 0 or stopEcho == 0) {
      // Keep looping unill start and stoptime is not 0 OR a timeout occurs
      if (startWait + timeout < millis())
      {
        // Timeout (object out of range for sensor)
        startEcho = 0;
        stopEcho = 20000;
        break;  // exit the while loop
      }
    }
    // Store latest measurement
    noInterrupts();   // cli()
    distances[s] = (stopEcho - startEcho) * (float)soundSpeed / 1000.0 / 10.0 / 2.0;   // in cm
    interrupts();   // sei();
  }
}

/****************************************************************
      Maker Drive
****************************************************************/
void makerDrive(byte controlByte) {
  /*
      controlByte     |
      M1A M1B M2A M2B | A B
       0   0   0   0  | 0 0 = Brake
       1   0   1   0  | 1 0 = Forward
       0   1   0   1  | 0 1 = Backward
       1   1   1   1  | 1 1 = Coast

      controlSpeed
      0: 0%
      127: 50%
      255: 100%
  */
  byte highSpeed;
  byte lowSpeed;

  // Handle speed values
  encoderCounter = constrain(encoderCounter, 0, 255);   // values can only be 0-255
  if (!useEncoder) {
    // No encoder used -> set speed values to fixed settings
    highSpeed = fullSpeed;
    lowSpeed = halfSpeed;
  }
  else {
    // Encoder used -> set speed values to encoder settings
    highSpeed = encoderCounter;
    lowSpeed = byte(float(encoderCounter) / 3 * 2);
  }
    
  bool M1A = bool(controlByte >> 3 & B1);  // Mask bit for M1A
  bool M1B = bool(controlByte >> 2 & B1);  // Mask bit for M1B
  bool M2A = bool(controlByte >> 1 & B1);  // Mask bit for M2A
  bool M2B = bool(controlByte >> 0 & B1);  // Mask bit for M2B

  switch (controlByte) {
    case B0000:   // Brake
    case B1010:   // Forward
      // Full speed
      analogWrite(pinDriveM1A, highSpeed * M1A);
      analogWrite(pinDriveM1B, highSpeed * M1B);
      analogWrite(pinDriveM2A, highSpeed * M2A);
      analogWrite(pinDriveM2B, highSpeed * M2B);
      break;
    case B1111:   // Coast
      digitalWrite(pinDriveM1A, 1);
      digitalWrite(pinDriveM1B, 1);
      digitalWrite(pinDriveM2A, 1);
      digitalWrite(pinDriveM2B, 1);
      break;
    case B0101:   // Backward
    case B1001:   // Hard Right (L forward ; R reverse)
    case B0110:   // Hard Left  (R forward ; L reverse)
      // Reduced speed
      analogWrite(pinDriveM1A, lowSpeed * M1A);
      analogWrite(pinDriveM1B, lowSpeed * M1B);
      analogWrite(pinDriveM2A, lowSpeed * M2A);
      analogWrite(pinDriveM2B, lowSpeed * M2B);
      break;
    case B1011:   // Soft Right (L fast ; R slow)
      // Mixed speed
      analogWrite(pinDriveM1A, highSpeed * M1A);
      analogWrite(pinDriveM1B, highSpeed * M1B);
      analogWrite(pinDriveM2A, lowSpeed * 1);  // Overwrite coast
      analogWrite(pinDriveM2B, lowSpeed * 0);  // Overwrite coast
      break;
    case B1110:   // Soft Left (L slow ; R fast)
      // Mixed speed
      analogWrite(pinDriveM1A, lowSpeed * 1);  // Overwrite coast
      analogWrite(pinDriveM1B, lowSpeed * 0);  // Overwrite coast
      analogWrite(pinDriveM2A, highSpeed * M2A);
      analogWrite(pinDriveM2B, highSpeed * M2B);
      break;
    default:
      // Unexpected value ==> brake
      digitalWrite(pinDriveM1A, LOW);
      digitalWrite(pinDriveM1B, LOW);
      digitalWrite(pinDriveM2A, LOW);
      digitalWrite(pinDriveM2B, LOW);
      break;
  } // END SWITCH
}

/****************************************************************
      Display update
****************************************************************/
void updateDisplay() {
  // Build display text
  String displayText;

  // Display distances on normal font
  if (not(useBigFont)) {
    // Display distance measured by the front sensor
    displayText = "Distances L/C/R (cm):\n";
    for (int s=0; s<(tSENS); s++) {
      displayText += String(distances[s]);
      if (s < (tSENS - 1)) { displayText += "-"; }
    }
    displayText += "\n";
    displayText += "Action:";
    // Display current state of  the robot
  }
  if (robotState == stopped) { displayText += "stopped"; }
  else if (robotState == stopping) { displayText += "stopping"; }
  else if (robotState == driving) { displayText += "driving"; }
  else if (robotState == reversing) { displayText += "reversing"; }
  else if (robotState == booting) { displayText += "booting"; }
  else if (robotState == paused) { displayText += "paused"; }
  else if (robotState == hard_left) { displayText += "hard left"; }
  else if (robotState == hard_right) { displayText += "hard right"; }
  else if (robotState == soft_left) { displayText += "soft left"; }
  else if (robotState == soft_right) { displayText += "soft right"; }
  displayText += "\n";
  displayText += "Speed: ";
  displayText += String(encoderCounter);
  displayText += "\n";

  // If display was not detected, send to serial monitor in stead
  if (displayPresent) {
    display.clearDisplay();             // Clear display
    if (useBigFont) {
      display.setTextSize(2);             // Big 2:1 pixel scale
    }
    else {
      display.setTextSize(1);             // Normal 1:1 pixel scale
    }
    display.setTextColor(WHITE);        // Draw white text
    display.setCursor(0, 0);            // Start at top-left corner
    display.println(displayText);       // Write text to buffer
    display.display();                  // Refresh display
  }
  else {
    Serial.println(displayText);        // Show on serial monitor instead
  }
}

/****************************************************************
      RGB LED update
****************************************************************/
void updateRGBLEDs() {
  // COMMON ANODE --> inverse logic
  // LOW is LED ON ; HIGH = LED OFF
  switch(robotState) {
    case booting:
      // White
      digitalWrite(pinEncLED_R, LOW);
      digitalWrite(pinEncLED_G, LOW);
      digitalWrite(pinEncLED_B, LOW);
      break;
    case stopped:
    case stopping:
      // Orange
      digitalWrite(pinEncLED_R, LOW);
      analogWrite(pinEncLED_G, 127);
      digitalWrite(pinEncLED_B, HIGH);
      break;
    case paused:
      // Red
      digitalWrite(pinEncLED_R, LOW);
      digitalWrite(pinEncLED_G, HIGH);
      digitalWrite(pinEncLED_B, HIGH);
      break;
    case driving:
      // Green
      digitalWrite(pinEncLED_R, HIGH);
      digitalWrite(pinEncLED_G, LOW);
      digitalWrite(pinEncLED_B, HIGH);
      break;
    case reversing:
    case hard_left:
    case hard_right:
    case soft_left:
    case soft_right:
      // Yellow
      digitalWrite(pinEncLED_R, LOW);
      digitalWrite(pinEncLED_G, LOW);
      digitalWrite(pinEncLED_B, HIGH);
      break;
    default:
      // Red
      digitalWrite(pinEncLED_R, LOW);
      digitalWrite(pinEncLED_G, HIGH);
      digitalWrite(pinEncLED_B, HIGH);
      break;
  } // END SWITCH
}

/****************************************************************
      INTERRUPT handling (keep as fast(=short) as possible)
****************************************************************/
// Echo pin
void ISR_ECHO()
{
  // bool pinRead = digitalRead(pinEcho);  // Use faster port registers for HC-SR04 instead

  // For ATmega328(P) chips, INT0 as at pin PD2
  bool pinRead = bool(PIND >> 2 & B0001);  // Read state of interrupt pin

  if (pinRead)  {
    // State = HIGH -> Remember start time
    startEcho = micros();
  }
  else {
    // State is LOW -> Remember stop time
    stopEcho = micros();
  }
}

/****************************************************************
      INTERRUPT handling encoder
****************************************************************/
// Encoder pin
void ISR_ENC()
{
  // static unsigned byte encoder_state = 0;     // Why not working ?
  static unsigned char encoder_state = 0;     // Only exists in this funtion and init once
  encoder_state = encoder_state << 2;         // Shift 2 bit to the left

  //  encoder_state = encoder_state | digitalRead(pinEncA);           // New value A on bit 0
  //  encoder_state = encoder_state | (digitalRead(pinEncB) << 1));   // New value B on bit 1

  // For ATmega328(P) chips, INT1 = pin 3 = PD3 , pin 4 = PD4
  byte pinRead = PIND;
  encoder_state = encoder_state | (PIND >> 3 & 1);    // New value A on bit 0 [xxxxxxxA]
  encoder_state = encoder_state | (PIND >> 3 & 10);   // New value B on bit 1 [xxxxxxBx]

  encoder_state = encoder_state & B00001111;          // Mask with 0x0F
  // Format is now 0000 <prev B> <prev A> <cur B> <cur A>

  // Decide counter direction (comment / uncomment for best stability)
  switch (encoder_state) {
    case B1001: // B high to low ; A low to high
    // case B0110: // B low to high ; A high to low
      encoderCounter++;
      break;
    // case B0011: // A and B low to high
    case B1100: // A and B high to low
      encoderCounter--;
      break;
    default:
      ; // unsure state
      break;
  }
}
//END

References
Arduino Boards: Arduino Uno R3 – Arduino Nano V3 – Arduino Micro
Arduino Software: Arduino IDE
Arduino Playground: Library NewPing
Arduino forum: Arduino Micro Interrupts 6
Arduino Reference: Port RegistersattachInterrupt()interrupt()noInterrrupt()pulseIn()pulseInLong()Servo library
Random Nerd Tutorials: Complete Guide for Ultrasonic Sensor HC-SR04 with Arduin
Circuit Digest: Arduino Interrupts Tutorial
Electro Noobs: Arduino register control, timers and interruptions
Wikipedia: Speed of sound – Echolocation