main.c

#include <Ultrasonic.h>

void stop(); //暫停
void gs(); //直走

void bl(); //大左轉
void ml(); //中左轉
void sl(); //小左轉

void br(); //大右轉
void mr(); //中右轉
void sr(); //小右轉

int _ABVAR_1_L2 = 0 ;
int _ABVAR_2_L1 = 0 ;
int _ABVAR_3_R1 = 0 ;
int _ABVAR_4_R2 = 0 ;
int _ABVAR_5_IR = 0 ;
int _ABVAR_6_MFSpeed = 0 ;
int _ABVAR_7_MBSpeed = 0 ;
int _ABVAR_8_UltraSonic = 0 ;

// 黑1 白0
bool bool_ABVAR_1_L2 = 0 ;
bool bool_ABVAR_2_L1 = 0 ;
bool bool_ABVAR_3_R1 = 0 ;  
bool bool_ABVAR_4_R2 = 0 ;

// 直到回到正軌 碰到LR2變1 直到碰到LR1才變回0
bool bool_overL = 0;
bool bool_overR = 0;


int ardublockUltrasonicSensorCodeAutoGeneratedReturnCM(int trigPin, int echoPin) //測距離
{
  long duration;
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(20);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  duration = duration / 59;
  if ((duration < 2) || (duration > 300)) return false;
  return duration;
}

void US(); // 超音波

void L2_R2();
void L1_R1();

void setup()
{
  pinMode( 11 , OUTPUT);
  pinMode( 5 , OUTPUT);
  pinMode( 6 , OUTPUT);
  pinMode( 10 , OUTPUT);
  digitalWrite( 12 , LOW );

  Serial.begin(9600);
  pinMode( 7 , OUTPUT);
  pinMode( 8 , OUTPUT);
  pinMode( 2 , OUTPUT);
  pinMode( 3 , OUTPUT);
  pinMode( 6 , OUTPUT);
  pinMode( 11 , OUTPUT);
  pinMode( 5 , OUTPUT);
  pinMode( 10 , OUTPUT);
}

void loop()
{
  _ABVAR_1_L2 = analogRead(0) ;
  _ABVAR_2_L1 = analogRead(1) ;
  _ABVAR_3_R1 = analogRead(2) ;
  _ABVAR_4_R2 = analogRead(3) ;
  _ABVAR_5_IR = 850 ; // 大於多少是白色
  _ABVAR_6_MFSpeed = 175 ; //右馬達速度
  _ABVAR_7_MBSpeed = 170 ; //左馬達速度
  _ABVAR_8_UltraSonic = ardublockUltrasonicSensorCodeAutoGeneratedReturnCM( 12 , 13 ) ;
 

  // bool bool_ABVAR_1_L2 = 0 ;
  // bool bool_ABVAR_2_L1 = 0 ;
  // bool bool_ABVAR_3_R1 = 0 ;
  // bool bool_ABVAR_4_R2 = 0 ;
  if (( _ABVAR_1_L2 ) >= ( _ABVAR_5_IR )) {
    bool_ABVAR_1_L2 = 0;
  } else {
    bool_ABVAR_1_L2 = 1;
  }
  if (( _ABVAR_2_L1 ) >= ( _ABVAR_5_IR )) {
    bool_ABVAR_2_L1 = 0;
  } else {
    bool_ABVAR_2_L1 = 1;
  }
  if (( _ABVAR_3_R1 ) >= ( _ABVAR_5_IR )) {
    bool_ABVAR_3_R1 = 0;
  } else {
    bool_ABVAR_3_R1 = 1;
  }
  if (( _ABVAR_4_R2 ) >= ( _ABVAR_5_IR )) {
    bool_ABVAR_4_R2 = 0;
  } else {
    bool_ABVAR_4_R2 = 1;
  }

  // print
  Serial.print("L2 =");
  Serial.print(_ABVAR_1_L2);
  Serial.print("    ");
  Serial.print(bool_ABVAR_1_L2);
  Serial.println();

  Serial.print("L1 =");
  Serial.print(_ABVAR_2_L1);
  Serial.print("    ");
  Serial.print(bool_ABVAR_2_L1);
  Serial.println();

  Serial.print("R1 =");
  Serial.print(_ABVAR_3_R1);
  Serial.print("    ");
  Serial.print(bool_ABVAR_3_R1);
  Serial.println();

  Serial.print("R2 =");
  Serial.print(_ABVAR_4_R2);
  Serial.print("    ");
  Serial.print(bool_ABVAR_4_R2);
  Serial.println();


  Serial.print("US =");
  Serial.print(_ABVAR_8_UltraSonic);
  Serial.println();
  digitalWrite( 7 , LOW );
  digitalWrite( 8 , LOW );
  digitalWrite( 2 , LOW );
  digitalWrite( 3 , LOW );

  // L1_R1();
  // L2_R2();
  judge();
  // US();
}

