• 日期: 2027-04-11 21:14:07
  • 标签: 常规

//selection int ledPin4 = 45 ; //led4 int sel0 = 37; int sel1 = 36; int sel2 = 35; int sel3 = 34; // int PWM_left = 6; int left_dir = 7; int PWM_right = 5; int right_dir = 2; int base_motor_speed = 10; //沿着黑线路径走 volatile int lineIndex[4];//四个ground sensor存数 int count = 1; int threshold=940; //地面传感器感知黑色的阈值，大于则是黑色，小于则不是黑色 //沿墙 int red_led = 11; volatile int proximity[8] ;//8个proximity sensor //全部引脚 unsigned char analogPin[4] = {A8,A9,A10,A11};// unsigned char proximitySensorsID[8] = {A0,A1,A2,A3,A4,A5,A6,A7};

int iCurLeftMotorForwardSpeed=0,iCurRightMotorForwardSpeed=0,iCurLeftMotorBackwardSpeed,iCurRightMotorBackwardSpeed=0; //当前左轮和右轮的速度 void setup() { //selection pinMode(sel0, INPUT); pinMode(sel1, INPUT); pinMode(sel2, INPUT); pinMode(sel3, INPUT); //初始化电动机相关引脚为输出模式 DDRJ |= 0x0F; DDRK &= ~0x0F; //将PK0-PK3设置为输入模式 initMotor(); //初始化沿墙相关引脚为输出模式 pinMode(red_led,OUTPUT); //初始化红外LED initIRLED() Serial.begin(57600); }

