Performance – Modifications of the sketch  

Hi, I am trying to bring som enew functions to my Evo.

I successfully added some led matrix eyes but now I am struggeling with some kind of collision avoidance. I am using a VL53L0X laser tof sensot that comminicates via i2c (I am using the Adfruit library Adafruit_VL53L0X).

I added my code into the slow loop section where I alreday had the led code. But now my Evo is stuggering and it looks like it hasn’t enought tome to finish the balancing code loop in time.

Did somebody implement a distance sensor successfully and can point me to some code?
Or give me some hints what I can do to solve the problem?

Parts I added:

...
 } // End of medium loop
  else if (slow_loop_counter >= 100) // 1Hz
  {     

    VL53L0X_RangingMeasurementData_t measure;
    
    lox.rangingTest(&measure, false); 

      if (measure.RangeMilliMeter  0) {
          lookRight();
        }
          else if (steering < 0) {
            lookLeft();
          }
          else {
            if (throttle == 0) {
              if (wait == 1) eyesClosed();
              else {
                lookStraight();
                singleDelay.start(10000);
              }
            if(singleDelay.elapsed()) wait = 1;
            }
            else {
              lookStraight();
              wait = 0;
            }
          }
      }    
    
    digitalWrite(RED_LED, HIGH);   // RED LED HEARTBEAT
    ...

Full code:

// BROBOT EVO 2 by JJROBOTS (M0 version)
// SELF BALANCE ARDUINO ROBOT WITH STEPPER MOTORS CONTROLLED WITH YOUR SMARTPHONE
// JJROBOTS BROBOT KIT: New JJROBOTS DEVIA board M0
// This code is prepared for new JJROBOTS DEVIA board based on ARM M0 microcontroles with Wifi.
// Author: JJROBOTS.COM
// Date: 02/09/2014
// Updated: 08/10/2019
// Version: 3.05
// License: GPL v2
// Compiled and tested with Arduino 1.8.3. This new version of code does not need external libraries (only Arduino standard libraries)
// Project URL:  http://jjrobots.com/b-robot-evo-2-much-more-than-a-self-balancing-robot  (Features,documentation,build instructions,how it works, SHOP,...)
// New updates:
//   - New default parameters specially tuned for BROBOT EVO 2 version (More agile, more stable...)
//   - New Move mode with position control (for externally programming the robot with a Blockly or pyhton programming interfaces)
//   - New telemtry packets send to TELEMETRY IP for monitoring Battery, Angle, ... (old battery packets for touch osc not working now)
//   - Default telemetry server is 192.168.4.2 (first client connected to the robot)
//  Get the free android app (jjrobots) from google play. For IOS users you need to use TouchOSC App + special template (info on jjrobots page)
//  Thanks to our users on the forum for the new ideas. Specially sasa999, KomX, ...

// The board needs at least 10-15 seconds with no motion (robot steady) at beginning to give good values... Robot move slightly when it´s ready!
// MPU6050 IMU connected via I2C bus. Angle estimation using complementary filter (fusion between gyro and accel)
// Angle calculations and control part is running at 100Hz

// The robot is OFF when the angle is high (robot is horizontal). When you start raising the robot it
// automatically switch ON and start a RAISE UP procedure.
// You could RAISE UP the robot also with the robot arm servo (Servo button on the interface)
// To switch OFF the robot you could manually put the robot down on the floor (horizontal)

// We use a standard PID controllers (Proportional, Integral derivative controller) for robot stability
// More info on the project page: How it works page at jjrobots.com
// We have a PI controller for speed control and a PD controller for stability (robot angle)
// The output of the control (motors speed) is integrated so it´s really an acceleration not an speed.

// We control the robot from a WIFI module using OSC standard UDP messages
// You need an OSC app to control de robot (Custom free JJRobots APP for android, and TouchOSC APP for IOS)
// Join the module Wifi Access Point (by default: JJROBOTS_XX) with your Smartphone/Tablet...
//   Wifi password: 87654321
// For TouchOSC users (IOS): Install the BROBOT layout into the OSC app (Touch OSC) and start play! (read the project page)
// OSC controls:
//    fader1: Throttle (0.0-1.0) OSC message: /1/fader1
//    fader2: Steering (0.0-1.0) OSC message: /1/fader2
//    push1: Move servo arm (and robot raiseup) OSC message /1/push1 
//    if you enable the touchMessage on TouchOSC options, controls return to center automatically when you lift your fingers
//    PRO mode (PRO button). On PRO mode steering and throttle are more aggressive
//    PAGE2: PID adjustements [optional][dont touch if you dont know what you are doing...;-) ]

