arduino避障小车阶段性开发

arduino_nano_tracker/arduino_nano_tracker.ino

@@ -0,0 +1,184 @@
+#include <Servo.h> // 舵机库
+
+// 引脚定义
+// 电机
+const int LEFT_MOTOR_PIN = 9;  // 左电机PWM控制引脚
+const int RIGHT_MOTOR_PIN = 10; // 右电机PWM控制引脚
+
+// TRCT5000 循迹传感器
+const int LEFT_TRCT_PIN = 7;  // 左循迹传感器引脚
+const int RIGHT_TRCT_PIN = 8; // 右循迹传感器引脚
+
+// 超声波传感器
+const int TRIG_PIN = 12; // 超声波Trig引脚
+const int ECHO_PIN = 11; // 超声波Echo引脚
+
+// 舵机
+const int SERVO_PIN = 3; // 舵机控制引脚
+
+Servo steeringServo; // 创建舵机对象
+
+// 常量定义
+const int OBSTACLE_DISTANCE_THRESHOLD_CM = 20; // 障碍物检测阈值 (厘米)
+const int SERVO_NEUTRAL_ANGLE = 90;          // 舵机中间位置角度
+const int SERVO_LEFT_ANGLE = 75;             // 舵机微调左转角度 (原为60, 调整为更小角度)
+const int SERVO_RIGHT_ANGLE = 105;           // 舵机微调右转角度 (原为120, 调整为更小角度)
+
+// 电机速度 (analogWrite PWM值范围 0-255) - 这些常量在新的电机控制逻辑下意义减弱
+const int MOTOR_STATE_STOP = LOW;   // 电机停止状态 (使用LOW)
+const int MOTOR_STATE_RUN = HIGH; // 电机运行状态 (使用HIGH)
+// 保留旧的常量名以便于理解之前的逻辑，但实际传递给电机控制函数的值会被转换为HIGH/LOW
+const int MOTOR_SPEED_STOP = 0;         // 逻辑上的停止 (会转换为LOW)
+const int MOTOR_SPEED_NORMAL = 1;     // 逻辑上的运行 (会转换为HIGH)
+const int MOTOR_SPEED_SLOW = 1;       // 逻辑上的运行 (会转换为HIGH)
+const int MOTOR_SPEED_TURN_ASSIST = 1; // 逻辑上的运行 (会转换为HIGH)
+// const int MOTOR_SPEED_DIFFERENTIAL_SLOW = 70; // 这个在单边驱动时不再直接使用
+
+// TRCT5000 传感器逻辑 (假设检测到黑线为低电平, 白色地面为高电平)
+// 具体根据你的传感器模块实际输出进行调整 (例如，如果模块输出反相，则需要调换定义)
+#define ON_LINE LOW  // 在线上 (检测到黑线)
+#define OFF_LINE HIGH // 不在线上 (检测到白色地面)
+
+void setup() {
+  // 初始化串口通信，波特率9600
+  Serial.begin(9600);
+  while (!Serial) {
+    ; // 等待串口连接 (对于某些板子如Leonardo是必需的，Nano通常不需要但加上无害)
+  }
+  Serial.println("串口已连接。");
+
+  // 初始化电机引脚为输出模式
+  pinMode(LEFT_MOTOR_PIN, OUTPUT);
+  pinMode(RIGHT_MOTOR_PIN, OUTPUT);
+
+  // 初始化TRCT5000循迹传感器引脚为输入模式
+  pinMode(LEFT_TRCT_PIN, INPUT);
+  pinMode(RIGHT_TRCT_PIN, INPUT);
+
+  // 初始化超声波传感器引脚
+  pinMode(TRIG_PIN, OUTPUT); // Trig引脚为输出
+  pinMode(ECHO_PIN, INPUT);  // Echo引脚为输入
+
+  // 舵机初始化，连接到SERVO_PIN，并设置到中间位置
+  steeringServo.attach(SERVO_PIN);
+  steeringServo.write(SERVO_NEUTRAL_ANGLE);
+
+  // 确保初始状态电机停止
+  stopMotors();
+  Serial.println("初始状态：电机已停止。");
+
+  Serial.println("Arduino Nano 循迹避障小车 初始化完成，准备就绪。");
+  Serial.println("-----------------------------------------");
+}
+
+void loop() {
+  Serial.println("--- 开始新循环 ---");
+  long distance = readUltrasonicDistance(); // 读取超声波测量的距离
+  Serial.print("超声波距离: "); Serial.print(distance); Serial.println(" cm");
+
+  if (distance > 0 && distance < OBSTACLE_DISTANCE_THRESHOLD_CM) {
+    // 如果检测到障碍物 (距离大于0且小于阈值)
+    Serial.println("状态更新：检测到障碍物! 执行停止。");
+    stopMotors(); // 停止电机
+    steeringServo.write(SERVO_NEUTRAL_ANGLE); // 舵机回中
+    Serial.println("动作：舵机回中。");
+  } else {
+    Serial.println("状态更新：无障碍物，执行循迹逻辑。");
+    trackLine();
+  }
+  Serial.println("--- 循环结束，延时 ---");
+  delay(100); // 稍微增加延时，方便观察串口输出，原为50
+}
+
+// --- 电机控制函数 ---
+void controlMotors(int leftState, int rightState) {
+  // 直接使用digitalWrite控制电机，HIGH为全速，LOW为停止
+  // leftState 和 rightState 现在应该代表 MOTOR_STATE_RUN (HIGH) 或 MOTOR_STATE_STOP (LOW)
+  
+  digitalWrite(LEFT_MOTOR_PIN, leftState);
+  digitalWrite(RIGHT_MOTOR_PIN, rightState);
+
+  Serial.print("电机控制(直接驱动)：左轮状态="); 
+  Serial.print(leftState == HIGH ? "运行(HIGH)" : "停止(LOW)");
+  Serial.print(", 右轮状态="); 
+  Serial.println(rightState == HIGH ? "运行(HIGH)" : "停止(LOW)");
+}
+
+void moveForward() { // 不再需要speed参数，因为总是全速
+  Serial.println("动作：向前行驶 (电机全速)。");
+  steeringServo.write(SERVO_NEUTRAL_ANGLE); // 确保舵机在中间位置 (直行)
+  Serial.println("动作：舵机回中 (配合直行)。");
+  controlMotors(MOTOR_STATE_RUN, MOTOR_STATE_RUN); // 设置左右电机运行
+}
+
+void stopMotors() {
+  Serial.println("动作：停止电机。");
+  controlMotors(MOTOR_STATE_STOP, MOTOR_STATE_STOP); // 设置左右电机停止
+}
+
+void turnLeft() { // 不再需要turnSpeed参数
+  Serial.println("动作：左转 (电机全速，舵机控制)。");
+  steeringServo.write(SERVO_LEFT_ANGLE); // 舵机打到左转角度
+  Serial.print("动作：舵机打到左转角度 ("); Serial.print(SERVO_LEFT_ANGLE); Serial.println("度)。");
+  // 两个电机都保持运行，转向仅靠舵机
+  controlMotors(MOTOR_STATE_RUN, MOTOR_STATE_RUN);
+}
+
+void turnRight() { // 不再需要turnSpeed参数
+  Serial.println("动作：右转 (电机全速，舵机控制)。");
+  steeringServo.write(SERVO_RIGHT_ANGLE); // 舵机打到右转角度
+  Serial.print("动作：舵机打到右转角度 ("); Serial.print(SERVO_RIGHT_ANGLE); Serial.println("度)。");
+  // 两个电机都保持运行，转向仅靠舵机
+  controlMotors(MOTOR_STATE_RUN, MOTOR_STATE_RUN);
+}
+
+// --- 传感器读取函数 ---
+long readUltrasonicDistance() {
+  // 读取超声波传感器距离 (返回厘米cm)
+
+  // 清除TRIG_PIN的任何先前状态，确保低电平
+  digitalWrite(TRIG_PIN, LOW);
+  delayMicroseconds(2);
+
+  // 发送一个10微秒的高电平脉冲到TRIG_PIN以触发超声波发送
+  digitalWrite(TRIG_PIN, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(TRIG_PIN, LOW);
+
+  // 读取ECHO_PIN返回的脉冲持续时间 (单位：微秒)
+  // pulseIn函数会等待引脚变为高电平，然后开始计时，再等待引脚变为低电平，停止计时。
+  long duration = pulseIn(ECHO_PIN, HIGH);
+
+  // 计算距离 (单位：厘米)
+  // 声速 = 343米/秒 = 0.0343 厘米/微秒
+  // 距离 = (时间 * 声速) / 2  (因为声波是往返的)
+  long distance = duration * 0.0343 / 2;
+  // Serial.print("[内部] 超声波原始时长: "); Serial.println(duration);
+  return distance;
+}
+
+void trackLine() {
+  // 循迹逻辑
+  int leftSensorVal = digitalRead(LEFT_TRCT_PIN);  // 读取左循迹传感器状态
+  int rightSensorVal = digitalRead(RIGHT_TRCT_PIN); // 读取右循迹传感器状态
+
+  Serial.print("循迹传感器状态: 左="); 
+  Serial.print(leftSensorVal == ON_LINE ? "线上(LOW)" : "线下(HIGH)");
+  Serial.print(", 右="); 
+  Serial.println(rightSensorVal == ON_LINE ? "线上(LOW)" : "线下(HIGH)");
+
+  // 新的循迹逻辑：默认前进，检测到线时微调方向 (电机始终全速)
+  if (leftSensorVal == ON_LINE && rightSensorVal == OFF_LINE) {
+    Serial.println("循迹判断：左线上，右线下 -> 执行左转 (电机保持运行)");
+    turnLeft(); 
+  } else if (leftSensorVal == OFF_LINE && rightSensorVal == ON_LINE) {
+    Serial.println("循迹判断：左线下，右线上 -> 执行右转 (电机保持运行)");
+    turnRight();
+  } else if (leftSensorVal == ON_LINE && rightSensorVal == ON_LINE) {
+    Serial.println("循迹判断：两侧均线上 -> 执行直行 (电机保持运行)");
+    moveForward(); // 两侧都压线，也直行
+  } else { // leftSensorVal == OFF_LINE && rightSensorVal == OFF_LINE
+    Serial.println("循迹判断：两侧均线下 -> 执行直行 (电机保持运行)");
+    moveForward();
+  }
+}