我在使用这段代码时遇到困难，我不知道出了什么问题，它只是不起作用。舵机抖动，有时转，有时停不下来，到处都是。我不知道如何开始调试这个。

#include <Servo.h>

const int trigPinLeft = 2;   // Trig pin for left ultrasonic sensor
const int echoPinLeft = 3;   // Echo pin for left ultrasonic sensor
const int trigPinRight = 4;  // Trig pin for right ultrasonic sensor
const int echoPinRight = 5;  // Echo pin for right ultrasonic sensor

const int upLimitSwitch = 6;   // Pin for up position limit switch
const int downLimitSwitch = 7; // Pin for down position limit switch

const int servoPin = 9;    // Pin for continuous servo motor

const int detectionDistance = 20; // Distance threshold for object detection in cm
const int doorOpenTime = 15000;   // Time the door stays open in milliseconds (15 seconds)

Servo doorServo;  // Servo motor object

void setup() {
  pinMode(trigPinLeft, OUTPUT);
  pinMode(echoPinLeft, INPUT);
  pinMode(trigPinRight, OUTPUT);
  pinMode(echoPinRight, INPUT);
  pinMode(upLimitSwitch, INPUT_PULLUP);
  pinMode(downLimitSwitch, INPUT_PULLUP);
  Serial.begin(9600);
}

void loop() {
  if (digitalRead(upLimitSwitch) == LOW) {
    // Door is in the up position, check for object in front
    if (isObstacleDetected()) {
      doorServo.attach(servoPin);
      rotateCounterClockwiseUntilDownLimit();
      doorServo.detach();
    }
  } else if (digitalRead(downLimitSwitch) == LOW) {
    // Door is in the down position, check for object in front
    if (isObstacleDetected()) {
      doorServo.attach(servoPin);
      rotateClockwiseUntilUpLimit();
      doorServo.detach();
    }
  }
}

bool isObstacleDetected() {
  int distanceLeft = getDistance(trigPinLeft, echoPinLeft);
  int distanceRight = getDistance(trigPinRight, echoPinRight);
  return (distanceLeft <= detectionDistance || distanceRight <= detectionDistance);
}

int getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  return pulseIn(echoPin, HIGH) * 0.034 / 2;
}

void rotateCounterClockwiseUntilDownLimit() {
  while (digitalRead(downLimitSwitch) == HIGH) {
    doorServo.write(0);  // Rotate the servo counter-clockwise at full speed
  }
  //doorServo.write(90);  // Stop the servo when the downLimitSwitch is reached
}

void rotateClockwiseUntilUpLimit() {
  while (digitalRead(upLimitSwitch) == HIGH) {
    doorServo.write(180);  // Rotate the servo clockwise at full speed
  }
  //doorServo.write(90);  // Stop the servo when the upLimitSwitch is reached
}