// Sensor sequence: SLeftLeft SMiddle SRightRight
#include "Ultrasonic.h"
Ultrasonic ultrasonic(12,13);  //Trige,Echo
int ulval;

const int SLeftLeft = 5;      //左感測器輸入腳
const int SMiddle = 6;     //中間感測器輸入腳
const int SRightRight = 7;     //右感測器輸入腳

// variables will change:

int SLL;    //左感測器狀態
int SM;    //中感測器狀態
int SRR;    //右感測器狀態

const int Motor_M1a = 13;    //R
const int Motor_M1b = 12;
const int Motor_E1 = 10;
const int Motor_M2a = 11;   //L
const int Motor_M2b = 8;  
const int Motor_E2 = 9;

byte byteSensorStatus=0;
int speed=220;

#define SENSOR_L 4;
#define SENSOR_M 2;
#define SENSOR_R 1;

void setup() {

   //set up serial communications

   Serial.begin(9600);

  // 輸出入接腳初始設定

  pinMode(SLeftLeft, INPUT);

  pinMode(SMiddle, INPUT);

  pinMode(SRightRight, INPUT);
  pinMode(Motor_M1a, OUTPUT);
  pinMode(Motor_M1b, OUTPUT);
  pinMode(Motor_M2a, OUTPUT);
  pinMode(Motor_M2b, OUTPUT);
  pinMode(Motor_E1,OUTPUT);
  pinMode(Motor_E2,OUTPUT);
}



void loop(){

 looptest();
 delay(3000);
/*
  ulval=ultrasonic.Ranging(CM);
   if (ulval < 10){
  forward(0,255);
  delay(2000);
  back(0,255);
  delay(2000);
  right(0,255);
  delay(2000);
  left(0,255);
  delay(2000);
  motorstop(0,0);
  delay(3000);

 
    }
*/



}

void looptest(){

  forward(0,255);

  delay(2000);

  back(0,255);

  delay(2000);

  right(0,255);

  delay(2000);

  left(0,255);

  delay(2000);

  motorstop(0,0);

  delay(3000);

}

void motorstop(byte flag, byte numOfValues)

{

  digitalWrite( Motor_E1, 0);

  digitalWrite( Motor_E2, 0);

//  Serial.println("stop : ");

}

void forward(byte flag, byte numOfValues)

{

  digitalWrite( Motor_M1a, LOW);
  digitalWrite( Motor_M1b, HIGH);

  digitalWrite( Motor_M2a, HIGH);
  digitalWrite( Motor_M2b, LOW);

  analogWrite( Motor_E1, numOfValues);

  analogWrite( Motor_E2, numOfValues);

}

void back(byte flag, byte numOfValues)

{

 digitalWrite( Motor_M1a, HIGH);
  digitalWrite( Motor_M1b, LOW);

  digitalWrite( Motor_M2a, LOW);
  digitalWrite( Motor_M2b, HIGH);


  analogWrite( Motor_E1, numOfValues);

  analogWrite( Motor_E2, numOfValues);

}

void right(byte flag, byte numOfValues)

{

  digitalWrite( Motor_M1a, HIGH);
  digitalWrite( Motor_M1b, LOW);

  digitalWrite( Motor_M2a, HIGH);
  digitalWrite( Motor_M2b, LOW);


  analogWrite( Motor_E1, numOfValues);

  analogWrite( Motor_E2, numOfValues);

}

void left(byte flag, byte numOfValues)

{

  digitalWrite( Motor_M1a, LOW);
  digitalWrite( Motor_M1b, HIGH);

  digitalWrite( Motor_M2a, LOW);
  digitalWrite( Motor_M2b, HIGH);
 
 analogWrite( Motor_E1, numOfValues);

  analogWrite( Motor_E2, numOfValues);

}