//引腳定義
const int left_Forward_PWM_Pin = 3; //左電機正轉控制引腳
const int left_Backward_PWM_pin = 5; //左電機反轉控制引腳
const int right_Forward_PWM_Pin = 6; //右電機正轉控制引腳
const int right_Backward_PWM_pin = 9; //右電機正轉控制引腳
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; //中心速度占空比、左、右電機占空比
void setup()
{
Serial.begin(9600); //串口設置，用于輸出，波特率9600
pinMode(left_Forward_PWM_Pin, OUTPUT); //配置各電機控制引腳為輸出
pinMode(left_Backward_PWM_pin, OUTPUT);
pinMode(right_Forward_PWM_Pin, OUTPUT);
pinMode(right_Backward_PWM_pin, OUTPUT);
analogWrite(left_Forward_PWM_Pin, 0); //初始化各電機正轉占空比, 0為停止
analogWrite(left_Backward_PWM_pin, 0);
analogWrite(right_Forward_PWM_Pin, 0);
analogWrite(right_Backward_PWM_pin, 0);
}
void loop()
{
DirectionControl(); //方向檢測
//根據方向量pos控制左右輪差速，當pos=0時小車直走，pos<0時左拐，pos>0時右拐
left_speed_Forward = speed_Forward + pos ; //左輪
right_speed_Forward = speed_Forward - pos + 10; //右輪， +10 為左右輪固有速度差補償
//限制電機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;}
//設置電機正轉占空比
analogWrite(left_Forward_PWM_Pin, left_speed_Forward); //左電機
analogWrite(right_Forward_PWM_Pin, right_speed_Forward); //左電機
delay(33); //延時33ms
}
void DirectionControl() //方向檢測函數
{
static int AD_Left, AD_Right = 0; //左右電磁傳感器AD值
static float posTemp; //位置臨時變量，為了提高精度，采用浮點型
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); //串口輸出，調試用
Serial.print(",");
Serial.println(AD_Right);
if(AD_Left+AD_Right>50) //當左右電磁傳感器AD值之和較大時
//說明：如果左右電磁傳感器AD值之和比較小，則說明傳感器偏離太遠，保持之前的舵機角度
{
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); //除以和值，為相對位置
posTemp = 0 - posTemp*145; //轉換成電磁線位置控制占空比（浮點），這里 *100 為比例值，值越大轉向越靈敏
pos = int(posTemp); //位置值取整
speed_Forward = 265; //正常前進速度
}
else if(AD_Left+AD_Right<10) //如果當左右電磁傳感器AD值之和特別小，則說明已經沖出賽道較遠，改為蝸牛速
{
speed_Forward = 0; //蝸牛速
}
}
|