void loop() { unsigned int x = SelectorPosition();

if(x == 6){ //x=6时，走的右边 灯闪的更快 greenLEDon(1); delay(1000); greenLEDoff(1); // sets the digital pin 13 off delay(1000);

/*设定Ground1和2作为左右点用于一开始就感应到触线(循迹)
设定Ground0和3也感应到黑线才算完全到达finish（stop）*/
bool leftIsBlack = readGroundSensorsIsBlack(1);
bool rightIsBlack = readGroundSensorsIsBlack(2);
bool left_2IsBlack = readGroundSensorsIsBlack(0);
bool right_2IsBlack = readGroundSensorsIsBlack(3);



//循迹运动

if(count<=4){
  //若两边同时感应到则转弯，其余直行
  if(leftIsBlack && rightIsBlack && !left_2IsBlack && !right_2IsBlack) 
  {
    //感应到后先暂停，给予机器人反应机会
    MotorStop();
    Serial.println('MotorStop'); 
    delay(10);
    //若为奇数次则右转，若为偶数次则左转，即第1、3次右转，2、4次左转
    if(count % 2 ==0) 
    {
      leftTurn(12);
      Serial.println('leftTurn'); 
      delay(1000)
    }else{
      rightTurn(12);
      Serial.println('rightTurn');
      delay(1000)
    } 
    count++;//计数器+1
  }
  else if(!leftIsBlack && rightIsBlack) {
    rightTurn(12);
    Serial.println('rightTurn'); 
  }
  else if(leftIsBlack && !rightIsBlack) {
    leftTurn(12);
    Serial.println('leftTurn');
  }
  else{
    forWard(12);
    Serial.println('forWard'); 
  }
  delay(1000);
}


//沿墙运动以及STOP

else{
  //当四个地面传感器都感应到，则STOP
  if(leftIsBlack && rightIsBlack && left_2IsBlack && right_2IsBlack ){
  MotorStop();
  Serial.println('MotorStop');
  }//STOP结束

  //沿墙
  //如果proximity sensor 0检测到前方障碍物且proximity sensor 6未检测到左边墙，则左转(*具体数值需要调整)
  else if(readProximityDistance(0) > 500 && readProximityDistance(6) < 900){
    leftTurn(12);
    Serial.println('leftTurn'); 
    delay(1000)
  }//沿墙转角处结束

  //如果proximity sensor 2检测到右侧墙，则进行沿墙运动(*具体数值需要调整)
  else if(readProximityDistance(2) < 400){ 

    //沿墙运动（此处distance15换成了700，distance20换成了500）
    if((readProximityDistance(1)>=700 &&  readProximityDistance(1)<500)&&(readProximityDistance(3)>=700 &&  readProximityDistance(3)<500) )
    {
      leftMotorForward(base_speed+2);
      rightMotorForward(base_speed);
    }
    else if(readProximityDistance(1)>=500 &&  readProximityDistance(3)<500)
    {
      leftMotorForward(base_speed);
      rightMotorForward(base_speed+1);
    }
    else if(readProximityDistance(1)>=500 &&  readProximityDistance(3)>=500){
      leftMotorForward(base_speed);
      rightMotorForward(base_speed+3);
    }
    else if(readProximityDistance(1)<700 &&  readProximityDistance(3)>700){
      leftMotorForward(base_speed+3);
      rightMotorForward(base_speed);
    }
    else if (readProximityDistance(1)<15 &&  readProximityDistance(3)<15){
      leftMotorForward(base_speed+5);
      rightMotorForward(base_speed);
    }

  }//右边沿墙运动结束

//

}//x=6结束

if(x == 5){ //x=5时，走的左边 灯闪的更慢 greenLEDon(1); delay(10000); greenLEDoff(1); // sets the digital pin 13 off delay(10000);

/*设定Ground1和2作为左右点用于一开始就感应到触线(循迹)
设定Ground0和3也感应到黑线才算完全到达finish（stop）*/
bool leftIsBlack = readGroundSensorsIsBlack(1);
bool rightIsBlack = readGroundSensorsIsBlack(2);
bool left_2IsBlack = readGroundSensorsIsBlack(0);
bool right_2IsBlack = readGroundSensorsIsBlack(3);



//循迹运动

if(count<=4){
  //若两边同时感应到则转弯，其余直行
  if(leftIsBlack && rightIsBlack && !left_2IsBlack && !right_2IsBlack) 
  {
    //感应到后先暂停，给予机器人反应机会
    MotorStop();
    Serial.println('MotorStop'); 
    delay(10);
    //若为奇数次则左转，若为偶数次则右转，即第1、3次左转，2、4次右转
    if(count % 2 ==0) 
    {
      rightTurn(12);
      Serial.println('rightTurn');
      delay(1000)
    }else{
      leftTurn(12);
      Serial.println('leftTurn'); 
      delay(1000)
    } 
    count++;//计数器+1
  } 
  else if(!leftIsBlack && rightIsBlack) {
    rightTurn(12);
    Serial.println('rightTurn'); 
  }
  else if(leftIsBlack && !rightIsBlack) {
    leftTurn(12);
    Serial.println('leftTurn');
  }
  else{
    forWard(12);
    Serial.println('forWard'); 
  }
  delay(1000);
}


//沿墙运动以及STOP

else{
  //当四个地面传感器都感应到，则STOP
  if(leftIsBlack && rightIsBlack && left_2IsBlack && right_2IsBlack ){
  MotorStop();
  Serial.println('MotorStop');
  }//STOP结束

  //沿墙
  //如果proximity sensor 0检测到前方障碍物且proximity sensor 2未检测到右边墙，则右转(*具体数值需要调整)
  else if(readProximityDistance(0) > 500 && readProximityDistance(2) < 900){
    rightTurn(12);
    Serial.println('rightTurn');
    delay(1000)
  }//沿墙转角处结束

  //如果proximity sensor 6检测到左侧墙，则进行沿墙运动(*具体数值需要调整)
  else if(readProximityDistance(6) < 400){ 

    //沿墙运动（此处distance15换成了700，distance20换成了500）
    if((readProximityDistance(7)>=700 &&  readProximityDistance(7)<500)&&(readProximityDistance(5)>=700 &&  readProximityDistance(5)<500) )
    {
      leftMotorForward(base_speed+2);
      rightMotorForward(base_speed);
    }
    else if(readProximityDistance(7)>=500 &&  readProximityDistance(5)<500)
    {
      leftMotorForward(base_speed);
      rightMotorForward(base_speed+1);
    }
    else if(readProximityDistance(7)>=500 &&  readProximityDistance(5)>=500){
      leftMotorForward(base_speed);
      rightMotorForward(base_speed+3);
    }
    else if(readProximityDistance(7)<700 &&  readProximityDistance(5)>700){
      leftMotorForward(base_speed+3);
      rightMotorForward(base_speed);
    }
    else if (readProximityDistance(7)<15 &&  readProximityDistance(5)<15){
      leftMotorForward(base_speed+5);
      rightMotorForward(base_speed);
    }

  }//沿墙运动结束

}//x=5 左边结束

if (Serial.available() > 0) { Serial.println(x, DEC); } //主体结束

}

//读取选择器的位置 unsigned char SelectorPosition() { unsigned char x = PINC & 0x0F; return x; }

//循迹

//打开ground LED void groundLEDon(unsigned char lineIndex) { PORTJ &= ~(1<<lineIndex); }

//关闭ground LED void groundLEDoff(unsigned char lineIndex) { PORTJ |= 1<<lineIndex; } //读取第lineIndex个传感器的值 int readGroundSensor(unsigned char lineIndex) { return analogRead(analogPin[lineIndex]); } //读出第地面传感器的值 bool readGroundSensorsIsBlack(unsigned char lineIndex) { groundLEDon(lineIndex); delay(1); int val=readGroundSensor(lineIndex); groundLEDoff(lineIndex); Serial.print('Ground Sensor '); Serial.print(lineIndex); Serial.print(' : '); Serial.println(val); if(val>threshold) //检测到黑色 { return true; }else{ return false; } }

//初始化电动机相关引脚为输出模式 void initMotor() { pinMode(PWM_right, OUTPUT); pinMode(PWM_left, OUTPUT); pinMode(right_dir, OUTPUT); pinMode(left_dir, OUTPUT); }

//沿墙

void proxLEDon(unsigned char proxIndex){ PORTA |= 1<<proxIndex; }

void proxLEDoff(unsigned char proxIndex){ PORTA &= ~(1<<proxIndex); }

//初始化红外LED void initIRLED() { DDRA |= B11111111; PORTA &= B00000000; }

int readProximityDistance(unsigned char proxIndex){

  proxLEDon(proxIndex);
  delay(1);          
  proximity[proxIndex] = analogRead(proximitySensorsID[proxIndex]); 
  proxLEDoff(proxIndex);

}

//绿灯亮 void greenLEDon(unsigned char LEDno) { DDRL |= 1<<LEDno; PORTL |= 1<<LEDno; }

void greenLEDoff(unsigned char LEDno) { DDRL |= 1<<LEDno; PORTL &= ~(1<<LEDno); }

//前后左右转

//左转 void leftTurn(float duty) { leftMotorForward(0); rightMotorForward(duty); } //右转 void rightTurn(float duty) { leftMotorForward(duty); rightMotorForward(0); } //前进 void forWard(float duty) { leftMotorForward(duty); rightMotorForward(duty); } void MotorStop(){ iCurLeftMotorForwardSpeed = 0; iCurRightMotorForwardSpeed = 0; iCurLeftMotorBackwardSpeed = 0; iCurRightMotorBackwardSpeed = 0; digitalWrite(right_dir, HIGH); digitalWrite(PWM_right, HIGH);

digitalWrite(left_dir, HIGH); digitalWrite(PWM_left, HIGH); }

//左轮前进，速度duty void leftMotorForward(float duty){ int dutyInt = duty; if(iCurLeftMotorForwardSpeed != dutyInt) { int dutyVal = map(dutyInt,0,100,0,255);

analogWrite(PWM_left, dutyVal);
analogWrite(left_dir, 0);
iCurLeftMotorForwardSpeed = dutyInt;

} } //右轮前进，速度duty void rightMotorForward(float duty){ int dutyInt = duty; if(iCurRightMotorForwardSpeed != dutyInt) { int dutyVal = map(dutyInt,0,100,0,255); analogWrite(PWM_right, dutyVal); analogWrite(right_dir, 0); iCurRightMotorForwardSpeed = dutyInt; } } //左轮后退，速度duty void leftMotorBackward(float duty){ int dutyInt = duty; if(iCurLeftMotorBackwardSpeed != dutyInt) { int dutyVal = map(dutyInt,0,100,0,200);
analogWrite(PWM_left, 0); analogWrite(left_dir, dutyVal); iCurLeftMotorBackwardSpeed = dutyInt; } } //右轮后退，速度duty void rightMotorBackward(float duty){ int dutyInt = duty; if(iCurRightMotorBackwardSpeed != dutyInt) { int dutyVal = map(dutyInt,0,100,0,200); analogWrite(PWM_right, 0); analogWrite(right_dir, dutyVal); iCurRightMotorBackwardSpeed = dutyInt; } }

