投捡球综合程序

卡卡西

int servo1=A1;
int servo2=A0;
int in1=A2;
int in2=A3;
int in3=A4;
int in4=A5;
int ena=6;
int enb=5;
int en=3;
int en2=4;
#include<Servo.h>
Servo myservo1;
Servo myservo2;
void setup() {
pinMode(en,INPUT);
pinMode(en2,INPUT);
pinMode(servo1,OUTPUT);
pinMode(servo2,OUTPUT);
pinMode(in1,OUTPUT);
pinMode(in2,OUTPUT);
pinMode(in3,OUTPUT);
pinMode(in4,OUTPUT);
pinMode(ena,OUTPUT);
pinMode(enb,OUTPUT);
pinMode(2,INPUT);
myservo1.attach(servo1);
myservo2.attach(servo2);
myservo1.write(80);
myservo2.write(170);
}
void loop() {
int a=digitalRead(en);
if(a==HIGH)
{
  delay(1000);
myservo1.write(30);
delay(100);
myservo2.write(10);
delay(900);
qian();
delay(2000);
ting();
delay(1000);
myservo2.write(170);
delay(1000);
myservo1.write(100);
delay(100);
hou();
delay(2000);
ting();
}
int p=digitalRead(en2);
if(p==1)
{
  delay(1000);
myservo1.write(30);
delay(100);
myservo2.write(10);
delay(900);
qian();
delay(2000);
ting();
delay(1000);
myservo2.write(170);
delay(1000);
myservo1.write(100);
delay(3000);
myservo1.write(30);
delay(100);
myservo2.write(10);
delay(900);
hou();
delay(2000);
ting();  
myservo2.write(170);
delay(900);
myservo1.write(80);
}
int y=digitalRead(2);
if(y==1)
{
myservo1.write(30);
delay(1000);
myservo2.write(10);
delay(1000);
myservo2.write(100);
delay(3000);
myservo2.write(170);
delay(1000);
myservo1.write(80);
}
}
void qian()
{
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
digitalWrite(ena,HIGH);
digitalWrite(enb,HIGH);  
}
void hou()
{
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
digitalWrite(ena,HIGH);
digitalWrite(enb,HIGH);
}
void ting()
{
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
digitalWrite(ena,0);
digitalWrite(enb,0);
}