Arduino IDE

#include

#define S_LIMIT_1 18

#define S_LIMIT_3 8

#define S_LIMIT_2 100

long timer,runtime;\

const long TIME_OUT=5000;

Servo myservo;

int ServoPin=10

int pos=0

#define MOTORADJUSTMENT

int MotorAdjustmengPin=A1;

const int trig=4;

const int echo=7;

const int DIR1_RIGHT=12;

const int DIR2_RIGHT=11;

const int DIR1_LEFT=8;

const int DIR2_LEFT=9;

const int PWM_LEFT=6;

const int PWM_RIGHT=5;

int Bit_Front,Bit_Left,Bit_Right;

 

void setup()

{

pinMode（echo,INPUT）;

pinMode(trig,OUTPUT);

pinMode(DIR1_RIGHT,OUTPUT);

pinMode(DIR2_RIGHT,OUTPUT);

pinMode(DIR1_LEFT,OUTPUT);

pinMode(DIR2_LEFT,OUTPUT);

pinMode(PWM_LEFT,OUTPUT);

pinMode(PWM_RIGHT,OUTPUT);

myservo.attach(ServoPin);

Servo.begin(9600);

}

 

void motorsWrite(int speedLeft,int speedRight)

float motorAdjustment=MotorAdjustment();

if(motorAdjustment<0){

 speedRight*=(1+motorAdjustment);

}

else{

   speedLeft*=(1-motorAdjustment);

}

if(speedRight>0);

{

     digitalWrite(DIR1_RIGHT,1);

     digitalWrite(DIR2_RIGHT,0);

}

else

{

    digitalWrite(DIR1_RIGHT,1);

    digitalWrite(DIR2_RIGHT,0);

}

analogWrite(PWM_RIGHT, abs(speedRight));

 

if(speedLeft>0);

{

  digitalWrite(DIR1_LEFT,1); 

  digitalWrite(DIR2_LEFT,0);

}

else

{

     digitalWrite(DIR1_LEFT,1);

     digitalWrite(DIR2_LEFT,1);

}

analogWrite(PWM_RIGHT, abs(speedRight));

 

int ranging()

{

 long IntervalTime=0;

 digitalWrite(trig,1);

 delayMicroseconds(15);

 digitalWrite(trig,0);

 IntervalTime=pulseIn(echo,HIGH);

 int S=IntervalTime/58;

 return S;

}

void loop()

{

int S,SL,SR;

int TimerFlag=0;

myservo.write(90);

while(1);

{

   if((millis()-TIME_OUT))

}

 TimerFlag=1;

}

myservo.write(90);

delay(300);

S= ranging();

Serial.printIn(5);

if(S<=S_LIMIT_3)

   back(300)

if(S<=S_LIMIT_1||TimerFlag==1)

