//2014.11.24 Mecanum wheel source code for Arduino Mega Board (1280, 2560)
// HC-SR04, 4CH MOTOR SHIELD FOR ARDUINO

// 전방 장애물을 감지하면 후진 0.5초, 우측이동 1초

long td1=0;
float dist1=0.;
int spd=80;   // speed (0~255)

void setup()
{
  //Motor input
  pinMode(3,OUTPUT);  //M1
  pinMode(5,OUTPUT);  //M1
   
  pinMode(6, OUTPUT); //M2
  pinMode(8, OUTPUT);  //M2
  
  pinMode(9, OUTPUT); //M3
  pinMode(10, OUTPUT); //M3
   
  pinMode(11, OUTPUT);  //M4
  pinMode(12, OUTPUT);  //M4

  //sonic
  pinMode(22,OUTPUT);  //trig1
  pinMode(24,INPUT);  //echo1

}
void loop()
{
  trig1();
  echo1();
 
  if(dist1<5)  // Detact distance_ultra sonic
  {
    backward();
    delay(500);
    right();
    delay(1000);
  }
 else
   forward();
}

void backward()
{
  analogWrite(3, 0);     // FL
  analogWrite(5, spd);    // FL           
  analogWrite(6, 0);     // BL
  analogWrite(8, spd);    // BL
         
  analogWrite(9, spd);    // BR
  analogWrite(10, 0);     // BR            
  analogWrite(11, spd);    // FR
  analogWrite(12, 0);     // FR

}

void forward()
{
  analogWrite(3, spd);      // FL
  analogWrite(5, 0);     // FL
  analogWrite(6, spd);      // BL
  analogWrite(8, 0);     // BL
         
  analogWrite(9, 0);     // BR
  analogWrite(10, spd);    // BR
  analogWrite(11, 0);     // FR
  analogWrite(12, spd);    // FR
}

void right()
{
  analogWrite(3, spd);      // FL
  analogWrite(5, 0);     // FL
  analogWrite(6, 0);      // BL
  analogWrite(8, spd);     // BL
         
  analogWrite(9, 0);   // BR
  analogWrite(10, spd);    // BR
  analogWrite(11, spd);    // FR
  analogWrite(12, 0);   // FR
}

void left()
{
  analogWrite(3, 0);      // FL
  analogWrite(5, spd);     // FL
  analogWrite(6, spd);      // BL
  analogWrite(8, 0);     // BL
         
  analogWrite(9, spd);   // BR
  analogWrite(10, 0);    // BR
  analogWrite(11, 0);    // FR
  analogWrite(12, spd);   // FR
}

void trig1()
{
  digitalWrite(22,LOW);
  delayMicroseconds(2);
  digitalWrite(22,HIGH);
  delayMicroseconds(10);
  digitalWrite(22,LOW);
}

void echo1()
{
  td1=pulseIn(24,HIGH);
  dist1=td1/58;
}