Код логики для измерения значений, сохраненных в буфере, и создания вложенного массива

У меня возникли трудности с созданием логики в моем коде. У меня есть потенциометр, подключенный к ESP32 для имитации тензодатчика и датчика диапазона. Значение для силы находится в диапазоне от 0 до 400 ньютонов, а для диапазона — от 50 до 0 мм. В коде у меня есть функция, которая при достижении положения 40 мм запускает процедуру измерения, где равномерно в зависимости от положения сила и соответствующее положение должны быть сохранены в буфере. При достижении 0 мм необходимо создать JSON с 5 точками измерения. Если NUM_SAMPLES будет равно 5, мы начнем первое измерение с 40 мм, второе с 30 мм, третье с 20 мм и т. д. При достижении 0 мм необходимо создать JSON из значений из буфера и распечатать. Конечный результат должен быть следующим:

{"range_array":[40,30,20,10,0],"force_array":[82,163,241,322,400]}.

Но теперь я создаю и распечатываю JSON непрерывно со скоростью 40 мм.

{"range_array":[40,30,20,10,0],"force_array":[82,82,82,82,82]}

Если я выхожу за пределы 40 мм, он останавливается. Код следует. Заранее спасибо за помощь.

#include <Arduino.h>            // Требуется platformIO
#include <ArduinoJson.h>        // Библиотека JSON для встроенного C++
#include <WebSocketsServer.h>   // Библиотека WebSocketServer

const int potPin = 14; // Аналоговый вывод, подключенный к потенциометру
const int rangeMin = 0; // Минимальный диапазон имитируемого датчика в мм
const int rangeMax = 50; // Максимальный диапазон имитируемого датчика в мм
const int forceMin = 0; // Минимальная сила моделируемого датчика в ньютонах
const int forceMax = 400; // Максимальная сила смоделированного датчика в ньютонах

int rangeValue = 0;
int range_reading = 0;
int forceValue = 0;
int force_reading = 0;

unsigned long previousMillis = 0;
const long interval = 100;

WebSocketsServer webSocket = WebSocketsServer(81);

int NUM_SAMPLES = 5;

void setup() {
  Serial.begin(9600);
}

void loop() {
  unsigned long currentMillis = millis();

  Read_Range_Sensor();
  Read_Loadcell();

  // Проверьте, равно ли положение 40 мм, затем начните измерение
  if (range_reading == 40) {
    performMeasurement();
  }

  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    Serial.print("Range: ");
    Serial.print(range_reading);
    Serial.print(" mm, Force: ");
    Serial.print(force_reading);
    Serial.println(" N");
  }
}

void Read_Range_Sensor() {
  rangeValue = analogRead(potPin);
  range_reading = map(rangeValue, 0, 4095, rangeMax, rangeMin);
}

void Read_Loadcell() {
  forceValue = analogRead(potPin);
  force_reading = map(forceValue, 0, 4095, forceMin, forceMax);
}

void performMeasurement() {
  // Массивы для хранения позиций и сил
  int range_array[NUM_SAMPLES];
  int force_array[NUM_SAMPLES];

  // Начните измерение от 40 мм до 0 мм
  for (int i = 0; i < NUM_SAMPLES; i++) {
    range_array[i] = 40 - i * (40 / (NUM_SAMPLES - 1));
    force_array[i] = map(analogRead(potPin), 0, 4095, forceMin, forceMax);
  }

  // Создать объект JSON
  StaticJsonDocument<200> doc;
  JsonArray rangeJson = doc.createNestedArray("range_array");
  JsonArray forceJson = doc.createNestedArray("force_array");

  // Заполнение массивов JSON
  for (int i = 0; i < NUM_SAMPLES; i++) {
    rangeJson.add(range_array[i]);
    forceJson.add(force_array[i]);
  }
  // Сериализовать JSON в строку
  String jsonString;
  serializeJson(doc, Serial);
  serializeJson(doc, jsonString);
}