//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;
}