Мертвая зона для vl53l0x

Вечер добрый!
Нужно добавить мертвую зону в скетч.
Чтобы "если расстояние меньше 50см мотор не работал.
500р

Код (C++):

#include <Wire.h>

#include <VL53L0X.h>


VL53L0X sensor;
// Раскомментируйте строку ниже, чтобы использовать дальний режим.
// Это увеличивает чувствительность датчика и расширяет его потенциальный диапазон,
// но увеличивает вероятность получения неточных показаний
// из-за отражений от объектов, отличных от намеченной цели.
// Лучше всего работает в темноте.
#define LONG_RANGE

// Раскомментируйте одну из двух строк ниже, чтобы получить:
// - более высокую скорость за счет меньшей точности ИЛИ
// - более высокую точность за счет меньшей скорости
//#define HIGH_SPEED
#define HIGH_ACCURACY
///////////////////////

int out1;
#define PULSE1 A3    /* Мотор №1 */
#define LED 13

#define pulse_ms 100  /* Длительность импульса мотора (мс) */

#define L1 50
#define L2 100
#define L3 150

int Level1 = 0;

int i = 0;

int t1st;
int dist1 = 0;
int count1 = 0;
int low1 = 0;
int pulse1_stp = 0;
void setup()
{
pinMode(13, OUTPUT);
digitalWrite(13, 0);


Serial.begin(9600);
  Wire.begin();

  sensor.setTimeout(200);

  if (!sensor.init())
  {
    Serial.println("Не удалось обнаружить и инициализировать датчик!");
    while (1) {}
  }

#if defined LONG_RANGE
  // понижает предел скорости обратного сигнала (по умолчанию 0,25 MCPS (мчип/с))
  sensor.setSignalRateLimit(0.1);
  // увеличить периоды лазерного импульса (по умолчанию 14 и 10 PCLK)
  // * - PCLK — это частота периферии
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
#endif

#if defined HIGH_SPEED
  // уменьшить тайминг до 20 мс (по умолчанию около 33 мс)
  sensor.setMeasurementTimingBudget(50000);
#elif defined HIGH_ACCURACY
  // увеличить тайминг 200 мс
  sensor.setMeasurementTimingBudget(50000);
#endif
///////////////////////////////////////////////

  pinMode(PULSE1, OUTPUT);
  Timer1_Init(32, 250);
}

void Timer1_Init(int prescaler1, int prescaler2)
{
  TCCR1A = 0;
  switch (prescaler1)
  {
    case (0):     TCCR1B = 0<<CS12 | 0<<CS11 | 0<<CS10; break;   // Timer stopped
    case (1):     TCCR1B = 0<<CS12 | 0<<CS11 | 1<<CS10; break;   // CLK
    case (8):     TCCR1B = 0<<CS12 | 1<<CS11 | 0<<CS10; break;   // CLK/8
    case (32):    TCCR1B = 0<<CS12 | 1<<CS11 | 1<<CS10; break;   // CLK/32
    case (64):    TCCR1B = 1<<CS12 | 0<<CS11 | 0<<CS10; break;   // CLK/64
    case (128):   TCCR1B = 1<<CS12 | 0<<CS11 | 1<<CS10; break;   // CLK/128
    case (256):   TCCR1B = 1<<CS12 | 1<<CS11 | 0<<CS10; break;   // CLK/256
    case (1024):  TCCR1B = 1<<CS12 | 1<<CS11 | 1<<CS10; break;   // CLK/1024
  }
  TIMSK1 = 1<<TOIE1;  // Overflov interrupt enabled
  t1st = 65535 - prescaler2;
  TCNT1 = t1st;       // Start count value
}

ISR(TIMER1_OVF_vect)
{
  TCNT1 = t1st;
  count1++;
  switch(pulse1_stp)
  {
    case (0):  digitalWrite(PULSE1, LOW); digitalWrite(LED, LOW); break;
    case (1):  digitalWrite(PULSE1, HIGH); digitalWrite(LED, HIGH); count1 = 0; pulse1_stp++; break;
    case (2):  (count1 >= pulse_ms) ? (digitalWrite(PULSE1, LOW), digitalWrite(LED, LOW), count1 = 0, pulse1_stp++) : (0); break;
    case (3):  (count1 >= low1) ? (pulse1_stp = 1) : (0); break;
  }
}


void GetLevel (int &dist, int &level)
{
  (dist >= 0  && dist < L1) ? (level = 1):
  (dist >= L1 && dist < L2) ? (level = 2):
  (dist >= L2 && dist < L3) ? (level = 3):
  (dist >= L3) ? (level = 0) : (level = 0);
}

void GetInterval (int level, int &low, int &stp)
{
  switch(level)
  {
    case (1):  low = pulse_ms*1; break;
    case (2):  low = pulse_ms*4; break;
    case (3):  low = pulse_ms*7; break;
  }
  if (level != 0 && stp == 0)
    stp = 1;
  else
    if (level == 0)
      stp = 0;
}

void loop()
{ out1=sensor.readRangeSingleMillimeters();
  //delay(100);
  dist1 = out1/10;
  GetLevel(dist1, Level1);
  GetInterval(Level1, low1, pulse1_stp);
}
 

 

