硬件控制代码

#include <Wire.h>
#include <VL53L0X.h>

VL53L0X sensor;

// A馬達引腳
const int STBY = 2; 
const int PWMA = 5; 
const int AIN1 = 4;  
const int AIN2 = 3;  

// B馬達引腳
const int PWMB = 6;
const int BIN1 = 7; 
const int BIN2 = 8; 

void setup() {
  Serial.begin(9600);
  Wire.begin();
  
  pinMode(STBY, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  
  pinMode(PWMB, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);

  if (!sensor.init()) {
    Serial.println("Error: Sensor not found!");
    while (1);
  }
  
  sensor.startContinuous();
  digitalWrite(STBY, HIGH);
  Serial.println("Switch System Ready!");
}

void loop() {
  int distance = sensor.readRangeContinuousMillimeters();
  
  Serial.print("Dist: ");
  Serial.print(distance);
  Serial.println(" mm");

  if (distance > 100) {
    // 【100mm 以外】：A 轉，B 停
    digitalWrite(AIN1, HIGH);
    digitalWrite(AIN2, LOW);
    analogWrite(PWMA, 255);   
    
    digitalWrite(BIN1, LOW);
    digitalWrite(BIN2, LOW);
    analogWrite(PWMB, 0);   
  } 
  else {
    // 【100mm 以內】：A 停，B 轉
    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, LOW);
    analogWrite(PWMA, 0);     
    
    digitalWrite(BIN1, HIGH);
    digitalWrite(BIN2, LOW);
    analogWrite(PWMB, 255);     
  }
  
  delay(100);
}