#include <Servo.h> 
#include <Metro.h>
Metro measureDistance = Metro(50);
Metro sweepServo = Metro(20);

//Standard PWM DC control
int E1 = 5;     //M1 Speed Control
int E2 = 6;     //M2 Speed Control
int M1 = 4;    //M1 Direction Control
int M2 = 7;    //M1 Direction Control

  int speedPin_M1 = 5;     //M1 Speed Control
int speedPin_M2 = 6;     //M2 Speed Control
int directionPin_M1 = 4;     //M1 Direction Control
int directionPin_M2 = 7;     //M1 Direction Control

///For previous Romeo, please use these pins.
//int E1 = 6;     //M1 Speed Control
//int E2 = 9;     //M2 Speed Control
//int M1 = 7;    //M1 Direction Control
//int M2 = 8;    //M1 Direction Control
  
Servo myservo;  // create servo object to control a servo 
int pos = 60; 
int sweepFlag = 1;
int myMaxSpeed = 255;

int URPWM = 3; // PWM Output 0－25000US，Every 50US represent 1cm
int URTRIG= 10; // PWM trigger pin
uint8_t EnPwmCmd[4]={0x44,0x02,0xbb,0x01};    // distance measure command


void SensorSetup(){ 
  pinMode(URTRIG,OUTPUT);                     // A low pull on pin COMP/TRIG
  digitalWrite(URTRIG,HIGH);                  // Set to HIGH 
  pinMode(URPWM, INPUT);                      // Sending Enable PWM mode command
  for(int i=0;i<4;i++){
      Serial.write(EnPwmCmd[i]);
   } 
}

void carStop(){                 //  Motor Stop
  digitalWrite(speedPin_M2,0); 
  digitalWrite(directionPin_M1,LOW);    
  digitalWrite(speedPin_M1,0);   
  digitalWrite(directionPin_M2,LOW);    
}   

void carTurnLeft(int leftSpeed,int rightSpeed){         //Turn Left
  analogWrite (speedPin_M2,leftSpeed);              //PWM Speed Control
  digitalWrite(directionPin_M1,HIGH);    
  analogWrite (speedPin_M1,rightSpeed);    
  digitalWrite(directionPin_M2,HIGH);
} 

void carTurnRight(int leftSpeed,int rightSpeed){        //Turn Right
  analogWrite (speedPin_M2,leftSpeed);
  digitalWrite(directionPin_M1,LOW);   
  analogWrite (speedPin_M1,rightSpeed);    
  digitalWrite(directionPin_M2,LOW);
}

void carBack(int leftSpeed,int rightSpeed){             //Move backward
  analogWrite (speedPin_M2,leftSpeed);
  digitalWrite(directionPin_M1,LOW);    
  analogWrite (speedPin_M1,rightSpeed);    
  digitalWrite(directionPin_M2,HIGH);
}
void carAdvance(int leftSpeed,int rightSpeed){          //Move forward  
  analogWrite (speedPin_M2,leftSpeed);
  digitalWrite(directionPin_M1,HIGH);    
  analogWrite (speedPin_M1,rightSpeed);    
  digitalWrite(directionPin_M2,LOW);
}

  
void stop(void)                    //Stop
{
  digitalWrite(E1,LOW);   
  digitalWrite(E2,LOW);      
}   
void advance(char a,char b)          //Move forward
{
  /*analogWrite (E1,a);      //PWM Speed Control
  digitalWrite(M1,HIGH);    
  analogWrite (E2,b);    
  digitalWrite(M2,HIGH);*/
  carAdvance(a, b);
}  
void back_off (char a,char b)          //Move backward
{
  /*analogWrite (E1,a);
  digitalWrite(M1,LOW);   
  analogWrite (E2,b);    
  digitalWrite(M2,LOW);*/
  carBack(a, b);
}
void turn_L (char a,char b)             //Turn Left
{
  /*analogWrite (E1,a);
  digitalWrite(M1,LOW);    
  analogWrite (E2,b);    
  digitalWrite(M2,HIGH);*/
  carTurnLeft(a, b);
}
void turn_R (char a,char b)             //Turn Right
{
  /*analogWrite (E1,a);
  digitalWrite(M1,HIGH);    
  analogWrite (E2,b);    
  digitalWrite(M2,LOW);*/
  carTurnRight(a, b);
}

void carTurnSoftLeft() {
  carAdvance(0,myMaxSpeed);
}

void carTurnSoftRight() {
  carAdvance(myMaxSpeed, 0);
}

void setup(void) 
{ 
  int i;
  /*for(i=4;i<=7;i++)
    pinMode(i, OUTPUT);  */
  myservo.attach(9); 
  Serial.begin(9600);                         // Sets the baud rate to 9600
  SensorSetup();
  Serial.println("Run keyboard control");
} 
void loop(void) 
{
  if(Serial.available()){
    char val = Serial.read();
    Serial.write(val);
    if(val != -1)
    {
      switch(val)
      {
      case 'z'://Move Forward
        advance (255,255);   //move forward in max speed
        break;
      case 'x'://Move Backward
        back_off (255,255);   //move back in max speed
        break;
      case 'q'://Turn Left
        turn_L (150,150);
        break;       
      case 'd'://Turn Right
        turn_R (150,150);
        break;
      case 'w':
        Serial.println("Hello");
        break;
      case 'a':
        carTurnSoftLeft();
        break;
      case 'e':
        carTurnSoftRight();
        break;
      case 's':
        stop();
        break;
      }
    }
    else stop();  
  }
}