за 500 рублей даже книжку не купишь, чтобы исправить этот говнокод.

 

Наверно так:

Код (C++):

#include <Wire.h>

#include <VL53L0X.h>


VL53L0X sensor;
// Раскомментируйте строку ниже, чтобы использовать дальний режим.
// Это увеличивает чувствительность датчика и расширяет его потенциальный диапазон,
// но увеличивает вероятность получения неточных показаний
// из-за отражений от объектов, отличных от намеченной цели.
// Лучше всего работает в темноте.
#define LONG_RANGE

// Раскомментируйте одну из двух строк ниже, чтобы получить:
// - более высокую скорость за счет меньшей точности ИЛИ
// - более высокую точность за счет меньшей скорости
//#define HIGH_SPEED
#define HIGH_ACCURACY
///////////////////////

int out1;
#define PULSE1 A3    /* Мотор №1 */
#define LED 13

#define pulse_ms 100  /* Длительность импульса мотора (мс) */
#define L0 50
#define L1 70
#define L2 100
#define L3 150

int Level1 = 0;

int i = 0;

int t1st;
int dist1 = 0;
int count1 = 0;
int low1 = 0;
int pulse1_stp = 0;
void setup()
{
pinMode(13, OUTPUT);
digitalWrite(13, 0);


Serial.begin(9600);
  Wire.begin();

  sensor.setTimeout(200);

  if (!sensor.init())
  {
    Serial.println("Не удалось обнаружить и инициализировать датчик!");
    while (1) {}
  }

#if defined LONG_RANGE
  // понижает предел скорости обратного сигнала (по умолчанию 0,25 MCPS (мчип/с))
  sensor.setSignalRateLimit(0.1);
  // увеличить периоды лазерного импульса (по умолчанию 14 и 10 PCLK)
  // * - PCLK — это частота периферии
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
#endif

#if defined HIGH_SPEED
  // уменьшить тайминг до 20 мс (по умолчанию около 33 мс)
  sensor.setMeasurementTimingBudget(50000);
#elif defined HIGH_ACCURACY
  // увеличить тайминг 200 мс
  sensor.setMeasurementTimingBudget(50000);
#endif
///////////////////////////////////////////////

  pinMode(PULSE1, OUTPUT);
  Timer1_Init(32, 250);
}

void Timer1_Init(int prescaler1, int prescaler2)
{
  TCCR1A = 0;
  switch (prescaler1)
  {
    case (0):     TCCR1B = 0<<CS12 | 0<<CS11 | 0<<CS10; break;   // Timer stopped
    case (1):     TCCR1B = 0<<CS12 | 0<<CS11 | 1<<CS10; break;   // CLK
    case (8):     TCCR1B = 0<<CS12 | 1<<CS11 | 0<<CS10; break;   // CLK/8
    case (32):    TCCR1B = 0<<CS12 | 1<<CS11 | 1<<CS10; break;   // CLK/32
    case (64):    TCCR1B = 1<<CS12 | 0<<CS11 | 0<<CS10; break;   // CLK/64
    case (128):   TCCR1B = 1<<CS12 | 0<<CS11 | 1<<CS10; break;   // CLK/128
    case (256):   TCCR1B = 1<<CS12 | 1<<CS11 | 0<<CS10; break;   // CLK/256
    case (1024):  TCCR1B = 1<<CS12 | 1<<CS11 | 1<<CS10; break;   // CLK/1024
  }
  TIMSK1 = 1<<TOIE1;  // Overflov interrupt enabled
  t1st = 65535 - prescaler2;
  TCNT1 = t1st;       // Start count value
}

ISR(TIMER1_OVF_vect)
{
  TCNT1 = t1st;
  count1++;
  switch(pulse1_stp)
  {
    case (0):  digitalWrite(PULSE1, LOW); digitalWrite(LED, LOW); break;
    case (1):  digitalWrite(PULSE1, HIGH); digitalWrite(LED, HIGH); count1 = 0; pulse1_stp++; break;
    case (2):  (count1 >= pulse_ms) ? (digitalWrite(PULSE1, LOW), digitalWrite(LED, LOW), count1 = 0, pulse1_stp++) : (0); break;
    case (3):  (count1 >= low1) ? (pulse1_stp = 1) : (0); break;
  }
}


void GetLevel (int &dist, int &level)
{
  (dist > L0  && dist < L1) ? (level = 1):
  (dist >= L1 && dist < L2) ? (level = 2):
  (dist >= L2 && dist < L3) ? (level = 3):
  ((dist >= L3)||(dist <= L0)) ? (level = 0) : (level = 0);
}

void GetInterval (int level, int &low, int &stp)
{
  switch(level)
  {
    case (1):  low = pulse_ms*1; break;
    case (2):  low = pulse_ms*4; break;
    case (3):  low = pulse_ms*7; break;
  }
  if (level != 0 && stp == 0)
    stp = 1;
  else
    if (level == 0)
      stp = 0;
}

void loop()
{ out1=sensor.readRangeSingleMillimeters();
  //delay(100);
  dist1 = out1/10;
  GetLevel(dist1, Level1);
  GetInterval(Level1, low1, pulse1_stp);
}