#include <SoftwareSerial.h>
#include <Servo.h>

Servo servoRight;
Servo servoLeft;

SoftwareSerial BTSerial(10, 11); // RX, TX

byte dis[6];  //distance package
int t,i,pp;   //temp value


void setup(){
  Serial.begin(9600);
  BTSerial.begin(57600);    //BTSerial 9600,19200,38400,57600
  servoRight.attach(12);
  servoLeft.attach(13);
  dis[0] = 65;              //check value
}

void loop(){
  if (BTSerial.available()>0){
    pp=BTSerial.read();
    Serial.println(pp);
    switch(pp){
    case 'f':  //Forward
      Up();
    break;
    case 'x':  //Back
      Down();
    break;
    case 'a':  //Turn Left
      Left();
    break;
    case 'd':  //Turn Right
      Right();
    break;
    case 's':  //Stop
      stop();
    break;
    case 65:  //Return distance value
      for(i=5;i>0;i--){
        dis[i] = t%10;
        t/=10;
      }
      //Pack value
      for(i=0;i<=5;i++)
        BTSerial.write(dis[i]);
        //Return value
    break;
    default:
    break;
    }
  }
}
int Up(){
  servoRight.writeMicroseconds(1300);
  servoLeft.writeMicroseconds(1700);
  //delay(1000);
}
int Down(){
  servoRight.writeMicroseconds(1700);
  servoLeft.writeMicroseconds(1300);
  //delay(1000);  //視情況調整
}
int Right(){
  servoRight.writeMicroseconds(1700);
  servoLeft.writeMicroseconds(1700);
}
int Left(){
  servoRight.writeMicroseconds(1300);
  servoLeft.writeMicroseconds(1300);
}
int stop(){
  servoRight.writeMicroseconds(1500);
  servoLeft.writeMicroseconds(1500);
}

const int Motor_E2 = 5; // digital pin 5 of Arduino (PWM)    
const int Motor_E1 = 6;  // digital pin 6 of Arduino (PWM)  
const int Motor_M1 = 7;     // digital pin 7 of Arduino
const int Motor_M2 = 8;    // digital pin 8 of Arduino
char val;  // variable to receive data from the serial port(bluetooth)

void setup()  
{
  // Start serial communication at 9600
  Serial.begin(9600); 
  pinMode(Motor_M1, OUTPUT);
  pinMode(Motor_M2, OUTPUT);
}

void loop()
{
  if(Serial.available()>0)
  {
    val = Serial.read();
    switch(val)
    {
      case 'f':   // car forward
                forward(0, 255);
                break;
      case 'd':   // car backward
                backward(0, 255);
                break;
      case 'l':   // car turn left
                left(0, 255);
                break;
      case 'r':   // car turn right
                right(0, 255);
                break;
      case 's':   // car stop
                motorstop(0, 0);
                break;
    }     
  }
}

void motorstop(byte flag, byte motorspeed)
{
  Serial.println("Stop!");
  digitalWrite( Motor_E1, motorspeed);
  digitalWrite( Motor_E2, motorspeed);
}

void forward(byte flag, byte motorspeed)
{
  Serial.println("Forward!");

  digitalWrite( Motor_M1, HIGH);
  digitalWrite( Motor_M2, HIGH);

  analogWrite( Motor_E1, motorspeed);
  analogWrite( Motor_E2, motorspeed);
}

void backward(byte flag, byte motorspeed)
{
  Serial.println("Backward!");

  digitalWrite( Motor_M1, LOW);
  digitalWrite( Motor_M2, LOW);

  analogWrite( Motor_E1, motorspeed);
  analogWrite( Motor_E2, motorspeed);
}

void right(byte flag, byte motorspeed)
{
  Serial.println("Turn Right! ");

  digitalWrite( Motor_M1, HIGH);
  digitalWrite( Motor_M2, HIGH);

  analogWrite( Motor_E1, motorspeed);
  analogWrite( Motor_E2, 0);
}

void left(byte flag, byte motorspeed)
{
  Serial.println("Turn Left!");

  digitalWrite( Motor_M1, HIGH);
  digitalWrite( Motor_M2, HIGH);

  analogWrite( Motor_E1, 0);
  analogWrite( Motor_E2, motorspeed); 
}