tensorlayer3/______(2).ino

#include <Arduino.h>
#include <Wire.h>
#include <Servo.h>
double _ABVAR_1_i = 0.0 ;
int a = 0;
int b = 0;
Servo servo_pin_4;
//TCS230连接设置 
const int s0 = A0; 
const int s1 = A1; 
const int s2 = A4; 
const int s3 = A5; 
const int out = 8; //10
const int led = 7;
// Variables 
int red = 0; 
int green = 0; 
int blue = 0; 
void trunleft();
void trunright();
void forward();
void forward1();
void forward2();//20
void stop1();
void color() ;
int color_see();
void zha_qi_qiu();
const int zuo=3;
const int you=17;
int c=0;
 int _ABVAR_2_a = 0;
  int _ABVAR_3_b = 0;
void setup()
{
    pinMode( zuo, INPUT);
  pinMode( you, INPUT);
  pinMode( 16, INPUT);
  pinMode( 14, INPUT);
  pinMode( 10, OUTPUT);
  pinMode( 6, OUTPUT);
  pinMode( 5, OUTPUT);
  pinMode( 9, OUTPUT);
  _ABVAR_1_i = 0.0 ;
 
  servo_pin_4.attach(4);
 
 pinMode(s0, OUTPUT); 
 pinMode(s1, OUTPUT); 
 pinMode(s2, OUTPUT); 
 pinMode(s3, OUTPUT); 
 pinMode(out, INPUT); 
 pinMode(led, OUTPUT);
 digitalWrite(s0, HIGH); 
 digitalWrite(s1, HIGH); 

}

void loop()
{
 /* if(!(digitalRead(16)))
 {
   forward();
 
 }
 else*/ 
  {if (( digitalRead(you) && digitalRead(zuo) ))
  {
    forward();
  }
  else
  {
    if (( !( digitalRead(you) ) && digitalRead(zuo) ))
    {
      trunright();
    }
    else
    {
      if (( digitalRead(you) && !( digitalRead(zuo) ) ))
      {
        trunleft();
      }
      else
      {
        if (( !( digitalRead(you) ) && !( digitalRead(zuo) ) ))
        {
          _ABVAR_1_i = ( _ABVAR_1_i + 1 ) ;
          if (( ( _ABVAR_1_i ) == ( 1 ) ))
          {
            forward1();
            delay( 3500 );
          }
          else
          {
            if (( ( _ABVAR_1_i ) == ( 2 ) ))
            {
              forward1_1();
              delay( 3700 );
            }
            else
            {
              if (( ( _ABVAR_1_i ) == ( 3 ) ))
             {   
              stop1();
                      digitalWrite(led, HIGH);
                      delay(500);
                      color();
                      digitalWrite(led, LOW);
                      if (red < blue && red < green)
                      { 
                        _ABVAR_2_a = 1;
                //LcdDisplay(" - (Red Color)");
                      } 
                      else if (blue < red && blue < green) 
                      { 
                        _ABVAR_2_a = 2; 
                //      LcdDisplay(" - (Blue Color)");
                      } 
                      else if (green < red && green < blue) 
                      { 
                        _ABVAR_2_a = 3; 
                //      LcdDisplay(" - (Green Color)");
                      }
                      while(((_ABVAR_2_a)!= (_ABVAR_3_b)))
                      {
                        pan_duan_yan_se();
                      }
                      duo_ji();               
              /*int j;
              stop1();
              a =( color_see());
               for( j=1;j<=3;j++)
               {
                 forward2();
              delay( 1000 );
              stop1();
              b = color_see();
              if(a==b)
              {  zha_qi_qiu() ;
                 forward();
                      delay(4000);
                      stop1();
                       delay(4000);}
               }
            //zha wan qi qiu   */         
             }
             
             
            }//three time
          }//two time
        }// one time
      }
    }
  }
  }
}

void forward()
{
  analogWrite(9 , 180);
  analogWrite(10 , 0);
  analogWrite(5 , 193);
  analogWrite(6 , 0);
}

void trunleft()
{
  analogWrite(9 , 0);
  analogWrite(10 , 120);
  analogWrite(5 , 120);
  analogWrite(6 , 0);
}

void forward2()
{
  analogWrite(9 , 100);
  analogWrite(10 , 0);
  analogWrite(5 , 110);
  analogWrite(6 , 0);
}
void stop1()
{
  analogWrite(9 , 0);
  analogWrite(10 , 0);
  analogWrite(5 , 0);
  analogWrite(6 , 0);
}

void forward1()
{
  analogWrite(9 , 200);
  analogWrite(10 , 0);
  analogWrite(5 , 200);
  analogWrite(6 , 0);
}
void forward1_1()
{
  analogWrite(9 , 250);
  analogWrite(10 , 0);
  analogWrite(5 , 250);
  analogWrite(6 , 0);
}

void trunright()
{
  analogWrite(9 , 120);
  analogWrite(10 , 0);
  analogWrite(5 , 0);
  analogWrite(6 , 120);
}
/*int color_see()
  {
    delay(150);
      digitalWrite(led, HIGH);
      delay(150);
      color();
  
    
      if (red < blue && red < green)
      { 
        return 1;      
      } 
     
      else if (blue < red && blue < green) 
      { 
        return 2; 
      }      
      else if (green < red && green < blue) 
      { 
       return 3; 
      }         
  }
  */
void color() 
{ 
 digitalWrite(s2, LOW); 
 digitalWrite(s3, LOW); 
 red = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); 
 digitalWrite(s3, HIGH); 
 blue = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); 
 digitalWrite(s2, HIGH); 
 green = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); 
}
void duo_ji()
{
  for(int i=0;i<3;i++)
{
  servo_pin_4.write( 90 );
  delay( 300 );
  servo_pin_4.write( 180 );
  delay( 600 );
  servo_pin_4.write( 90 );
  delay( 300 );
}
}
void pan_duan_yan_se()
{
  forward();
  delay(600);
  stop1();
  digitalWrite(led, HIGH);
  delay(500);
  color();
  digitalWrite(led, LOW);
  if (red < blue && red < green)
  { 
    _ABVAR_3_b = 1;
//        LcdDisplay(" - (Red Color)");
  } 
 
  else if (blue < red && blue < green) 
  { 
    _ABVAR_3_b = 2; 
//        LcdDisplay(" - (Blue Color)");
  } 
 
  else if (green < red && green < blue) 
  { 
    _ABVAR_3_b = 3; 
//        LcdDisplay(" - (Green Color)");
  }                       
}
/*void zha_qi_qiu()
{
for(int i=0;i<3;i++)
{
 servo_pin_4.write(90);
 delay(1000);
 servo_pin_4.write(150);
 delay(1000);
 servo_pin_4.write(90);
 delay(1000);
}
}*/