초음파 센서를 이용한 자동차 충돌 방지 소스코드 - 속도 컨트롤 추가
속도 컨트롤을 하기 위해서는 PIN OUT이 PWM을 지원해 줘야 한다.
그래서 아래와 같이 회로를 바꾸고 모터 컨트롤을 수정하였다.
#define echoPin 7 // Echo Pin
#define trigPin 8 // Trigger Pin
#define SPEED 80 // you can use from 0 ~ 256
#define DETECT_DIST 15
//we can use below port number, becuase they can support PWM signal.
#define Motor_right_0 3
#define Motor_right_1 5
#define Motor_left_0 6
#define Motor_left_1 9
//int maximumRange = 200; // Maximum range needed
int minimumRange = 0; // Minimum range needed
long duration; // Duration used to calculate distance
enum direction{
RIGHT = 0,
STRAIGHT,
LEFT,
STOP,
BACK,
};
// motor
//3,5,6,9 for speed control(use PWM)
void setup()
{
pinMode(Motor_right_0, OUTPUT);
pinMode(Motor_right_1, OUTPUT);
pinMode(Motor_left_0, OUTPUT);
pinMode(Motor_left_1, OUTPUT);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(13, OUTPUT);
Serial.begin(9600);
}
void turn(int d)
{
if(d == RIGHT)
{
analogWrite(Motor_right_0, SPEED);
digitalWrite(Motor_right_1, LOW);
analogWrite(Motor_left_0, SPEED);
digitalWrite(Motor_left_1, LOW);
}
else if ( d == LEFT)
{
analogWrite(Motor_right_1, SPEED);
digitalWrite(Motor_right_0, LOW);
analogWrite(Motor_left_1, SPEED);
digitalWrite(Motor_left_0, LOW);
}
else if (d == STRAIGHT)
{ //for STRAIGHT
analogWrite(Motor_right_1, SPEED);
digitalWrite(Motor_right_0, LOW);
analogWrite(Motor_left_0, SPEED);
digitalWrite(Motor_left_1, LOW);
}else if (d == BACK)
{ //for STRAIGHT
analogWrite(Motor_right_0, SPEED);
digitalWrite(Motor_right_1, LOW);
analogWrite(Motor_left_1, SPEED);
digitalWrite(Motor_left_0, LOW);
}
else
{ //for STOP
digitalWrite(Motor_right_1, LOW);
digitalWrite(Motor_right_0, LOW);
digitalWrite(Motor_left_0, LOW);
digitalWrite(Motor_left_1, LOW);
}
}
long check_distance(void)
{
long distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = duration/58.2;
Serial.println(distance);
| // if (distance >= maximumRange | distance <= minimumRange) |
if (distance <= minimumRange)
return -1;
else
return distance;
}
void loop()
{
long check_dist;
// Serial.println(check_distance());
check_dist = check_distance();
if(check_dist >DETECT_DIST)
{
digitalWrite(13, HIGH);
Serial.println(“straight”);
turn(STRAIGHT);
}
else if(check_dist >= 0 && check_dist <= DETECT_DIST)
{
digitalWrite(13, HIGH);
Serial.println(“turn left”);
turn(LEFT);
} else
{
digitalWrite(13, LOW);
Serial.println(“BACK”);
turn(BACK);
}
delay(100);
}
\