#include 
#include 

// include library for led eyes 
#include "LedControl.h"

#include 
VirtualDelay singleDelay; // default = millis

// include laser tof sensor
#include "Adafruit_VL53L0X.h"
Adafruit_VL53L0X lox = Adafruit_VL53L0X();

// Uncomment this lines to connect to an external Wifi router (join an existing Wifi network)
//#define EXTERNAL_WIFI
//#define WIFI_SSID "YOUR_WIFI"
//#define WIFI_PASSWORD "YOUR_PASSWORD"
//#define WIFI_IP "192.168.1.101"  // Force ROBOT IP
//#define TELEMETRY "192.168.1.38" // Tlemetry server port 2223

#define TELEMETRY "192.168.4.2" // Default telemetry server (first client) port 2223

// NORMAL MODE PARAMETERS (MAXIMUN SETTINGS)o
#define MAX_THROTTLE 550
#define MAX_STEERING 140
#define MAX_TARGET_ANGLE 14

// PRO MODE = MORE AGGRESSIVE (MAXIMUN SETTINGS)
#define MAX_THROTTLE_PRO 780   // Max recommended value: 860
#define MAX_STEERING_PRO 260   // Max recommended value: 280
#define MAX_TARGET_ANGLE_PRO 24   // Max recommended value: 32

// Default control terms for EVO 2
#define KP 0.32       
#define KD 0.050     
#define KP_THROTTLE 0.080 
#define KI_THROTTLE 0.1 
#define KP_POSITION 0.06  
#define KD_POSITION 0.45  
//#define KI_POSITION 0.02

// Control gains for raiseup (the raiseup movement requiere special control parameters)
#define KP_RAISEUP 0.1   
#define KD_RAISEUP 0.16   
#define KP_THROTTLE_RAISEUP 0   // No speed control on raiseup
#define KI_THROTTLE_RAISEUP 0.0

#define MAX_CONTROL_OUTPUT 500
#define ITERM_MAX_ERROR 30   // Iterm windup constants for PI control 
#define ITERM_MAX 10000

#define ANGLE_OFFSET 0.0  // Offset angle for balance (to compensate robot own weight distribution)

// Servo definitions
#define SERVO_AUX_NEUTRO 1500  // Servo neutral position
#define SERVO_MIN_PULSEWIDTH 700
#define SERVO_MAX_PULSEWIDTH 2500

#define SERVO2_NEUTRO 1500
#define SERVO2_RANGE 1400

// Telemetry
#define TELEMETRY_BATTERY 1
#define TELEMETRY_ANGLE 1
//#define TELEMETRY_DEBUG 1  // Dont use TELEMETRY_ANGLE and TELEMETRY_DEBUG at the same time!

#define ZERO_SPEED 65535
#define MAX_ACCEL 14 //14     // Maximun motor acceleration (MAX RECOMMENDED VALUE: 20) (default:14)

#define MICROSTEPPING 16   // 8 or 16 for 1/8 or 1/16 driver microstepping (default:16)

#define DEBUG 0   // 0 = No debug info (default) DEBUG 1 for console output

// AUX definitions
#define CLR(x,y) (x&=(~(1<<y)))
#define SET(x,y) (x|=(1<<y))
#define RAD2GRAD 57.2957795
#define GRAD2RAD 0.01745329251994329576923690768489

// Board leds
#define GREEN_LED A1
#define RED_LED A2
#define SWITCH_IN 30 //PorB03

// LED eyes
#define CLK_PIN   SCK  // or SCK
#define DATA_PIN  MOSI  // or MOSI
#define CS_PIN    MISO  // or SS /LOAD

//LedControl lc=LedControl(12,11,10,2);
LedControl lc=LedControl(DATA_PIN,CLK_PIN,CS_PIN,2);

/* we always wait a bit between updates of the display */
unsigned long delaytime=1500;

String MAC;  // MAC address of Wifi module

uint8_t cascade_control_loop_counter = 0;
uint8_t loop_counter;       // To generate a medium loop 40Hz
uint8_t slow_loop_counter;  // slow loop 2Hz
uint8_t sendBattery_counter; // To send battery status
int16_t BatteryValue;

long timer_old;
long timer_value;
float debugVariable;
float dt;

