|
#include "Servo.h" // 서보모터를 사용하기 위한 헤더파일 호출
Servo LKservo; // 서보모터 객체 선언
#define EA 3 // 모터드라이버 EA 핀, 아두이노 디지털 3번 핀에 연결
#define EB 11 // 모터드라이버 EB 핀, 아두이노 디지털 11번 핀에 연결
#define M_IN1 4 // 모터드라이버 IN1 핀, 아두이노 디지털 4번 핀에 연결
#define M_IN2 5 // 모터드라이버 IN2 핀, 아두이노 디지털 5번 핀에 연결
#define M_IN3 13 // 모터드라이버 IN3 핀, 아두이노 디지털 13번 핀에 연결
#define M_IN4 12 // 모터드라이버 IN4 핀, 아두이노 디지털 12번 핀에 연결
#define echo 6 // 초음파 센서 에코(Echo) 핀, 아두이노 우노 보드의 디지털 6번 핀 연결
#define trig 7 // 초음파 센서 트리거(Trigger) 핀, 아두이노 우노 보드의 디지털 7번 핀 연결
#define servo_motor 9 // 서보모터 Signal 핀, 아두이노 우노 보드 디지털 9번 핀에 연결
int motorA_vector = 1; // 모터의 회전방향이 반대일 시 0을 1로, 1을 0으로 바꿔주면 모터의 회전방향이 바뀜.
int motorB_vector = 1; // 모터의 회전방향이 반대일 시 0을 1로, 1을 0으로 바꿔주면 모터의 회전방향이 바뀜.
int motor_speed = 150; // 모터 스피드 0 ~ 255
void setup() // 초기화
{
Serial.begin(9600);
pinMode(EA, OUTPUT); // EA 핀 출력 설정
pinMode(EB, OUTPUT); // EB 핀 출력 설정
pinMode(M_IN1, OUTPUT); // IN1 핀 출력 설정
pinMode(M_IN2, OUTPUT); // IN2 핀 출력 설정
pinMode(M_IN3, OUTPUT); // IN3 핀 출력 설정
pinMode(M_IN4, OUTPUT); // IN4 핀 출력 설정
pinMode(trig, OUTPUT); // trig 출력 설정
pinMode(echo, INPUT); // echo 입력설정
LKservo.attach(servo_motor); // 서보모터 핀 설정
LKservo.write(110); // 서보모터 초기값 90도 설정
delay(2000); // 갑작스러운 움직임을 막기위한 3초간 지연
}
void loop()
{
Serial.println(read_ultrasonic());
if (read_ultrasonic() < 25) {
digitalWrite(EA, HIGH);
digitalWrite(M_IN1, motorA_vector);
digitalWrite(M_IN2, !motorA_vector);
digitalWrite(EB, HIGH);
digitalWrite(M_IN3, motorB_vector);
digitalWrite(M_IN4, !motorB_vector);
} else {
digitalWrite(EA, HIGH);
digitalWrite(M_IN1, motorA_vector);
digitalWrite(M_IN2, !motorA_vector);
digitalWrite(EB, HIGH);
digitalWrite(M_IN3, !motorB_vector);
digitalWrite(M_IN4, motorB_vector);
}
#include <read_ultrasonic>
}
int resd_ultrasonic(void) {
int distance;
digitalWrite(trig, HIGH);
delayMicroseconds(10);
digitalWrite(trig, LOW);
distance = pulseIn(echo, HIGH) * 17 / 1000;
return distance;
}
void motor_con(int M1, int M2)
{
if (M1 > 0) // M1이 양수일 때, 모터A 정회전
{
digitalWrite(M_IN1, motorA_vector); // IN1번에 HIGH(motorA_vector가 0이면 LOW)
digitalWrite(M_IN2, !motorA_vector); // IN2번에 LOW(motorA_vector가 0이면 HIGH)
}
else if (M1 < 0) // M1이 음수일 대, 모터A 역회전
{
digitalWrite(M_IN1, !motorA_vector); // IN1번에 LOW(motorA_vector가 0이면 HIGH)
digitalWrite(M_IN2, motorA_vector); // IN2번에 HIGH(motorA_vector가 0이면 LOW)
}
else // 모터A 정지
{
digitalWrite(M_IN1, LOW); // IN1번에 LOW
digitalWrite(M_IN2, LOW); // IN2번에 LOW
}
if (M2 > 0) // M2가 양수일 때, 모터B 정회전
{
digitalWrite(M_IN3, !motorB_vector); // IN3번에 HIGH(motorB_vector가 0이면 LOW)
digitalWrite(M_IN4, motorB_vector); // IN4번에 LOW(motorB_vector가 0이면 HIGH)
}
else if (M2 < 0) // M2가 음수일 때, 모터B 역회전
{
digitalWrite(M_IN3, motorB_vector); // IN3번에 LOW(motorB_vector가 0이면 HIGH)
digitalWrite(M_IN4, !motorB_vector); // IN4번에 HIGH(motorB_vector가 0이면 LOW)
}
else // 모터B 정지
{
digitalWrite(M_IN3, LOW); // IN3번에 LOW
digitalWrite(M_IN4, LOW); // IN4번에 LOW
}
analogWrite(EA, abs(M1));
analogWrite(EB, abs(M2)); // M2의 절대값으로 모터B 속도 제어
}
코드는 이러합니다 ㅠㅠㅠ 원인을 찾아주세요...ㅠㅠㅠㅠ
|
