//pin脚
int lgSP1=2; //1号光敏
int lgSP2=3; //2号光敏
int lgSP3=4; //3号光敏
//小车控制pin脚
int speedPin_M1 = 5; //M1 Speed Control
int speedPin_M2 = 6; //M2 Speed Control
int directionPin_M1 = 4; //M1 Direction Control
int directionPin_M2 = 7; //M1 Direction Control
//光敏读数
int lgS1;
int lgS2;
int lgS3;
int LR;
int FB;
//环境光初始值
int lgS10=0;
int lgS20=0;
int lgS30=0;
//其他参数
int threshold=100;//阙值
int thresholdLR=200;
int thresholdFBMin=100;
int thresholdFBMax=300;
int state=1;
int advanceSpeed=200;
int backSpeed=150;
int turnSpeed=250;
int moveState=0;//状态参数
int period=30000;
unsigned long time0;
unsigned long time1;
//*********************主程序*************************
void setup() {
// put your setup code here, to run once:
pinMode(lgSP1,INPUT);
pinMode(lgSP2,INPUT);
pinMode(lgSP3,INPUT);
//环境光初始化
for(int i=0;i<20;i++){
lgS10=analogRead(lgSP1)+lgS10;
lgS20=analogRead(lgSP2)+lgS20;
lgS30=analogRead(lgSP3)+lgS30;
delay(10);
}
lgS10=lgS10/10;
lgS20=lgS20/10;
lgS30=lgS30/10;
//设置通信
Serial.begin(9600);
}
void loop() {
carStop();
if(lgS1<threshold&&lgS2<threshold&&lgS3<threshold){//隔一段时间常态下更新光值
resetLight();
}
//读取环境光值
lgS1=analogRead(lgSP1)-lgS10;
lgS2=analogRead(lgSP2)-lgS20;
lgS3=analogRead(lgSP3)-lgS30;
printLR();
printFB();
Serial.println(" 无光");
while(lgS1>threshold||lgS2>threshold||lgS3>threshold){//环境光值变化大于阙值
delay(100);
lgS1=analogRead(lgSP1)-lgS10; //测量值减初始值
lgS2=analogRead(lgSP2)-lgS20;
LR=lgS1-lgS2; //左右光值差
//print0();printLR();printFB();
if(abs(LR)>thresholdLR){//左右光值差大于阙值
swerve();
moveState=1; //行驶状态参数
}else{
carStop();
moveState=0;
printLR();
Serial.println(" 左右正常");
}
lgS1=analogRead(lgSP1)-lgS10;
lgS3=analogRead(lgSP3)-lgS30;
FB=lgS1-lgS3;
if(abs(FB)>thresholdFBMin&&abs(FB)<thresholdFBMax){//前后光值差大于阙值
walk();
}else if(moveState==1){
Serial.println(" 转弯中");
}else
carStop();
printFB();
Serial.println(" 前后正常");
}
}
//*********************函数*************************
//-----------------环境光初始函数-----------------
void resetLight(){
if(state==1){
time0= millis();
state=0;
}
time1=millis()-time0;//间隔一段时间
// Serial.print("time0=");
// Serial.print(time0);
// Serial.print("time1=");
// Serial.print(time1);
// Serial.println();
if(time1>period){//间隔一段时间后初始环境光
for(int i=0;i<10;i++){
lgS10=analogRead(lgSP1)+lgS10;
lgS20=analogRead(lgSP2)+lgS20;
lgS30=analogRead(lgSP3)+lgS30;
delay(10);
}
lgS10=lgS10/10;
lgS20=lgS20/10;
lgS30=lgS30/10;
state=1;
print0();
Serial.println(" success");
}
}
//-----------------打印函数-----------------
void print0(){
Serial.print("lgS10=");
Serial.print(lgS10);
Serial.print(" ");
Serial.print("lgS20=");
Serial.print(lgS20);
Serial.print(" ");
Serial.print("lgS30=");
Serial.print(lgS30);
}
void printLR(){
Serial.print("lgS1=");
Serial.print(lgS1);
Serial.print(" ");
Serial.print("lgS2=");
Serial.print(lgS2);
Serial.print(" ");
Serial.print("LR=");
Serial.print(LR);
}
void printFB(){
Serial.print("lgS1=");
Serial.print(lgS1);
Serial.print(" ");
Serial.print("lgS3=");
Serial.print(lgS3);
Serial.print(" ");
Serial.print("FB=");
Serial.print(FB);
}
//-----------------光判断函数-----------------
void swerve(){//转向判断函数
if(LR>10){
printLR();
Serial.println(" 左转");
carTurnRight(turnSpeed,turnSpeed);
}else if(LR<-10){
printLR();
Serial.println(" 右转");
carTurnLeft(turnSpeed,turnSpeed);
}
}
void walk(){//前进判断函数
if(FB>0){
printFB();
Serial.println(" 前进");
carAdvance(advanceSpeed,advanceSpeed);
// isCliff=analogRead(distLPin);
// isCliff=analogRead(distRPin);
}else if(FB<0){
printFB();
Serial.println(" 后退");
carBack(backSpeed,backSpeed);
}
}
//-----------------车运动函数-----------------
void carStop(){ // Motor Stop
digitalWrite(speedPin_M2,0);
digitalWrite(directionPin_M2,LOW);
digitalWrite(speedPin_M1,0);
digitalWrite(directionPin_M1,LOW);
}
void carAdvance(int leftSpeed,int rightSpeed){ //Move FORkward
analogWrite (speedPin_M2,leftSpeed); //PWM Speed Control
digitalWrite(directionPin_M2,LOW);
analogWrite (speedPin_M1,rightSpeed);
digitalWrite(directionPin_M1,HIGH);
}
void carBack(int leftSpeed,int rightSpeed){ //Move BACKrward
analogWrite (speedPin_M2,leftSpeed);
digitalWrite(directionPin_M2,HIGH);
analogWrite (speedPin_M1,rightSpeed);
digitalWrite(directionPin_M1,LOW);
}
void carTurnLeft(int leftSpeed,int rightSpeed){ //Turn Left
analogWrite (speedPin_M2,leftSpeed);
digitalWrite(directionPin_M2,HIGH);
analogWrite (speedPin_M1,rightSpeed);
digitalWrite(directionPin_M1,HIGH);
}
void carTurnRight(int leftSpeed,int rightSpeed){ //Turn Right
analogWrite (speedPin_M2,leftSpeed);
digitalWrite(directionPin_M2,LOW);
analogWrite (speedPin_M1,rightSpeed);
digitalWrite(directionPin_M1,LOW);
}