该code转弯部分如何改 leftTurn(12) 在不知道角度和时间的关系的情况下让他每次转弯都转90度 (注：此处的12指速度 怎么添加角度功能)内容：要让机器人每次转弯都转90度，需要在转弯函数中添加角度控制的代码。可以通过计算机器人转动的时间和速度来实现。具体实现方法如下：

1.首先，需要确定机器人转90度所需要的时间。可以通过实验来得到一个大致的数值，例如假设转90度需要1秒钟。

2.在转弯函数中，先设置左右轮的速度为0，让机器人停下来。

3.接着，根据转弯方向调整左右轮的速度，让机器人开始转弯。

4.在转弯过程中，使用delay函数控制机器人转弯的时间。根据实验得到的转弯时间，计算出需要延迟的时间，例如转90度需要1秒钟，那么就需要在转弯过程中延迟1秒钟。

5.转弯完成后，再将左右轮的速度设置为正常的速度，让机器人继续前进。

下面是修改后的转弯函数示例代码：

//左转90度 void leftTurn(float duty) { leftMotorForward(0); rightMotorForward(duty); delay(1000); //延迟1秒钟，让机器人转90度 leftMotorForward(duty); rightMotorForward(duty); }

//右转90度 void rightTurn(float duty) { leftMotorForward(duty); rightMotorForward(0); delay(1000); //延迟1秒钟，让机器人转90度 leftMotorForward(duty); rightMotorForward(duty); }

需要注意的是，由于机器人转弯的精度受到多种因素的影响，例如摩擦力、轮子的直径等，所以实际转弯的角度可能会略微偏差。如果需要更精确的转弯控制，可以通过使用陀螺仪等传感器来实现。

Arduino 机器人循迹和沿墙代码 - 优化版

原文地址: https://www.cveoy.top/t/topic/fVpT 著作权归作者所有。请勿转载和采集!

免费AI点我,无需注册和登录