// Angle of the robot (used for stability control)
float angle_adjusted;
float angle_adjusted_Old;
float angle_adjusted_filtered=0.0;

// Default control values from constant definitions
float Kp = KP;
float Kd = KD;
float Kp_thr = KP_THROTTLE;
float Ki_thr = KI_THROTTLE;
float Kp_user = KP;
float Kd_user = KD;
float Kp_thr_user = KP_THROTTLE;
float Ki_thr_user = KI_THROTTLE;
float Kp_position = KP_POSITION;
float Kd_position = KD_POSITION;
bool newControlParameters = false;
bool modifing_control_parameters = false;
int16_t position_error_sum_M1;
int16_t position_error_sum_M2;
float PID_errorSum;
float PID_errorOld = 0;
float PID_errorOld2 = 0;
float setPointOld = 0;
float target_angle;
int16_t throttle;
float steering;
float max_throttle = MAX_THROTTLE;
float max_steering = MAX_STEERING;
float max_target_angle = MAX_TARGET_ANGLE;
float control_output;
float angle_offset = ANGLE_OFFSET;

boolean positionControlMode = false;
uint8_t mode;  // mode = 0 Normal mode, mode = 1 Pro mode (More agressive)

int16_t motor1;
int16_t motor2;

// position control
volatile int32_t steps1;
volatile int32_t steps2;
int32_t target_steps1;
int32_t target_steps2;
int16_t motor1_control;
int16_t motor2_control;

int16_t speed_M1, speed_M2;        // Actual speed of motors
int8_t  dir_M1, dir_M2;            // Actual direction of steppers motors
int16_t actual_robot_speed;        // overall robot speed (measured from steppers speed)
int16_t actual_robot_speed_Old;
float estimated_speed_filtered;    // Estimated robot speed

// OSC output variables
uint8_t OSCpage;
uint8_t OSCnewMessage;
float OSCfader[4];
float OSCxy1_x;
float OSCxy1_y;
float OSCxy2_x;
float OSCxy2_y;
uint8_t OSCpush[4];
uint8_t OSCtoggle[4];
uint8_t OSCmove_mode;
int16_t OSCmove_speed;
int16_t OSCmove_steps1;
int16_t OSCmove_steps2;

Servo servo1;
Servo servo2;

boolean wait = 0;

  /* here is the data for the characters */
  byte auge[8]={B11111111,B11111111,B11000011,B11000011,B11000011,B11000011,B11111111,B11111111 };
  byte auge_rechts[8]={B11111111,B11000011,B11000011,B11000011,B11000011,B11111111,B11111111,B11111111 };
  byte auge_links[8]={B11111111,B11111111,B11111111,B11000011,B11000011,B11000011,B11000011,B11111111 };
  byte auge_oben[8]={B11111111,B11111111,B10000111,B10000111,B10000111,B10000111,B11111111,B11111111 };
  byte auge_unten[8]={B11111111,B11111111,B11100001,B11100001,B11100001,B11100001,B11111111,B11111111 };
  byte auge_zu[8]={B00000000,B00010000,B00010000,B00010000,B00010000,B00010000,B00010000,B00000000 };
  byte auge_schock[8]={B11111111,B10000001,B10000001,B10000001,B10000001,B10000001,B10000001,B11111111 };

void lookLeft() {
  for(int row=0;row<8;row++) {
    lc.setRow(0,row,auge_links[row]);
    lc.setRow(1,row,auge_links[row]);
  }
}

void lookRight() {
  for(int row=0;row<8;row++) {
    lc.setRow(0,row,auge_rechts[row]);
    lc.setRow(1,row,auge_rechts[row]);
  }
  
}

void lookStraight() {
  for(int row=0;row<8;row++) {
    lc.setRow(0,row,auge[row]);
    lc.setRow(1,row,auge[row]);
  }
}

void eyesClosed() {
  for(int row=0;row<8;row++) {
    lc.setRow(1,row,auge_zu[row]);
    lc.setRow(0,row,auge_zu[row]);
  }
}

