# arduino temp 2

int speed1 =150,R=0,L=0,greenL=0,greenR=0;//速度
int RedL=0, BlueL=0, GreenL=0,
    RedR=0, BlueR=0, GreenR=0;//左右
#define s0R 4       
#define s1R 5
#define s2R 6
#define s3R 7
#define outR 8  //右模塊引腳接線
#define s0L 9       
#define s1L 10
#define s2L 11
#define s3L 12
#define outL 13  //左模塊引腳接線
#define speedfootR 2
#define speedfootL 3 //速度的腳位
#define motorL1 14
#define motorL2 15
#define motorR1 16
#define motorR2 17//定義馬達角位
#define infrared1 54
#define infrared2 55
#define infrared3 56
#define infrared4 57
#define infrared5 58//紅外線感應器的腳位定義
void straight()
{

   digitalWrite(14,1);
   digitalWrite(15,0);
   digitalWrite(16,1);
   digitalWrite(17,0);
   analogWrite(2,speed1);
   analogWrite(3,speed1);
}
void left()
{
   digitalWrite(14,0);
   digitalWrite(15,1);
   digitalWrite(16,1);
   digitalWrite(17,0);
   analogWrite(2,210);
   analogWrite(3,210);
}
void right()
{
   digitalWrite(14,1);
   digitalWrite(15,0);
   digitalWrite(16,0);
   digitalWrite(17,1);
   analogWrite(2,210);
   analogWrite(3,210);
}
void GetColorsL()
{
  digitalWrite(s2L,LOW);//S2L/S3L 級別定義我們使用哪一組光電二極管 LOW/LOW 代表紅色 LOW/HIGH 代表藍色，HIGH/HIGH 代表綠色                                           
  digitalWrite(s3L,LOW);                                           
  RedL = pulseIn(outL, digitalRead(outL) == HIGH ? LOW : HIGH);//這裡我們等到“out”變低，我們開始測量持續時間並在“out”再次為高時停止 
  delay(20);      
  digitalWrite(s3L,HIGH);//這裡我們選擇其他顏色（光電二極管組）並使用相同的技術測量其他顏色值                                         
  BlueL = pulseIn(outL, digitalRead(outL) == HIGH ? LOW : HIGH);
  delay(20);  
  digitalWrite(s2L,HIGH);    
  GreenL = pulseIn(outL, digitalRead(outL) == HIGH ? LOW : HIGH);
  delay(20);
}
void GetColorsR()
{
  digitalWrite(s2R,LOW);//S2R/S3R 級別定義我們使用哪一組光電二極管 LOW/LOW 代表紅色 LOW/HIGH 代表藍色，HIGH/HIGH 代表綠色                                           
  digitalWrite(s3R,LOW);                                           
  RedR = pulseIn(outR, digitalRead(outR) == HIGH ? LOW : HIGH);//這裡我們等到“out”變低，我們開始測量持續時間並在“out”再次為高時停止 
  delay(20);      
  digitalWrite(s3R,HIGH);//這裡我們選擇其他顏色（光電二極管組）並使用相同的技術測量其他顏色值                                         
  BlueR = pulseIn(outR, digitalRead(outR) == HIGH ? LOW : HIGH);
  delay(20);  
  digitalWrite(s2R,HIGH);    
  GreenR = pulseIn(outR, digitalRead(outR) == HIGH ? LOW : HIGH);
  delay(20);
}
//白==1  黑==0
void setup() 
{
   for(int i=14;i<=17;i++)//馬達
      pinMode(i,1);

   for(int i=54;i<=58;i++)//感測器
      pinMode(i,0);
  
   pinMode(s0R,OUTPUT);
   pinMode(s1R,OUTPUT);
   pinMode(s2R,OUTPUT);
   pinMode(s3R,OUTPUT);
   pinMode(outR,INPUT);//引腳模式

   digitalWrite(s0R,HIGH);
   digitalWrite(s1R,HIGH);
   
   pinMode(s0L,OUTPUT);
   pinMode(s1L,OUTPUT);
   pinMode(s2L,OUTPUT);
   pinMode(s3L,OUTPUT);
   pinMode(outL,INPUT);//引腳模式

   digitalWrite(s0L,HIGH);
   digitalWrite(s1L,HIGH);

   pinMode(19,OUTPUT);
   pinMode(20,OUTPUT);

   Serial.begin(9600);
}

void loop() 
{
   GetColorsL();
   GetColorsR();
   Serial.print(RedL);
   Serial.print(",");
   Serial.print(BlueL);
   Serial.print(","); 
   Serial.print(GreenL);
   Serial.print("  ");  
   Serial.print(RedR);
   Serial.print(",");
   Serial.print(BlueR);
   Serial.print(","); 
   Serial.println(GreenR);

   if(RedL - BlueL > 20 && RedL > 40 && GreenL < 25)
   {
      digitalWrite(20,HIGH);
      greenL = 1;
   }
   else
      digitalWrite(20,LOW); 
        
   if(RedR - BlueR > 20 && RedR > 40 && GreenR < 25)
   {
      digitalWrite(19,HIGH);
      greenR = 1;
   }
   else
      digitalWrite(19,LOW);       
    
   if(greenR == 1 || greenL == 1)
   {
           digitalWrite(motorL1,1);
           digitalWrite(motorL2,1);
           digitalWrite(motorR1,1);
           digitalWrite(motorR2,1);   
   }
   else
   {
   if(digitalRead(54)==0)//最左邊感測器54,55,56,57,58
   {
    straight();//直走
    delay(100);
    do
    {
    left();
    }while(digitalRead(57)==1);//先做一次左轉如果右邊感測器看到白色繼續左轉直到看到黑色跳出去,do while =先做一次，然後如果裡面要判斷的值是對的就形成迴圈不對就會跳出去了
    right();//右轉修正
    delay(100);    
    
   }
   else if(digitalRead(58)==0)//最右邊感測器
   {
    straight();//直走
    delay(100);
    do
    {
      right();
      
    }while(digitalRead(55)==1);//先做一次又轉如果左邊感測器看到白色繼續左轉直到看到黑色跳出去
    left();//左轉修正
    delay(100);
   }
   else
   {
    //straight();
    if(digitalRead(55)==0)//左邊感測器
      {
           digitalWrite(motorL1,0);
           digitalWrite(motorL2,1);
           L = 0;
      }
      else
      {
           digitalWrite(motorL1,1);
           digitalWrite(motorL2,0);
           L = 1;
      }
      if(digitalRead(infrared4)==0)//右邊感測器
      {
           digitalWrite(motorR1,0);
           digitalWrite(motorR2,1);
           R = 0;
      }
      else
      {
           digitalWrite(motorR1,1);
           digitalWrite(motorR2,0);
           R = 1;
      }
      
//      if(L + R == 2)
//        speed1 = 150;
//      else
//        speed1 = 210; 
        
      analogWrite(speedfootR,speed1);
      analogWrite(speedfootL,speed1);
   } 
   }
}