|
- #include <Servo.h>
-
- int _ABVAR_1_i = 0 ;
- Servo servo_pin_4;
- const int s0 = A3;
- const int s1 = A4;
- const int s2 = A1;
- const int s3 = A5;
- const int out = 8;
- const int led = 7;
- const int xun_ji1 = A2;
- const int xun_ji2 = 3;
- const int xun_ji3 = A0;
- int red = 0;
- int green = 0;
- int blue = 0;
- int _ABVAR_2_a = 0;
- int _ABVAR_3_b = 0;
-
- void trunright();
- void trunleft();
- void stop();
- void duo_ji();
- void forwoard_drop();
- void forwoard_rise();
- void back();
- void forwoard();
- void color();
- void pan_duan_yan_se();
-
- void setup()
- {
- pinMode(xun_ji1 , INPUT);
- pinMode( xun_ji2, INPUT);
- pinMode( xun_ji3, INPUT);
- pinMode( 10, OUTPUT);
- pinMode( 6, OUTPUT);
- pinMode( 5, OUTPUT);
- pinMode( 9, OUTPUT);
- pinMode( 9 , OUTPUT);
- pinMode( 10 , OUTPUT);
- pinMode( 5 , OUTPUT);
- pinMode( 6 , OUTPUT);
- servo_pin_4.attach(4);
- _ABVAR_1_i = 0 ;
- 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(xun_ji3) && digitalRead(xun_ji1) ) && digitalRead(xun_ji2) ))
- {
- back();
- }
- else
- {
- if (( ( digitalRead(xun_ji3) && digitalRead(xun_ji2) ) && !( digitalRead(xun_ji1) ) ))
- {
- trunleft();
- }
- else
- {
- if (( ( digitalRead(xun_ji3) && digitalRead(xun_ji1) ) && !( digitalRead(xun_ji2) ) ))
- {
- forwoard();
- }
- else
- {
- if (( digitalRead(xun_ji3) && ( !( digitalRead(xun_ji2) ) && !( digitalRead(xun_ji1) ) ) ))
- {
- trunleft();
- }
- else
- {
- if (( !( digitalRead(xun_ji3) ) && ( digitalRead(xun_ji2) && digitalRead(xun_ji1) ) ))
- {
- trunright();
- }
- else
- {
- if (( ( !( digitalRead(xun_ji3) ) && !( digitalRead(xun_ji2) ) ) && digitalRead(xun_ji1) ))
- {
- trunright();
- }
- else
- {
- if (( ( !( digitalRead(xun_ji3) ) && !( digitalRead(xun_ji2) ) ) && !( digitalRead(xun_ji1) ) ))
- {
- _ABVAR_1_i = ( _ABVAR_1_i + 1 ) ;
- if (( ( _ABVAR_1_i ) == ( 1 ) ))
- {
- forwoard_rise();
- delay( 1500 );
- forwoard();
- delay( 5000 );
- }
- else
- {
- if (( ( _ABVAR_1_i ) == ( 2 ) ))
- {
- forwoard_rise();
- delay( 3000 );
- forwoard();
- delay( 500 );
- }
- else
- {
- if (( ( _ABVAR_1_i ) == ( 3 ) ))
- {
- stop();
- 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();
- }
- }
- }
- }
- }
- }
- }
- }
- }
- }
- }
-
- void trunright()
- {
- analogWrite(9 , 180);
- analogWrite(10 , 0);
- analogWrite(5 , 0);
- analogWrite(6 , 0);
- }
-
- void stop()
- {
- digitalWrite( 9 , LOW );
- digitalWrite( 10 , LOW );
- digitalWrite( 5 , LOW );
- digitalWrite( 6 , LOW );
- }
-
- void forwoard_drop()
- {
- analogWrite(9 , 80);
- analogWrite(10 , 0);
- analogWrite(5 , 80);
- analogWrite(6 , 0);
- }
-
- void duo_ji()
- {
- servo_pin_4.write( 90 );
- delay( 300 );
- servo_pin_4.write( 180 );
- delay( 600 );
- servo_pin_4.write( 90 );
- delay( 300 );
- }
-
- void trunleft()
- {
- analogWrite(9 , 0);
- analogWrite(10 , 0);
- analogWrite(5 , 180);
- analogWrite(6 , 0);
- }
-
- void forwoard()
- {
- analogWrite(9 , 180);
- analogWrite(10 , 0);
- analogWrite(5 , 180);
- analogWrite(6 , 0);
- }
-
- void back()
- {
- analogWrite(9 , 0);
- analogWrite(10 , 120);
- analogWrite(5 , 0);
- analogWrite(6 , 120);
- }
-
- void forwoard_rise()
- {
- analogWrite(9 , 255);
- analogWrite(10 , 0);
- analogWrite(5 , 255);
- analogWrite(6 , 0);
- }
-
- void color()
- {
- digitalWrite(s2, LOW);
- digitalWrite(s3, LOW);
- //count OUT, pRed, RED
- red = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);
- digitalWrite(s3, HIGH);
- //count OUT, pBLUE, BLUE
- blue = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);
- digitalWrite(s2, HIGH);
- //count OUT, pGreen, GREEN
- green = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);
- }
- void pan_duan_yan_se()
- {
- forwoard();
- delay(600);
- stop();
- 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)");
- }
- }
-
-
|