|
1. 프로젝트 사용한 보드 종류
(EX : 오렌지보드, 오렌지 BLE보드, 아두이노 UNO 등)
아두이노 UNO
2. 사용한 개발 프로그램명
(EX : 아두이노 IDE, 스크래치, mBlock 등)
아두이노 IDE
3. 사용한 센서 모델명
(센서 모델명을 자세하게 적어 주실 경우 더 정확하게 확인할 수 있습니다.)
mpu6050, sr04
4. 연결한 회로 설명 (또는 이미지)
5. 소스코드 (주석 필수)
(원하는 기능의 코드를 전부 작성해 드리긴 어렵습니다. 기초부터 차근차근 공부해보면서 코드를 만들어 보세요.)
#include "Wire.h"
#include "I2Cdev.h"
#include "SPI.h"
#include "MPU6050_6Axis_MotionApps20.h"
#define I2C_SLAVE_ADDR 7 // >>>>> I2C 슬레이브 주소(초음파 센서모듈 주소)
#define ULTRASONIC_NUM 3 // >>>>> 초음파 센서를 하나 더 추가할 경우 4로 변경('알칸도 초음파 센서모듈-2')
uint8_t distance[ULTRASONIC_NUM];
MPU6050 mpu; // mpu interface object
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q;
VectorFloat gravity;
float ypr[3];
float yprLast[3];
int16_t gyro[3];
volatile bool mpuInterrupt = false;
void dmpDataReady() {
mpuInterrupt = true;
}
void dmpsetup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
while (!Serial);
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(33);
mpu.setYGyroOffset(-13);
mpu.setZGyroOffset(8);
mpu.setZAccelOffset(1416);
if (devStatus == 0) {
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
else {
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
void setup() {
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
Serial.begin(115200);
dmpsetup();
}
void dmploop() {
motor_run();
if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize) {
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
mpu.resetFIFO();
// Serial.println(F("FIFO overflow!"));
} else if (mpuIntStatus & 0x02) {
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetGyro(gyro, fifoBuffer);
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
ypr[1] = (ypr[1] * 180 / M_PI); //x
ypr[2] = (ypr[2] * 180 / M_PI); //y
ypr[0] = (ypr[0] * 180 / M_PI); //z
//gyro[0],gyro[1],gyro[2]//x,y,z 각속도값
Serial.print(ypr[1]);
Serial.print("\t");
Serial.print(ypr[2]);
Serial.print("\t");
Serial.print(ypr[0]);
Serial.println("\t");
Wire.requestFrom(I2C_SLAVE_ADDR, ULTRASONIC_NUM); // 슬레이브로 부터 ULTRASONIC_NUM 바이트 데이터 요청
for (int i = 0; i < ULTRASONIC_NUM; i++) {
distance[i] = Wire.read(); //초음파 센서 값 받아오는 부분
Serial.print(distance[i]);
Serial.print(" ");
if(ypr[1] > 45 | distance[0] < 20 | distance[1] < 20 | distance[2] < 20) //자이로 센서, 초음파 센서를 이용해 모터를 제어하는 부분
{
Serial.println("aaaaaaaaaa");
motor_stop();
}
else
{
Serial.println("---------");
motor_run();
}
}
}
}
void loop() {
dmploop(); //refresh new angle datas from MPU6050
}
void motor_run()
{
//motorA
digitalWrite(5, HIGH); // Motor A 방향설정1
digitalWrite(4, LOW); // Motor A 방향설정2
analogWrite(3, 200); // Motor A 속도조절 (0~255)
//motorB
digitalWrite(8, HIGH); // Motor B 방향설정1
digitalWrite(7, LOW); // Motor B 방향설정2
analogWrite(6, 200); // Motor B 속도조절 (0~255)
}
void motor_stop()
{
//motorA
digitalWrite(5, LOW); // Motor A 방향설정1
digitalWrite(4, LOW); // Motor A 방향설정2
analogWrite(3, 50); // Motor A 속도조절 (0~255)
//motorB
digitalWrite(8, LOW); // Motor B 방향설정1
digitalWrite(7, LOW); // Motor B 방향설정2
analogWrite(6, 50); // Motor B 속도조절 (0~255)
}
6. 문제점 및 에러 내용
(소스코드 문제일 경우 에러 내용도 같이 올려주세요.)
자이로 센서와 초음파 센서 값을 이용해 모터를 제어하는 것을 목적으로 짠 소스입니다.
위 빨간색으로 표시된 if문 조건에 따라 print 값이 'aaaa' 혹은 '------' 로 바뀌긴 하나, 모터 제어가 되지 않습니다.
혹시 왜 그런지 알 수 있을까요?
|
