|
|
bool cross;
void setup() {
// put your setup code here, to run once:
pinMode(12,INPUT);
pinMode(13,INPUT);
pinMode(8,INPUT);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pinMode(9,OUTPUT);
pinMode(10,OUTPUT);
Serial.begin(9600);
cross=false;
Serial.println("Initialization completion!");
}
#define LS 12
#define RS 13
#define MS 8
#define LMF 9
#define LMB 10
#define RMF 5
#define RMB 6
void loop() {
// put your main code here, to run repeatedly:
void TURNLEFT();
void TURNRIGHT();
void FORWARD();
void STOP();
static int i=0;
if(digitalRead(LS)==0&&digitalRead(RS)==0&&cross==0)
{
Serial.println("001");
FORWARD();
}
if(digitalRead(LS)==1&&digitalRead(RS)==1&&digitalRead(MS)==1)
{
Serial.println("111");
cross=true;
i++;
switch(i%2)
{
case 1:
FORWARD();
delay(650);
do
{
TURNRIGHT();
Serial.print("cross1=");
Serial.println(cross);
if(digitalRead(LS)==0&&digitalRead(RS)==0&&digitalRead(MS)==1)
break;
}while(1);
Serial.println("001");
break;
case 0:
FORWARD();
delay(650);
do
{
TURNLEFT();
Serial.print("cross1=");
Serial.println(cross);
if(digitalRead(LS)==0&&digitalRead(RS)==0&&digitalRead(MS)==1)
break;
}while(1);
Serial.println("002");
break;
default :STOP();break;
}
Serial.print("i%2=");
Serial.println(i%2);
cross=false;
Serial.print("cross2=");
Serial.println(cross);
}
if(digitalRead(LS)==0&&digitalRead(RS)==1&&cross==0)
{
Serial.println("01");
TURNRIGHT();
}
if(digitalRead(LS)==1&&digitalRead(RS)==0&&cross==0)
{
Serial.println("10");
TURNLEFT();
}
}
void TURNLEFT()
{
digitalWrite(LMF, LOW);
analogWrite(LMB,100);
analogWrite(RMF,200);
digitalWrite(RMB,LOW);
}
void TURNRIGHT()
{
analogWrite(LMF, 200);
digitalWrite(LMB,LOW);
digitalWrite(RMF,LOW);
analogWrite(RMB,100);
}
void FORWARD()
{
analogWrite(LMF,200);
digitalWrite(LMB,LOW);
analogWrite(RMF,200);
digitalWrite(RMB,LOW);
}
void STOP()
{
digitalWrite(LMF, LOW);
digitalWrite(LMB,LOW);
digitalWrite(RMF,LOW);
digitalWrite(RMB,LOW);
} |
|
