#include <Servo.h>    
//#include <Ultrasonic.h>
//#define TRIGGER_PIN  9
//#define ECHO_PIN     8
//Ultrasonic ultrasonic(TRIGGER_PIN, ECHO_PIN);
//1 
int EN2 = 2;  
int EN3 = 3;
//1 
int EN4 = 4;
int EN5 = 5;
int LED1 = 6; 
int LED2 = 7; 
int val,kkl,lkf;
int lx=90;
//2- 
Servo servoX; //    
Servo servoY; //Y     
int mkk;
///////////////////////////////////////
void sto(void)
        {
          digitalWrite(EN2,LOW);   
          digitalWrite(EN3,LOW);
          digitalWrite(EN4,LOW);   
          digitalWrite(EN5,LOW);       
        }
        
void left(void)
        {        
          digitalWrite(EN2,LOW);
          digitalWrite(EN3,HIGH);
          digitalWrite(EN4,LOW);
          digitalWrite(EN5,HIGH);
        }
void right(void)
        {        
          digitalWrite(EN2,HIGH);
          digitalWrite(EN3,LOW);
          digitalWrite(EN4,HIGH);
          digitalWrite(EN5,LOW);            
        }
void back(void)
        {        
          digitalWrite(EN2,LOW);
          digitalWrite(EN3,HIGH);
          digitalWrite(EN4,HIGH);
          digitalWrite(EN5,LOW);     
        }        
void forward(void)
        {        
          digitalWrite(EN2,HIGH);
          digitalWrite(EN3,LOW);
          digitalWrite(EN4,LOW);
          digitalWrite(EN5,HIGH);
          
        }  

// 
void servo_left()
{
  int servotemp=servoX.read();//
  servotemp-=2;//
  if(servotemp<170&&servotemp>10) //
  servoX.write(servotemp);
  else if (servotemp<=10)  servoX.write(10);
  else servoX.write(170);
}


// 
void servo_right()
{
  int servotemp=servoX.read();//
  servotemp+=2;//
  if(servotemp<170&&servotemp>10)//
  servoX.write(servotemp);
  else if (servotemp<=10)  servoX.write(10);
  else servoX.write(170);
}


// 
void servo_up()
{
  int servotemp1=servoY.read();
  servotemp1+=2;
  if(servotemp1<170&&servotemp1>10)
  servoY.write(servotemp1);
  else if (servotemp1<=10)  servoY.write(10);
  else servoY.write(170);
}


// 
void servo_down()
{
  int servotemp1=servoY.read();
  servotemp1-=2;
  if(servotemp1<170&&servotemp1>10)
  servoY.write(servotemp1);
  else if (servotemp1<=10)  servoY.write(10);
  else servoY.write(170);
}


void setup()
{
  servoX.attach(14);
  servoY.attach(15);
  servoX.write(90); //    90
  servoY.write(90); //    90
  Serial.begin(9600); //  9600bps
  pinMode(EN2,OUTPUT);
  pinMode(EN3,OUTPUT);
  pinMode(EN4,OUTPUT);
  pinMode(EN5,OUTPUT);
  pinMode(LED1,OUTPUT);
  pinMode(LED2,OUTPUT);
  for(kkl=0;kkl<65;kkl++)
  {
    digitalWrite(LED1,HIGH);
    delay(350);
    digitalWrite(LED2,HIGH);
    delay(350); // ,     WF171         
    digitalWrite(LED1,LOW);
    digitalWrite(LED2,LOW);
    delay(400);
  }
}


void loop()
{ 
  if(Serial.available())
  {
    lkf = Serial.read();
  switch(lkf)
  {
  case 'W':  //
    servoX.write(lx);
     forward();
     Serial.println("forward!"); 
     break;     
   case 'S': //
     back();
      Serial.println("back!"); 
     break;     
   case 'A': /
     left();
      Serial.println("left!"); 
     break;
   case 'D': //
    right();
     Serial.println("right!"); 
     break; 
   case 'E': //
    sto();
     Serial.println("stop!"); 
     break;  
   case 'L':  // 
     servo_left();
     break;
   case 'J': // 
     servo_right();
     break;
   case 'K': // 
    servo_up();
    break;
   case 'I': // 
     servo_down();
     break;
  case 'n': //  
     digitalWrite(LED1,HIGH);
     digitalWrite(LED2,HIGH);
     break;
  case 'm': // 
    digitalWrite(LED1,LOW);
    digitalWrite(LED2,LOW);
    break;
}
}
}


