int PWM_left = 6;int left_dir = 7;int PWM_right = 5;int right_dir = 2;int base_motor_speed = 10;沿着黑线路径走int count = 1;int threshold=940; 地面传感器感知黑色的阈值大于则是黑色小于则不是黑色沿墙int red_led = 11;volatile int proxim

int PWM_left = 6; int left_dir = 7; int PWM_right = 5; int right_dir = 2; int base_motor_speed = 10; //沿着黑线路径走 int count = 1; int threshold=940; //地面传感器感知黑色的阈值，大于则是黑色，小于则不是黑色 //沿墙 int red_led = 11; volatile int proximity[8] ; //全部引脚 unsigned char analogPin[20] = {A8,A9,A10,A11,A0,A1,A2,A3,A4,A5,A6,A7}; int distance=0; //表示距离，单位毫米 int iCurLeftMotorForwardSpeed=0,iCurRightMotorForwardSpeed=0,iCurLeftMotorBackwardSpeed,iCurRightMotorBackwardSpeed=0; //当前左轮和右轮的速度

void setup() { //初始化电动机相关引脚为输出模式 DDRJ |= 0x0F; DDRK &= ~0x0F; //将PK0-PK3设置为输入模式 initMotor(); //初始化沿墙相关引脚为输出模式 pinMode(red_led,OUTPUT); //初始化红外LED initIRLED(); Serial.begin(57600); }

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

//循迹运动

if(count<=4){ //若两边同时感应到则转弯，其余直行 if(leftIsBlack && rightIsBlack) { //感应到后先暂停，给予机器人反应机会 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{ forWard(12); Serial.println("forWard"); } delay(1000); }

//沿墙运动以及STOP

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

//沿墙
//如果proximity sensor 0检测到前方障碍物且proximity sensor 2未检测到右边墙，则右转
else if(readProximityDistance(0) > 400 && readProximityDistance(2) < 400){
  rightTurn(12);
  Serial.println("rightTurn");
  delay(1000);
}
//如果proximity sensor 6检测到左侧墙，则进行沿墙运动
else if(readProximityDistance(6) < 400){ 
  //沿左侧墙运动
  forWard(12);
  Serial.println("forWard");
  //如果proximity sensor 5检测到左侧墙，则左轮速度减小
  if(readProximityDistance(5) < 400){
    leftMotorForward(8);
  }
  //如果proximity sensor 7检测到左侧墙，则右轮速度减小
  else if(readProximityDistance(7) < 400){
    rightMotorForward(8);
  }
}
//继续沿左侧墙运动
else{
  forWard(12);
  Serial.println("forWard");
}

} }

//循迹

//打开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){

  int distance;
  proxLEDon(proxIndex);
  delay(1);          
  proximity[proxIndex] = analogRead(analogPin[proxIndex]); 
  proxLEDoff(proxIndex); 
  return proximity[proxIndex] > 400;

}

//前后左右转

//左转 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; } } //右轮后退，速度dut