Arduino智能車入門程序

//引腳定義
const int left_Forward_PWM_Pin  = 3; //左電機(jī)正轉(zhuǎn)控制引腳
const int left_Backward_PWM_pin = 5; //左電機(jī)反轉(zhuǎn)控制引腳

const int right_Forward_PWM_Pin  = 6; //右電機(jī)正轉(zhuǎn)控制引腳
const int right_Backward_PWM_pin = 9; //右電機(jī)正轉(zhuǎn)控制引腳

const int AD_Left_Pin  = A0; //左電磁傳感器AD引腳
const int AD_Right_Pin = A1; //右電磁傳感器AD引腳

int pos; //傳感器位置變量
int speed_Forward,left_speed_Forward,right_speed_Forward; //中心速度占空比、左、右電機(jī)占空比

void setup()
{
  Serial.begin(9600);  //串口設(shè)置，用于輸出，波特率9600

  pinMode(left_Forward_PWM_Pin, OUTPUT);  //配置各電機(jī)控制引腳為輸出
  pinMode(left_Backward_PWM_pin, OUTPUT);
  pinMode(right_Forward_PWM_Pin, OUTPUT);
  pinMode(right_Backward_PWM_pin, OUTPUT);

  analogWrite(left_Forward_PWM_Pin, 0);   //初始化各電機(jī)正轉(zhuǎn)占空比, 0為停止
  analogWrite(left_Backward_PWM_pin, 0);
  analogWrite(right_Forward_PWM_Pin, 0);
  analogWrite(right_Backward_PWM_pin, 0);
}

void loop()
{
  DirectionControl(); //方向檢測

  //根據(jù)方向量pos控制左右輪差速，當(dāng)pos=0時(shí)小車直走，pos<0時(shí)左拐，pos>0時(shí)右拐
  left_speed_Forward  = speed_Forward + pos ;     //左輪
  right_speed_Forward = speed_Forward - pos + 10; //右輪， +10 為左右輪固有速度差補(bǔ)償

  //限制電機(jī)PWM控制輸出的極值（0<PWM<255）
  if(left_speed_Forward<20){left_speed_Forward=20;}
  if(left_speed_Forward>220){left_speed_Forward=220;}
  if(right_speed_Forward<20){right_speed_Forward=20;}
  if(right_speed_Forward>220){right_speed_Forward=220;}

  //設(shè)置電機(jī)正轉(zhuǎn)占空比
  analogWrite(left_Forward_PWM_Pin, left_speed_Forward);    //左電機(jī)
  analogWrite(right_Forward_PWM_Pin, right_speed_Forward);  //左電機(jī)

  delay(33); //延時(shí)33ms
}

void DirectionControl() //方向檢測函數(shù)
{
  static int AD_Left, AD_Right = 0; //左右電磁傳感器AD值
  static float posTemp; //位置臨時(shí)變量，為了提高精度，采用浮點(diǎn)型
  static float error,last_error=0.0;

  AD_Left  = analogRead(AD_Left_Pin);   //讀左電磁傳感器AD值
  AD_Right = analogRead(AD_Right_Pin);  //讀右電磁傳感器AD值H
  Serial.print(AD_Left); //串口輸出，調(diào)試用
  Serial.print(",");
  Serial.println(AD_Right);

  if(AD_Left+AD_Right>50) //當(dāng)左右電磁傳感器AD值之和較大時(shí)
  //說明：如果左右電磁傳感器AD值之和比較小，則說明傳感器偏離太遠(yuǎn)，保持之前的舵機(jī)角度
  {
    error=AD_Left-AD_Right;
    if(last_error==0)
    last_error=error;
    posTemp=(last_error+error)/2;
    last_error=error;
    posTemp /=(AD_Left+AD_Right); //除以和值，為相對(duì)位置
    posTemp = 0 - posTemp*145; //轉(zhuǎn)換成電磁線位置控制占空比（浮點(diǎn)），這里 *100 為比例值，值越大轉(zhuǎn)向越靈敏
    pos = int(posTemp); //位置值取整

    speed_Forward = 265; //正常前進(jìn)速度
  }
  else if(AD_Left+AD_Right<10) //如果當(dāng)左右電磁傳感器AD值之和特別小，則說明已經(jīng)沖出賽道較遠(yuǎn)，改為蝸牛速
  {
    speed_Forward = 0;  //蝸牛速
  }
}

原理圖能分享一下嗎？