Ультразвуковой датчик и проблема с цветом светодиодной ленты

Итак, я сделал датчик парковки с ультразвуковым датчиком и светодиодной полосой. Но светодиодная полоса не становится желтой или зеленой. Думаю, это связано с порогами. Может ли кто-нибудь помочь мне настроить это? Я также хочу более длинное расстояние больше, чем 1 метр. Это мой код:

#include "FastLED.h"
 #include <math.h>


    #define NUM_LEDS 32
    #define BRIGHTNESS 10
    #define DATA_PIN 4

    // Определяем массив светодиодов
    CRGB leds[NUM_LEDS];

    // Пины модуля
    #define trigPin 11
    #define echoPin 12


    // Пороги (в CM)
    long SECTION_SIZE = 80;
    long STOP_DISTANCE = 60;
    long WARNING_THRESHOLD = 50;
    long MAX_THRESHOLD = 375;

    long RED_THRESHOLD = STOP_DISTANCE;
    long YELLOW_THRESHOLD = RED_THRESHOLD + SECTION_SIZE;
    long GREEN_THRESHOLD = YELLOW_THRESHOLD + SECTION_SIZE;

    const int numReadings = 5;

    int readings[numReadings];      // показания с аналогового входа
    int readIndex = 0;              // индекс текущего показания
    int total = 0;                  // промежуточный итог
    int average = 0;                // среднее

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


      FastLED.addLeds<NEOPIXEL, DATA_PIN>(leds, NUM_LEDS);
      FastLED.setBrightness(10);

      // Настройка HC-SR04
      pinMode(trigPin, OUTPUT);
      pinMode(echoPin, INPUT);

      // инициализируем все показания равными 0:
      for (int thisReading = 0; thisReading < numReadings; thisReading++) {
        readings[thisReading] = 0;
      }
    }

    void loop() {
      long distance, lastRun;
      bool red, yellow, green;
      total = total - readings[readIndex];
      readings[readIndex] = calculateDistance();
      total = total + readings[readIndex];

      readIndex++;
      if (readIndex >= numReadings) readIndex = 0;

      distance = abs(total / numReadings);

      yellow = distance <= YELLOW_THRESHOLD && distance > RED_THRESHOLD;
      red = distance <= RED_THRESHOLD;
      green = distance > YELLOW_THRESHOLD && distance <= GREEN_THRESHOLD;

      if (red) {
        setRed();
      } else if (yellow) {
        setYellow(distance);
      } else if (green) {
        setGreen(distance);
      } else {
        setGreenStandby();
      }



      Serial.print(distance);
      Serial.println(" cm");
      FastLED.show();
      delay(1);
    }

    void lightItUp(const CRGB& pColor, const CRGB& sColor, uint16_t travel) {
      uint16_t travelFix = travel * (NUM_LEDS/SECTION_SIZE);
      uint16_t leftStop = travelFix/2;
      uint16_t rightStop = NUM_LEDS - leftStop;
      for (int i = 0; i < NUM_LEDS; i++) {
        if (i < leftStop || i > rightStop) {
          leds[i] = pColor;
        } else {
          leds[i] = sColor;
        }
      }

      FastLED.show();
    }


    long calculateDistance() {
      long duration;
      digitalWrite(trigPin, LOW);
      delayMicroseconds(2);
      digitalWrite(trigPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin, LOW);

      duration = pulseIn(echoPin, HIGH);
      return (duration/2) / 29.1;
    }

    void setGreenStandby() {
      return fill_solid(leds, NUM_LEDS, CRGB::Black);
    }

    void setRed() {
      return fill_solid(leds, NUM_LEDS, CRGB::Red);
    }

    void setGreen(long distance) {
      long travel = GREEN_THRESHOLD - distance;
      return lightItUp(CRGB::Yellow, CRGB::Green, travel);
    }

    void setYellow(long distance) {
      long travel = YELLOW_THRESHOLD - distance;
      return lightItUp(CRGB::Red, CRGB::Yellow, travel);  
    }