void judge(){ //判斷
  US();
  digitalWrite( 7 , LOW);
  digitalWrite( 8 , LOW);
  digitalWrite( 2 , LOW);
  digitalWrite( 3 , LOW);
  digitalWrite( 6 , LOW);
  digitalWrite( 11 , LOW);
  digitalWrite( 5 , LOW);
  digitalWrite( 10 , LOW);

  if (bool_ABVAR_3_R1 && bool_ABVAR_1_L2 && bool_ABVAR_2_L1 && bool_ABVAR_4_R2) { //都黑色
    stop();
    bool_overR = 0;
    bool_overL = 0;
  }

  if (!bool_ABVAR_3_R1 && !bool_ABVAR_1_L2 && !bool_ABVAR_2_L1 && !bool_ABVAR_4_R2) { //都白色
    if (bool_overR) {
      br();
    } else if (bool_overL) {
      bl();
    } else {
      gs();
    }
  }

  if (bool_ABVAR_3_R1 && !bool_ABVAR_4_R2) { //正正
    sr();
    bool_overR = 0;
  }

  if (bool_ABVAR_2_L1 && !bool_ABVAR_1_L2) { //正正
    sl();
    bool_overL = 0;
  }

  if (bool_ABVAR_1_L2 && !bool_ABVAR_2_L1) { //正反
    ml();
    bool_overL = 1;
  }

  if (bool_ABVAR_4_R2 && !bool_ABVAR_3_R1) { //正反
    mr();
    bool_overR = 1;
  }

  if (bool_ABVAR_3_R1 && bool_ABVAR_4_R2) { //正反 兩個都碰
    mr();
    bool_overR = 1;
  }

  if (bool_ABVAR_2_L1 && bool_ABVAR_1_L2) { //正反 兩個都碰
    ml();
    bool_overL = 1;
  }

}

void US()
{
  if (( ( ( _ABVAR_8_UltraSonic ) < ( 18 ) ) && ( ( _ABVAR_8_UltraSonic ) > ( 0 ) ) ))
  {
    stop();
  }
}

void stop() {
  digitalWrite( 6 , LOW );
  analogWrite(5 , 0);
  digitalWrite( 11 , LOW );
  analogWrite(10 , 0);
  digitalWrite( 7 , LOW );
  digitalWrite( 8 , LOW );
  digitalWrite( 2 , LOW );
  digitalWrite( 3 , LOW );
  delay( 100 );
  digitalWrite( 7 , HIGH );
  digitalWrite( 8 , HIGH );
  digitalWrite( 2 , HIGH );
  digitalWrite( 3 , HIGH );
  delay( 100 );
};
void gs() { //直走
  digitalWrite( 2 , HIGH );
  digitalWrite( 8 , HIGH );
  analogWrite(5 , 170);
  analogWrite(10 , 170);
};
void sl() { //小左轉
  digitalWrite( 2 , HIGH );
  digitalWrite( 8 , HIGH );
  analogWrite(5 , 130);
  analogWrite(10 , 170);
};
void ml() { //中左轉
  digitalWrite( 2 , HIGH );
  digitalWrite( 8 , HIGH );
  analogWrite(6 , 0);
  analogWrite(10 , 170);
}
void bl() { //大左轉
  digitalWrite( 2 , HIGH );
  digitalWrite( 8 , HIGH );
  analogWrite(6 , 170);
  analogWrite(10 , 170);
};
void sr() { //小右轉
  digitalWrite( 2 , HIGH );
  digitalWrite( 8 , HIGH );
  analogWrite(5 , 170);
  analogWrite(10 , 130);
};
void mr() { //中右轉
  digitalWrite( 2 , HIGH );
  digitalWrite( 8 , HIGH );
  analogWrite(5 , 170);
  analogWrite(11 , 0);
};
void br() { //大右轉
  digitalWrite( 2 , HIGH );
  digitalWrite( 8 , HIGH );
  analogWrite(5 , 170);
  analogWrite(11 , 170);
};

