//引脚定义
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; //蜗牛速
}
}
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