void eyesShocked() {
  for(int row=0;row 0
  delay(9000);
#else
  delay(1000);
#endif
  SerialUSB.println("JJROBOTS");
  delay(200);
  SerialUSB.println("Don't move for 10 sec...");
  MPU6050_setup();  // setup MPU6050 IMU
  delay(500);

  // With the new ESP8266 WIFI MODULE WE NEED TO MAKE AN INITIALIZATION PROCESS
  SerialUSB.println("WIFI init");
  Serial1.flush();
  Serial1.print("+++");  // To ensure we exit the transparent transmision mode
  delay(100);
  ESPsendCommand(String("AT"), String("OK"), 1);
  //ESPsendCommand("AT+RST", "OK", 2); // ESP Wifi module RESET
  //ESPwait("ready", 6);
  ESPsendCommand(String("AT+GMR"), String("OK"), 5);

#ifdef EXTERNAL_WIFI
  ESPsendCommand(String("AT+CWQAP", String("OK"), 3);
  ESPsendCommand(String("AT+CWMODE=1", String("OK"), 3);
  //String auxCommand = (String)"AT+CWJAP="+WIFI_SSID+","+WIFI_PASSWORD;
  String auxCommand = String("AT+CWJAP="") + String(WIFI_SSID) + String("","") + String(WIFI_PASSWORD) + String(""");    
  ESPsendCommand(auxCommand, String("OK"), 14);
#ifdef WIFI_IP
  String auxCommand2 = String("AT+CIPSTA="") + String(WIFI_IP) + String(""");  
  ESPsendCommand(auxCommand2, String("OK"), 4);
#endif
  ESPsendCommand(String("AT+CIPSTA?"), String("OK"), 4);
#else  // Deafault : we generate a wifi network
  Serial1.println("AT+CIPSTAMAC?");
  ESPgetMac();
  SerialUSB.print("MAC:");
  SerialUSB.println(MAC);
  delay(200);
  //ESPsendCommand(String("AT+CWQAP"), String("OK"), 3);
  ESPsendCommand(String("AT+CWMODE=2"), String("OK"), 3); // Soft AP mode
  // Generate Soft AP. SSID=JJROBOTS_XX (XX= user MAC last characters), PASS=87654321
  String cmd =  String("AT+CWSAP="JJROBOTS_") + MAC.substring(MAC.length() - 2, MAC.length()) + String("","87654321",5,3");
  ESPsendCommand(cmd, String("OK"), 6);
#endif
  // Start UDP SERVER on port 2222, telemetry port 2223
  SerialUSB.println("Start UDP server");
  ESPsendCommand(String("AT+CIPMUX=0"), String("OK"), 3);  // Single connection mode
  delay(10);
  ESPsendCommand(String("AT+CIPMODE=1"), String("OK"), 3); // Transparent mode
  delay(10);
  String Telemetry = String("AT+CIPSTART="UDP","") + String(TELEMETRY) + String("",2223,2222,0");
  ESPsendCommand(Telemetry, String("CONNECT"), 3);
  SerialUSB.println(Telemetry);
  delay(200);
  ESPsendCommand(String("AT+CIPSEND"), String('>'), 2); // Start transmission (transparent mode)

  // Calibrate gyros
  MPU6050_calibrate();
  
  // Init servos
  SerialUSB.println("Servo init");
  BROBOT_initServo();
  BROBOT_moveServo1(SERVO_AUX_NEUTRO);
 
  // STEPPER MOTORS INITIALIZATION
  SerialUSB.println("Steper motors initialization...");
  timersConfigure();
  SerialUSB.println("Timers initialized");
  timersStart(); //starts the timers
  SerialUSB.println("Timers started");
  delay(200);


    /*
   The MAX72XX is in power-saving mode on startup,
   we have to do a wakeup call
   */
  lc.shutdown(0,false);
  lc.shutdown(1,false);
  /* Set the brightness to a medium values */
  lc.setIntensity(0,8);
  lc.setIntensity(1,8);
  /* and clear the display */
  lc.clearDisplay(0);
  lc.clearDisplay(1);

  for(int row=0;row<8;row++) {
    lc.setRow(1,row,auge_zu[row]);
    lc.setRow(0,row,auge_zu[row]);
  }
  

  // Little motor vibration and servo move to indicate that robot is ready
  digitalWrite(11, LOW);  // Motors enable
  for (uint8_t k = 0; k < 5; k++)
  {
    setMotorSpeedM1(5);
    setMotorSpeedM2(5);
    BROBOT_moveServo1(SERVO_AUX_NEUTRO + 100);
    BROBOT_moveServo2(SERVO2_NEUTRO + 100);
    delay(200);
    setMotorSpeedM1(-5);
    setMotorSpeedM2(-5);
    BROBOT_moveServo1(SERVO_AUX_NEUTRO - 100);
    BROBOT_moveServo2(SERVO2_NEUTRO - 100);
    delay(200);
  }
  BROBOT_moveServo1(SERVO_AUX_NEUTRO);
  BROBOT_moveServo2(SERVO2_NEUTRO);
  digitalWrite(11, HIGH);  // Motors disable

  for(int row=0;row 0)
          steering = (steering * steering + 0.5 * steering) * max_steering;
        else
          steering = (-steering * steering + 0.5 * steering) * max_steering;
      }

      if ((mode == 0) && (OSCtoggle[0]))
      {
        // Change to PRO mode
        max_throttle = MAX_THROTTLE_PRO;
        max_steering = MAX_STEERING_PRO;
        max_target_angle = MAX_TARGET_ANGLE_PRO;
        mode = 1;
      }
      if ((mode == 1) && (OSCtoggle[0] == 0))
      {
        // Change to NORMAL mode
        max_throttle = MAX_THROTTLE;
        max_steering = MAX_STEERING;
        max_target_angle = MAX_TARGET_ANGLE;
        mode = 0;
      }
    }
    else if (OSCpage == 2) { // OSC page 2
      // Check for new user control parameters
      readControlParameters();
    }
#if DEBUG==1
    SerialUSB.print(throttle);
    SerialUSB.print(" ");
    SerialUSB.println(steering);
#endif
  } // End new OSC message

  timer_value = micros();

  // New IMU data?
  digitalWrite(GREEN_LED, HIGH);
  if (MPU6050_newData())
  {
    MPU6050_read_3axis();
    digitalWrite(GREEN_LED, LOW);
    loop_counter++;
    slow_loop_counter++;
    dt = (timer_value - timer_old) * 0.000001; // dt in seconds
    timer_old = timer_value;

    angle_adjusted_Old = angle_adjusted;
    // Get new orientation angle from IMU (MPU6050)
    float MPU_sensor_angle = MPU6050_getAngle(dt);
    angle_adjusted = MPU_sensor_angle + angle_offset;
    if ((MPU_sensor_angle>-15)&&(MPU_sensor_angle<15))
      angle_adjusted_filtered = angle_adjusted_filtered*0.99 + MPU_sensor_angle*0.01;
      
#if DEBUG==1
    SerialUSB.print(int(dt*1000));
    SerialUSB.print(" ");
    SerialUSB.print(angle_adjusted);
    //SerialUSB.print(",");
    //SerialUSB.print(angle_adjusted_filtered);
    SerialUSB.println();
#endif
    //SerialUSB.print("t");

    // We calculate the estimated robot speed:
    // Estimated_Speed = angular_velocity_of_stepper_motors(combined) - angular_velocity_of_robot(angle measured by IMU)
    actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward  

    int16_t angular_velocity = (angle_adjusted - angle_adjusted_Old) * 25.0; // 25 is an empirical extracted factor to adjust for real units
    int16_t estimated_speed = -actual_robot_speed + angular_velocity;
    estimated_speed_filtered = estimated_speed_filtered * 0.9 + (float)estimated_speed * 0.1; // low pass filter on estimated speed

#if DEBUG==2
    SerialUSB.print(angle_adjusted);
    SerialUSB.print(" ");
    SerialUSB.println(estimated_speed_filtered);
#endif

    if (positionControlMode)
    {
      // POSITION CONTROL. INPUT: Target steps for each motor. Output: motors speed
      motor1_control = positionPDControl(steps1, target_steps1, Kp_position, Kd_position, speed_M1);
      motor2_control = positionPDControl(steps2, target_steps2, Kp_position, Kd_position, speed_M2);

      // Convert from motor position control to throttle / steering commands
      throttle = (motor1_control + motor2_control) / 2;
      throttle = constrain(throttle, -190, 190);
      
      steering = motor2_control - motor1_control;
      
      steering = constrain(steering, -50, 50);
     
    }

#if DEBUG==4
    SerialUSB.print(throttle);
    SerialUSB.print(" ");
    SerialUSB.println(steering);
#endif


    // ROBOT SPEED CONTROL: This is a PI controller.
    //    input:user throttle(robot speed), variable: estimated robot speed, output: target robot angle to get the desired speed
    target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
    target_angle = constrain(target_angle, -max_target_angle, max_target_angle); // limited output


#if DEBUG==3
    SerialUSB.print(angle_adjusted);
    SerialUSB.print(" ");
    SerialUSB.print(estimated_speed_filtered);
    SerialUSB.print(" ");
    SerialUSB.println(target_angle);
#endif

    // Stability control (100Hz loop): This is a PD controller.
    //    input: robot target angle(from SPEED CONTROL), variable: robot angle, output: Motor speed
    //    We integrate the output (sumatory), so the output is really the motor acceleration, not motor speed.
    control_output += stabilityPDControl(dt, angle_adjusted, target_angle, Kp, Kd);
    control_output = constrain(control_output, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT); // Limit max output from control

    // The steering part from the user is injected directly to the output
    motor1 = control_output + steering;
    motor2 = control_output - steering;

    // Limit max speed (control output)
    motor1 = constrain(motor1, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);
    motor2 = constrain(motor2, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);

    int angle_ready;
    if (OSCpush[0])     // If we press the SERVO button we start to move
      angle_ready = 82;
    else
      angle_ready = 74;  // Default angle
    if ((angle_adjusted  -angle_ready)) // Is robot ready (upright?)
    {
      // NORMAL MODE
      digitalWrite(11, LOW);  // Motors enable
      // NOW we send the commands to the motors
      setMotorSpeedM1(motor1);
      setMotorSpeedM2(motor2);
    }
    else   // Robot not ready (flat), angle > angle_ready => ROBOT OFF
    {
      digitalWrite(11, HIGH);  // Disable motors
      setMotorSpeedM1(0);
      setMotorSpeedM2(0);
      PID_errorSum = 0;  // Reset PID I term
      Kp = KP_RAISEUP;   // CONTROL GAINS FOR RAISE UP
      Kd = KD_RAISEUP;
      Kp_thr = KP_THROTTLE_RAISEUP;
      Ki_thr = KI_THROTTLE_RAISEUP;
      // RESET steps
      steps1 = 0;
      steps2 = 0;
      positionControlMode = false;
      OSCmove_mode = false;
      throttle = 0;
      steering = 0;
    }

    // Push1 Move servo arm
    if (OSCpush[0])  // Move arm
    {
      if (angle_adjusted > -40)
        BROBOT_moveServo1(SERVO_MIN_PULSEWIDTH);
      else
        BROBOT_moveServo1(SERVO_MAX_PULSEWIDTH);
    }
    else
      BROBOT_moveServo1(SERVO_AUX_NEUTRO);

    // Servo2
    BROBOT_moveServo2(SERVO2_NEUTRO + (OSCfader[2] - 0.5) * SERVO2_RANGE);

    // Normal condition?
    if ((angle_adjusted  -56))
    {
      Kp = Kp_user;            // Default user control gains
      Kd = Kd_user;
      Kp_thr = Kp_thr_user;
      Ki_thr = Ki_thr_user;
    }
    else    // We are in the raise up procedure => we use special control parameters
    {
      Kp = KP_RAISEUP;         // CONTROL GAINS FOR RAISE UP
      Kd = KD_RAISEUP;
      Kp_thr = KP_THROTTLE_RAISEUP;
      Ki_thr = KI_THROTTLE_RAISEUP;
    }

  } // End of new IMU data
  digitalWrite(GREEN_LED, LOW);

  // Medium loop 7.5Hz
  if (loop_counter >= 15)
  {
    
    
    loop_counter = 0;
    // Telemetry here?
#if TELEMETRY_ANGLE==1
    char auxS[25];
    int ang_out = constrain(int(angle_adjusted * 10),-900,900);
    sprintf(auxS, "$tA,%+04d", ang_out);
    Serial1.println(auxS);
#endif
#if TELEMETRY_DEBUG==1
    char auxS[50];
    sprintf(auxS, "$tD,%d,%d,%ld", int(angle_adjusted * 10), int(estimated_speed_filtered), steps1);
    Serial1.println(auxS);
#endif

  } // End of medium loop
  else if (slow_loop_counter >= 100) // 1Hz
  {     

    VL53L0X_RangingMeasurementData_t measure;
    
    lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    //SerialUSB.print("Distance (mm): "); SerialUSB.println(measure.RangeMilliMeter);

      if (measure.RangeMilliMeter  0) {
          lookRight();
        }
          else if (steering = 3) { //Every 3 seconds we send a message
      sendBattery_counter = 0;
      SerialUSB.print("B");
      SerialUSB.println(BatteryValue);
      char auxS[25];
      sprintf(auxS, "$tB,%04d", BatteryValue);
      Serial1.println(auxS);
    }
#endif
  }  // End of slow loop
  if (slow_loop_counter>=5)       // RED LED HEARTBEAT
    digitalWrite(RED_LED, LOW);
}