{

 timer=millis();

 TimerFlag=0;

 stopMotor();

 myservo.write(45);

 delay(300);

 int SR=ranging();

 myservo.write(145);

 delay(300);

 int SL=ranging();

 myservo.write(90);

 

 

 

 

 

 



返回

1 条评论

  • 改正后的
    #1 楼 · IP: 124.79.159.90
    2025-12-12 15:52

    #include <Servo.h> // 1. 修复头文件
     
    // 常量定义
    #define S_LIMIT_1 18
    #define S_LIMIT_3 8
    #define S_LIMIT_2 100
     
    // 全局变量
    long timer;
    const long TIME_OUT = 5000;
     
    Servo myservo;
    int ServoPin = 10; // 2. 补全分号
    int pos = 0;       // 2. 补全分号
     
    // 3. 修正变量名拼写
    int MotorAdjustmentPin = A1;
     
    const int trig = 4;
    const int echo = 7;
     
    // 电机引脚
    const int DIR1_RIGHT = 12;
    const int DIR2_RIGHT = 11;
    const int DIR1_LEFT = 8;
    const int DIR2_LEFT = 9;
    const int PWM_LEFT = 6;
    const int PWM_RIGHT = 5;
     
    // 函数声明
    int ranging();
    void stopMotor();
    void back(int duration);
    float GetMotorAdjustment();
     
    void setup() {
      pinMode(echo, INPUT); // 4. 修复中文括号
      pinMode(trig, OUTPUT);
      pinMode(DIR1_RIGHT, OUTPUT);
      pinMode(DIR2_RIGHT, OUTPUT);
      pinMode(DIR1_LEFT, OUTPUT);
      pinMode(DIR2_LEFT, OUTPUT);
      pinMode(PWM_LEFT, OUTPUT);
      pinMode(PWM_RIGHT, OUTPUT);
     
      myservo.attach(ServoPin);
      Serial.begin(9600); // 5. 修复 Servo.begin 为 Serial.begin
    }
     
    // 获取电机微调值 (假设是电位器读取)
    float GetMotorAdjustment() {
      int val = analogRead(MotorAdjustmentPin);
      // 简单的映射逻辑：将0-1023映射到 -0.3 到 0.3 之间
      // 这样可以用来微调左右轮速度差
      return (float)(val - 512) / 512.0 * 0.3; 
    }
     
    void motorsWrite(int speedLeft, int speedRight) { // 6. 补全左大括号
      float motorAdjustment = GetMotorAdjustment();
     
      // 根据微调值调整速度
      if (motorAdjustment < 0) {
        speedRight *= (1 + motorAdjustment);
      } else {
        speedLeft *= (1 - motorAdjustment);
      }
     
      // --- 右电机控制 ---
      if (speedRight > 0) { // 7. 去掉多余分号
        digitalWrite(DIR1_RIGHT, HIGH);
        digitalWrite(DIR2_RIGHT, LOW);
      } else {
        digitalWrite(DIR1_RIGHT, LOW); // 修正反转逻辑
        digitalWrite(DIR2_RIGHT, HIGH);
      }
      analogWrite(PWM_RIGHT, abs(speedRight));
     
      // --- 左电机控制 ---
      if (speedLeft > 0) { // 7. 去掉多余分号
        digitalWrite(DIR1_LEFT, HIGH);
        digitalWrite(DIR2_LEFT, LOW);
      } else {
        digitalWrite(DIR1_LEFT, LOW);  // 修正反转逻辑
        digitalWrite(DIR2_LEFT, HIGH); // 修正之前两个都为1的错误
      }
      // 8. 修正这里原本写成 PWM_RIGHT 的错误
      analogWrite(PWM_LEFT, abs(speedLeft)); 
    }
     
    // 停止电机
    void stopMotor() {
      digitalWrite(DIR1_RIGHT, LOW);
      digitalWrite(DIR2_RIGHT, LOW);
      digitalWrite(DIR1_LEFT, LOW);
      digitalWrite(DIR2_LEFT, LOW);
      analogWrite(PWM_RIGHT, 0);
      analogWrite(PWM_LEFT, 0);
    }
     
    // 后退函数
    void back(int duration) {
      motorsWrite(-150, -150); // 假设负数为后退，速度150
      delay(duration);
      stopMotor();
    }
     
    // 测距函数
    int ranging() {
      digitalWrite(trig, LOW);
      delayMicroseconds(2);
      digitalWrite(trig, HIGH);
      delayMicroseconds(10);
      digitalWrite(trig, LOW);
      
      long IntervalTime = pulseIn(echo, HIGH);
      int S = IntervalTime / 58;
      return S;
    }
     
    void loop() {
      // 9. 将原本在 loop 外面的代码移进来，并整理逻辑
      
      int S, SL, SR;
      
      myservo.write(90); // 舵机归中
      delay(300);        // 等待舵机到位
      
      S = ranging();     // 测距
      Serial.println(S); // 打印距离
     
      // 如果距离太近（小于8cm），后退
      if (S <= S_LIMIT_3) {
        back(300);
      }
      // 如果距离小于警戒值（18cm），停车观察
      else if (S <= S_LIMIT_1) {
        stopMotor();
        
        // 向右看
        myservo.write(45);
        delay(500);
        SR = ranging();
        
        // 向左看
        myservo.write(135); // 修正：145对于某些舵机可能过大，对称一点通常是135或180
        delay(500);
        SL = ranging();
        
        myservo.write(90); // 回中
        delay(300);
     
        // 简单的避障逻辑决策
        if (SL > SR) {
          // 左边空旷，左转 (左轮反转，右轮正转)
          motorsWrite(-150, 150);
          delay(400); 
        } else {
          // 右边空旷，右转
          motorsWrite(150, -150);
          delay(400);
        }
      } 
      else {
        // 前方无障碍，直行
        motorsWrite(150, 150);
      }
      
      // 简单的延时，避免循环过快
      delay(100);
    }

发表评论