const int pwmFreq = 1000; // PWM频率设置为1kHz
const int maxSpeed = 255; // 最大速度对应的PWM值
const int minDistanceForMotion = 10; // 距离阈值起始点
const int distanceThresholds[] = {10, 20, 30}; // 不同速度区间的距离阈值
const int speedRanges[] = {50, 150, 250}; // 对应距离阈值的速度范围起点


void setup() {
    Serial.begin(9600);
    pinMode(0, OUTPUT); // PWM输出，连接到马达控制器的输入
    pinMode(2, OUTPUT);
    pinMode(17, OUTPUT);
    pinMode(16, INPUT);

  
}

void loop() {

    unsigned long time;
    unsigned long dis;

    digitalWrite(17, LOW);
    delayMicroseconds(2);
    digitalWrite(17, HIGH);
    delayMicroseconds(10);
    digitalWrite(17, LOW);
    time = pulseIn(16,HIGH);
    dis = time * 0.017;

  int speed = 0; // 初始化speed，确保有默认值
  bool foundSpeed = false; // 标记是否找到了对应的速度

    for(int i = 0; i < sizeof(distanceThresholds)/sizeof(distanceThresholds[0])-1; i++) {
        if(dis >= distanceThresholds[i] && dis < distanceThresholds[i+1]) {
            int calculatedSpeed = map(dis, distanceThresholds[i], distanceThresholds[i+1], speedRanges[i], speedRanges[i+1]);
            calculatedSpeed = constrain(calculatedSpeed, 0, maxSpeed);
           digitalWrite(0, HIGH);
            digitalWrite(2, LOW);
            Serial.print("Distance: ");
            Serial.print(dis);
            Serial.print(" cm, Speed: ");
            Serial.println(calculatedSpeed);
            speed = calculatedSpeed; // 给speed赋值
            foundSpeed = true;
            break;
        }
    }

    if(!foundSpeed || dis <= 10 || dis >= distanceThresholds[sizeof(distanceThresholds)/sizeof(distanceThresholds[0])-1]) {
        digitalWrite(2, LOW); 
        digitalWrite(0, LOW);
        Serial.print("Distance: ");
            Serial.print(dis);
            Serial.print(" cm, Speed: ");
        Serial.println("0");
        speed = 0; // 如果没找到合适的speed或超出范围，设置为0
    }
     delay(2000); // 防止频繁读取 

}