#include <Servo.h>
#define CLK1  12
#define CW1   11
#define CLK2  2
#define CW2   3
#define CLK3  7
#define CW3   8
#define pumpPin         41
#define valvePin        40
#define sensor1        	23
#define sensor2        	27
#define buzzer        	33

Servo SERVO_HAND; 
Servo SERVO_HAND_ROT;

void jiaodu(int motor,int fangxiang,int x)
{ 
    long int a=x*16/1.8;
    Serial.print(a);
    if(motor ==1)
   {
  	if(fangxiang ==1)
  	{
    	digitalWrite(CW1,HIGH);
    	delay(50);
    	for(int i=0;i<a;++i)
 		{
    		digitalWrite(CLK1,HIGH);
    		delayMicroseconds(100);
    		digitalWrite(CLK1,LOW);
    		delayMicroseconds(100);
   		//Serial.print(fangxiang);

  		}
  	}
 	if(fangxiang ==0)
  		{
		digitalWrite(CW1,LOW);
  		delay(50);
    		for(int i=0;i<a;++i)
 			{  
			digitalWrite(CLK1,HIGH);
    			delayMicroseconds(100);
    			digitalWrite(CLK1,LOW);
     			//digitalWrite(CLK1,LOW);กก
    			delayMicroseconds(100);
  			}
  		}
  }
 	if(motor ==2)
    	{
  		if(fangxiang ==1)
  		{
		digitalWrite(CW2,HIGH);
  		delay(50);
    		for(int i=0;i<a;++i)
 			{
    			digitalWrite(CLK2,HIGH);
    			delayMicroseconds(100);
    			digitalWrite(CLK2,LOW);
     			delayMicroseconds(100);
  			}

  		}
 		if(fangxiang ==0)
  			{
			digitalWrite(CW2,LOW);
  			delay(50);
    			for(int i=0;i<a;++i)
 				{  
				digitalWrite(CLK2,HIGH);
   				delayMicroseconds(100);
    				digitalWrite(CLK2,LOW);
     				delayMicroseconds(100);
  				}
  			}
  	}
    		if(motor ==3)
    		{
  		if(fangxiang ==1)
  			{
			digitalWrite(CW3,HIGH);
  			delay(50);
    			for(int i=0;i<a;++i)
 				{
    				digitalWrite(CLK3,HIGH);
     				delayMicroseconds(100);
    				digitalWrite(CLK3,LOW);
   				delayMicroseconds(100);
  				}
  			}
 			if(fangxiang ==0)
  			{
			digitalWrite(CW3,LOW);
  			delay(50);
    			for(int i=0;i<a;++i)
 				{  
				digitalWrite(CLK3,HIGH);
     				delayMicroseconds(100);
    				digitalWrite(CLK3,LOW);
     				delayMicroseconds(100);
  				}
  			}
  		}


}


void setup() 
{
  pinMode(CLK1,OUTPUT);
  pinMode(CW1,OUTPUT);
  pinMode(CLK2,OUTPUT);
  pinMode(CW2,OUTPUT);
  pinMode(CLK3,OUTPUT);
  pinMode(CW3,OUTPUT);
//pinMode(EN,OUTPUT);
  pinMode(pumpPin, OUTPUT);  
  pinMode(valvePin, OUTPUT);
  pinMode(sensor1,INPUT);
  pinMode(sensor2,INPUT);
  pinMode(buzzer,OUTPUT);
  Serial.begin(9600);
  Serial1.begin(9600);
  digitalWrite(pumpPin,HIGH);
  digitalWrite(valvePin,HIGH);
//digitalWrite(buzzer,HIGH);
  SERVO_HAND.attach(9); 
  SERVO_HAND_ROT.attach(48);
  SERVO_HAND.write(10); 
  SERVO_HAND_ROT.write(180); 



//jiaodu(1,1,60);
//put your setup code here, to run once:

}
 void loop() 
{

//if(digitalRead(sensor1) == 1||digitalRead(sensor2) == 1)
//{digitalWrite(buzzer,HIGH);}


//1 rot 2 dabi 3 xiaobi 2 and 3 expecial!

   while(Serial1.available())
   {
   	char c=Serial1.read();
      	if(c=='Q')
        { 
		Serial1.print('a');
          	jiaodu(1,1,30);
        }
        if(c=='A')
        {
          	jiaodu(1,0,30);
        }
        if(c=='W')
        {
          	jiaodu(2,1,90);
        }
        if(c=='S')
        {
          	jiaodu(2,0,90);
        }
        if(c=='E')
        {
          	jiaodu(3,1,90);
        }
        if(c=='D')
        {
          	jiaodu(3,0,90);
  	}
        if(c=='R')
        { 
		int pos1 = SERVO_HAND_ROT.read(); 
		int pos2 = pos1+30;
          	if (pos2 >= 180)
         		pos2 = 180 ;
		SERVO_HAND_ROT.write(pos2); 
          	delay(400);

        }
        if(c=='F')
        {
         	int pos3 = SERVO_HAND_ROT.read(); 
		int pos4 = pos3 - 30;
          	if (pos4 <= 5)
         		pos4 = 5 ;
		SERVO_HAND_ROT.write(pos4);
          	delay(400);
        }
        if(c=='T')
        {
          	SERVO_HAND.write(10); 	//expand
          	delay(400);
        }
        if(c=='G')
        {
          	SERVO_HAND.write(60); 	//bihe
          	delay(400);
        }
        if(c=='B')
        { 
		digitalWrite(buzzer,HIGH);
        }
   }
//jiaodu(2,0,400);
//jiaodu(3,0,400);
//jiaodu(2,1,400);
//jiaodu(3,1,400);
//jiaodu(1,0,120);
 }