void L2_R2()
{
  if (( ( ( _ABVAR_1_L2 ) < ( _ABVAR_5_IR ) ) && ( ( ( _ABVAR_2_L1 ) > ( _ABVAR_5_IR ) ) && ( ( ( _ABVAR_3_R1 ) > ( _ABVAR_5_IR ) ) && ( ( _ABVAR_4_R2 ) > ( _ABVAR_5_IR ) ) ) ) ))
  {
    digitalWrite( 7 , HIGH );
    while ( ( ( analogRead(1) ) > ( _ABVAR_5_IR ) ) )
    {
      digitalWrite( 5 , LOW );
      analogWrite(6 , _ABVAR_7_MBSpeed);
      digitalWrite( 11 , LOW );
      analogWrite(10 , _ABVAR_6_MFSpeed);
    }

  }
  if (( ( ( _ABVAR_4_R2 ) < ( _ABVAR_5_IR ) ) && ( ( ( _ABVAR_3_R1 ) > ( _ABVAR_5_IR ) ) && ( ( ( _ABVAR_2_L1 ) > ( _ABVAR_5_IR ) ) && ( ( _ABVAR_1_L2 ) > ( _ABVAR_5_IR ) ) ) ) ))
  {
    digitalWrite( 3 , HIGH );
    while ( ( ( analogRead(2) ) >= ( _ABVAR_5_IR ) ) )
    {
      digitalWrite( 6 , LOW );
      analogWrite(5 , _ABVAR_6_MFSpeed);
      digitalWrite( 10 , LOW );
      analogWrite(11 , _ABVAR_7_MBSpeed);
    }

  }
}

void L1_R1()
{
  if (( ( ( _ABVAR_2_L1 ) < ( _ABVAR_5_IR ) ) && ( ( ( _ABVAR_1_L2 ) > ( _ABVAR_5_IR ) ) && ( ( ( _ABVAR_3_R1 ) > ( _ABVAR_5_IR ) ) && ( ( _ABVAR_4_R2 ) > ( _ABVAR_5_IR ) ) ) ) ))
  {
    digitalWrite( 8 , HIGH );
    digitalWrite( 5 , LOW );
    analogWrite(6 , _ABVAR_7_MBSpeed);
    digitalWrite( 11 , LOW );
    analogWrite(10 , _ABVAR_6_MFSpeed);
  }
  if (( ( ( _ABVAR_3_R1 ) < ( _ABVAR_5_IR ) ) && ( ( ( _ABVAR_4_R2 ) > ( _ABVAR_5_IR ) ) && ( ( ( _ABVAR_2_L1 ) > ( _ABVAR_5_IR ) ) && ( ( _ABVAR_1_L2 ) > ( _ABVAR_5_IR ) ) ) ) ))
  {
    digitalWrite( 2 , HIGH );
    digitalWrite( 6 , LOW );
    analogWrite(5 , _ABVAR_6_MFSpeed);
    digitalWrite( 10 , LOW );
    analogWrite(11 , _ABVAR_7_MBSpeed);
  }
}