|
아두이노와 거리 측정 센서와 자이로 센서를 이용하여
측정 거리와 회전 각도를 출력하는 기기를 제작하는 프로젝트를 하고있습니다.
아래의 두 코드와 같이 SRF01(초음파 센서)과 MPU6050(자이로 센서)을 따로 컴파일 했을 때는 각자 센싱값의 출력이 빠르게 잘 나옵니다.
SRF01 arduino code
#include <SoftwareSerial.h>
#define SRF_TXRX 0x05 // Defines pin 5 to be used as RX and TX for SRF01
#define SRF_ADDRESS 0x01 // Address of the SFR01
#define GETSOFT 0x5D // Byte to tell SRF01 we wish to read software version
#define GETRANGE 0x54 // Byte used to get range from SRF01
#define GETSTATUS 0x5F // Byte used to get the status of the transducer
SoftwareSerial srf01 = SoftwareSerial(SRF_TXRX, SRF_TXRX); // Sets up software serial port for the SRF01
void setup(){
srf01.begin(9600);
Serial.begin(115200);
srf01.listen(); // Make sure that the SRF01 software serial port is listening for data as only one software serial port can listen at a time
delay(200); // Waits some time to make sure everything is powered up
Serial.println("SRF01 Example");
byte softVer;
SRF01_Cmd(SRF_ADDRESS, GETSOFT); // Request the SRF01 software version
while (srf01.available() < 1);
softVer = srf01.read(); // Read software version from SRF01
}
void loop(){
byte hByte, lByte, statusByte, b1, b2, b3;
SRF01_Cmd(SRF_ADDRESS, GETRANGE); // Get the SRF01 to perform a ranging and send the data back to the arduino
while (srf01.available() < 2);
hByte = srf01.read(); // Get high byte
lByte = srf01.read(); // Get low byte
int range = ((hByte<<8)+lByte); // Put them together
Serial.println("Range = ");
Serial.println(range, DEC); // Print range result to the screen
Serial.println(" "); // Print some spaces to the screen to make sure space direcly after the result is clear
SRF01_Cmd(SRF_ADDRESS, GETSTATUS); // Request byte that will tell us if the transducer is locked or unlocked
while (srf01.available() < 1);
statusByte = srf01.read(); // Reads the SRF01 status, The least significant bit tells us if it is locked or unlocked
int newStatus = statusByte & 0x01; // Get status of lease significan bit
if(newStatus == 0){
Serial.println("Unlocked"); // Prints the word unlocked followd by a couple of spaces to make sure space after has nothing in
}
else {
Serial.println("Locked "); // Prints the word locked followd by a couple of spaces to make sure that the space after has nothing in
}
delay(100);
}
void SRF01_Cmd(byte Address, byte cmd){ // Function to send commands to the SRF01
pinMode(SRF_TXRX, OUTPUT);
digitalWrite(SRF_TXRX, LOW); // Send a 2ms break to begin communications with the SRF01
delay(2);
digitalWrite(SRF_TXRX, HIGH);
delay(1);
srf01.write(Address); // Send the address of the SRF01
srf01.write(cmd); // Send commnd byte to SRF01
pinMode(SRF_TXRX, INPUT);
int availbleJunk = srf01.available(); // As RX and TX are the same pin it will have recieved the data we just sent out, as we dont want this we read it back and ignore it as junk before waiting for useful data to arrive
for(int x = 0; x < availbleJunk; x++) byte junk = srf01.read();
}
MPU6050 angle calculation(pitch, roll)
///////////////////////////////////////////////////////////////////////////////////////
//THIS IS A DEMO SOFTWARE JUST FOR EXPERIMENT PURPOER IN A NONCOMERTIAL ACTIVITY
//Version: 1.0 (AUG, 2016)
//Gyro - Arduino UNO R3
//VCC - 5V
//GND - GND
//SDA - A4
//SCL - A5
//INT - port-2
#include <Wire.h>
//Declaring some global variables
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;
long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;
long loop_timer;
int temp;
void setup() {
Wire.begin(); //Start I2C as master
setup_mpu_6050_registers(); //Setup the registers of the MPU-6050
for (int cal_int = 0; cal_int < 1000 ; cal_int ++){ //Read the raw acc and gyro data from the MPU-6050 for 1000 times
read_mpu_6050_data();
gyro_x_cal += gyro_x; //Add the gyro x offset to the gyro_x_cal variable
gyro_y_cal += gyro_y; //Add the gyro y offset to the gyro_y_cal variable
gyro_z_cal += gyro_z; //Add the gyro z offset to the gyro_z_cal variable
delay(3); //Delay 3us to have 250Hz for-loop
}
// divide by 1000 to get avarage offset
gyro_x_cal /= 1000;
gyro_y_cal /= 1000;
gyro_z_cal /= 1000;
Serial.begin(115200);
loop_timer = micros(); //Reset the loop timer
}
void loop(){
read_mpu_6050_data();
//Subtract the offset values from the raw gyro values
gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;
//Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
angle_pitch += gyro_x * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_roll += gyro_y * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll
if(set_gyro_angles){ //If the IMU is already started
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle
}
else{ //At first start
angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle
angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle
set_gyro_angles = true; //Set the IMU started flag
}
//To dampen the pitch and roll angles a complementary filter is used
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value
Serial.print(" | Angle = "); Serial.print(angle_pitch_output); Serial.print(" | Angle = "); Serial.println(angle_roll_output);
while(micros() - loop_timer < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
loop_timer = micros();//Reset the loop timer
}
void setup_mpu_6050_registers(){
//Activate the MPU-6050
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x6B); //Send the requested starting register
Wire.write(0x00); //Set the requested starting register
Wire.endTransmission();
//Configure the accelerometer (+/-8g)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1C); //Send the requested starting register
Wire.write(0x10); //Set the requested starting register
Wire.endTransmission();
//Configure the gyro (500dps full scale)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1B); //Send the requested starting register
Wire.write(0x08); //Set the requested starting register
Wire.endTransmission();
}
void read_mpu_6050_data(){ //Subroutine for reading the raw gyro and accelerometer data
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x3B); //Send the requested starting register
Wire.endTransmission(); //End the transmission
Wire.requestFrom(0x68,14); //Request 14 bytes from the MPU-6050
while(Wire.available() < 14); //Wait until all the bytes are received
acc_x = Wire.read()<<8|Wire.read();
acc_y = Wire.read()<<8|Wire.read();
acc_z = Wire.read()<<8|Wire.read();
temp = Wire.read()<<8|Wire.read();
gyro_x = Wire.read()<<8|Wire.read();
gyro_y = Wire.read()<<8|Wire.read();
gyro_z = Wire.read()<<8|Wire.read();
}
문제는 두 코드를 하나로 합쳐보니 SRF01의 센싱값은 출력이 잘 되는데 MPU6050의 센싱값이 엄청 느리게 바뀝니다.
#include <SoftwareSerial.h>
#include <Wire.h>
#define SRF_TXRX 0x05 // Defines pin 5 to be used as RX and TX for SRF01
#define SRF_ADDRESS 0x01 // Address of the SFR01
#define GETSOFT 0x5D // Byte to tell SRF01 we wish to read software version
#define GETRANGE 0x54 // Byte used to get range from SRF01
#define GETSTATUS 0x5F // Byte used to get the status of the transducer
SoftwareSerial srf01 = SoftwareSerial(SRF_TXRX, SRF_TXRX); // Sets up software serial port for the SRF01
//Declaring some global variables
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;
long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;
long loop_timer;
int temp;
unsigned long ms = 0;
unsigned long prev_ms;
void setup() {
Wire.begin(); //Start I2C as master
setup_mpu_6050_registers(); //Setup the registers of the MPU-6050
for (int cal_int = 0; cal_int < 1000 ; cal_int ++){ //Read the raw acc and gyro data from the MPU-6050 for 1000 times
read_mpu_6050_data();
gyro_x_cal += gyro_x; //Add the gyro x offset to the gyro_x_cal variable
gyro_y_cal += gyro_y; //Add the gyro y offset to the gyro_y_cal variable
gyro_z_cal += gyro_z; //Add the gyro z offset to the gyro_z_cal variable
delay(3); //Delay 3us to have 250Hz for-loop
}
// divide by 1000 to get avarage offset
gyro_x_cal /= 1000;
gyro_y_cal /= 1000;
gyro_z_cal /= 1000;
srf01.begin(9600);
Serial.begin(115200);
srf01.listen(); // Make sure that the SRF01 software serial port is listening for data as only one software serial port can listen at a time
delay(200); // Waits some time to make sure everything is powered up
}
void loop(){
byte hByte, lByte, statusByte, b1, b2, b3;
SRF01_Cmd(SRF_ADDRESS, GETRANGE); // Get the SRF01 to perform a ranging and send the data back to the arduino
while (srf01.available() < 2);
hByte = srf01.read(); // Get high byte
lByte = srf01.read(); // Get low byte
int range = ((hByte<<8)+lByte); // Put them together
Serial.println("Range = ");
Serial.println(range, DEC); // Print range result to the screen
Serial.println(" "); // Print some spaces to the screen to make sure space direcly after the result is clear
read_mpu_6050_data();
//Subtract the offset values from the raw gyro values
gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;
//Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
angle_pitch += gyro_x * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_roll += gyro_y * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll
if(set_gyro_angles){ //If the IMU is already started
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle
}
else{ //At first start
angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle
angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle
set_gyro_angles = true; //Set the IMU started flag
}
//To dampen the pitch and roll angles a complementary filter is used
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value
Serial.print(" | Angle = "); Serial.print(angle_pitch_output); Serial.print(" | Angle = "); Serial.println(angle_roll_output);
while(micros() - loop_timer < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
loop_timer = micros();//Reset the loop timer
}
void setup_mpu_6050_registers(){
//Activate the MPU-6050
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x6B); //Send the requested starting register
Wire.write(0x00); //Set the requested starting register
Wire.endTransmission();
//Configure the accelerometer (+/-8g)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1C); //Send the requested starting register
Wire.write(0x10); //Set the requested starting register
Wire.endTransmission();
//Configure the gyro (500dps full scale)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1B); //Send the requested starting register
Wire.write(0x08); //Set the requested starting register
Wire.endTransmission();
}
void read_mpu_6050_data(){ //Subroutine for reading the raw gyro and accelerometer data
unsigned long ms = 0;
unsigned long prev_ms;
ms = millis();
if(ms - prev_ms >= 1000){
if (ms - prev_ms < 2000 ) prev_ms = ms - (ms - prev_ms - 1000); // prev_ms += 1000;
else prev_ms = ms;
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x3B); //Send the requested starting register
Wire.endTransmission(); //End the transmission
Wire.requestFrom(0x68,14); //Request 14 bytes from the MPU-6050
while(Wire.available() < 14); //Wait until all the bytes are received
acc_x = Wire.read()<<8|Wire.read();
acc_y = Wire.read()<<8|Wire.read();
acc_z = Wire.read()<<8|Wire.read();
temp = Wire.read()<<8|Wire.read();
gyro_x = Wire.read()<<8|Wire.read();
gyro_y = Wire.read()<<8|Wire.read();
gyro_z = Wire.read()<<8|Wire.read();
}
}
void SRF01_Cmd(byte Address, byte cmd){ // Function to send commands to the SRF01
pinMode(SRF_TXRX, OUTPUT);
digitalWrite(SRF_TXRX, LOW); // Send a 2ms break to begin communications with the SRF01
delay(2);
digitalWrite(SRF_TXRX, HIGH);
delay(1);
srf01.write(Address); // Send the address of the SRF01
srf01.write(cmd); // Send commnd byte to SRF01
pinMode(SRF_TXRX, INPUT);
int availbleJunk = srf01.available(); // As RX and TX are the same pin it will have recieved the data we just sent out, as we dont want this we read it back and ignore it as junk before waiting for useful data to arrive
for(int x = 0; x < availbleJunk; x++) byte junk = srf01.read();
}
이 문제에 대해서 도움을 받을 수 있을까요?
|
