Good evening,
I am using Arduino Uno with GY-87. I have been struggling with it for couple of days now.. I changed the sensor modules, but still get the same issue: Arduino Uno restarts at random times.
I have 3.3V, GND, SCL and SDA connected.
Thank you.
#include <Wire.h> //Include the Wire.h library so we can communicate with the gyro
//Declaring Global Variables
byte lowByte, highByte, type, gyro_address, error, clockspeed_ok;
byte channel_1_assign, channel_2_assign, channel_3_assign, channel_4_assign;
byte roll_axis, pitch_axis, yaw_axis;
byte gyro_check_byte;
int address, cal_int, loop_counter;
unsigned long zero_timer, timer, timer_1, timer_2, timer_3, timer_4, current_time;
float gyro_pitch, gyro_roll, gyro_yaw, angle_pitch, angle_roll;
int acc_y, acc_x, acc_z;
float gyro_roll_cal, gyro_pitch_cal, gyro_yaw_cal;
int temperature;
void setup() {
Wire.begin(); //Start the I2C as master
Serial.begin(57600); //Start the serial connetion @ 57600bps
delay(250); //Give the gyro time to start
intro();
Serial.println(F(""));
Serial.println(F("==================================================="));
Serial.println(F("System check"));
Serial.println(F("==================================================="));
delay(1000);
Serial.println(F("Checking I2C clock speed."));
delay(1000);
TWBR = 12; //Set the I2C clock speed to 400kHz.
#if F_CPU == 16000000L //If the clock speed is 16MHz include the next code line when compiling
clockspeed_ok = 1; //Set clockspeed_ok to 1
#endif //End of if statement
if(TWBR == 12 && clockspeed_ok){
Serial.println(F("I2C clock speed is correctly set to 400kHz."));
}
else{
Serial.println(F("I2C clock speed is not set to 400kHz. (ERROR 8)"));
error = 1;
}
gyro_address = 0x68;
address = gyro_address;
delay(3000);
Serial.println(F(""));
Serial.println(F("==================================================="));
Serial.println(F("Gyro register settings"));
Serial.println(F("==================================================="));
start_gyro(); //Setup the gyro for further use
//If the gyro is found we can setup the correct gyro axes.
if(error == 0){
delay(3000);
Serial.println(F(""));
Serial.println(F("==================================================="));
Serial.println(F("Gyro calibration"));
Serial.println(F("==================================================="));
Serial.println(F("Don't move the quadcopter!! Calibration starts in 3 seconds"));
delay(3000);
Serial.println(F("Calibrating the gyro, this will take +/- 8 seconds"));
Serial.print(F("Please wait"));
//Let's take multiple gyro data samples so we can determine the average gyro offset (calibration).
for (cal_int = 0; cal_int < 2000 ; cal_int ++){ //Take 2000 readings for calibration.
if(cal_int % 100 == 0)Serial.print(F(".")); //Print dot to indicate calibration.
gyro_signalen(); //Read the gyro output.
gyro_roll_cal += gyro_roll; //Ad roll value to gyro_roll_cal.
gyro_pitch_cal += gyro_pitch; //Ad pitch value to gyro_pitch_cal.
gyro_yaw_cal += gyro_yaw; //Ad yaw value to gyro_yaw_cal.
delay(4); //Wait 3 milliseconds before the next loop.
}
//Now that we have 2000 measures, we need to devide by 2000 to get the average gyro offset.
gyro_roll_cal /= 2000; //Divide the roll total by 2000.
gyro_pitch_cal /= 2000; //Divide the pitch total by 2000.
gyro_yaw_cal /= 2000; //Divide the yaw total by 2000.
//Show the calibration results
Serial.println(F(""));
Serial.print(F("Axis 1 offset="));
Serial.println(gyro_roll_cal);
Serial.print(F("Axis 2 offset="));
Serial.println(gyro_pitch_cal);
Serial.print(F("Axis 3 offset="));
Serial.println(gyro_yaw_cal);
Serial.println(F(""));
}
Serial.println(F("==================================================="));
Serial.println(F("Gyro axes configuration"));
Serial.println(F("==================================================="));
//Detect the left wing up movement
Serial.println(F("Lift the left side of the quadcopter to a 45 degree angle within 10 seconds"));
//Check axis movement
check_gyro_axes(1);
if(error == 0){
Serial.println(F("OK!"));
Serial.print(F("Angle detection = "));
Serial.println(roll_axis & 0b00000011);
if(roll_axis & 0b10000000)Serial.println(F("Axis inverted = yes"));
else Serial.println(F("Axis inverted = no"));
Serial.println(F("Put the quadcopter back in its original position"));
waitForSerial();
//Detect the nose up movement
Serial.println(F(""));
Serial.println(F(""));
Serial.println(F("Lift the nose of the quadcopter to a 45 degree angle within 10 seconds"));
//Check axis movement
check_gyro_axes(2);
}
if(error == 0){
Serial.println(F("OK!"));
Serial.print(F("Angle detection = "));
Serial.println(pitch_axis & 0b00000011);
if(pitch_axis & 0b10000000)Serial.println(F("Axis inverted = yes"));
else Serial.println(F("Axis inverted = no"));
Serial.println(F("Put the quadcopter back in its original position"));
waitForSerial();
//Detect the nose right movement
Serial.println(F(""));
Serial.println(F(""));
Serial.println(F("Rotate the nose of the quadcopter 45 degree to the right within 10 seconds"));
//Check axis movement
check_gyro_axes(3);
}
if(error == 0){
Serial.println(F("OK!"));
Serial.print(F("Angle detection = "));
Serial.println(yaw_axis & 0b00000011);
if(yaw_axis & 0b10000000)Serial.println(F("Axis inverted = yes"));
else Serial.println(F("Axis inverted = no"));
Serial.println(F("Put the quadcopter back in its original position"));
waitForSerial();
}
Serial.println(F(""));
if(error == 0){
Serial.println(F("==================================================="));
Serial.println(F("Final setup check"));
Serial.println(F("==================================================="));
delay(1000);
if(gyro_check_byte == 0b00000111){
Serial.println(F("Gyro axes ok"));
}
else{
Serial.println(F("Gyro exes verification failed!!! (ERROR 7)"));
error = 1;
}
}
Serial.println(F("Gyro set up complete!"));
Serial.println(F("==================================================="));
}
void loop() {
zero_timer = micros();
while(1){
while(zero_timer + 4000 > micros()); //Start the pulse after 4000 micro seconds.
zero_timer = micros();
//Let's get the current gyro data.
gyro_signalen();
//Gyro angle calculations
//0.0000611 = 1 / (250Hz / 65.5)
angle_pitch += gyro_pitch * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable.
angle_roll += gyro_roll * 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
float tmp = angle_pitch - angle_roll * sin(gyro_yaw * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel.
angle_roll += angle_pitch * sin(gyro_yaw * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel.
angle_pitch = tmp;
//We can't print all the data at once. This takes to long and the angular readings will be off.
if(loop_counter == 0)Serial.print("Pitch: ");
if(loop_counter == 1)Serial.print(angle_pitch);
if(loop_counter == 2)Serial.print(" Roll: ");
if(loop_counter == 3)Serial.print(angle_roll);
if(loop_counter == 4)Serial.print(" Yaw: ");
if(loop_counter == 5)Serial.println(gyro_yaw / 65.5);
if(loop_counter == 6)Serial.print(" Temperature: ");
if(loop_counter == 7)Serial.println(temperature / 340.00 + 36.53);
if(loop_counter == 8)Serial.print(" Acc x: ");
if(loop_counter == 9)Serial.println(acc_x);
if(loop_counter == 10)Serial.print(" Acc y: ");
if(loop_counter == 11)Serial.println(acc_y);
if(loop_counter == 12)Serial.print(" Acc z: ");
if(loop_counter == 13)Serial.println(acc_z);
loop_counter ++;
if(loop_counter == 60)loop_counter = 0;
}
}
//Intro subroutine
void intro(){
Serial.println(F("==================================================="));
delay(1500);
Serial.println(F(""));
Serial.println(F("MPU6500 set up"));
delay(1000);
Serial.println(F(""));
Serial.println(F("Jasma"));
Serial.println(F(""));
Serial.println(F("==================================================="));
delay(1500);
Serial.println(F("Have fun!"));
}
void start_gyro(){
//Setup the MPU-6500
Wire.beginTransmission(address); //Start communication with the gyro
Wire.write(0x6B); //PWR_MGMT_1 register
Wire.write(0x00); //Set to zero to turn on the gyro
Wire.endTransmission(); //End the transmission
Wire.beginTransmission(address); //Start communication with the gyro
Wire.write(0x6B); //Start reading @ register 28h and auto increment with every read
Wire.endTransmission(); //End the transmission
Wire.requestFrom(address, 1); //Request 1 bytes from the gyro
while(Wire.available() < 1); //Wait until the 1 byte is received
Serial.print(F("Register 0x6B is set to:"));
Serial.println(Wire.read(),BIN);
Wire.beginTransmission(address); //Start communication with the gyro
Wire.write(0x1B); //GYRO_CONFIG register
Wire.write(0x08); //Set the register bits as 00001000 (500dps full scale)
Wire.endTransmission(); //End the transmission
Wire.beginTransmission(address); //Start communication with the gyro (adress 1101001)
Wire.write(0x1B); //Start reading @ register 28h and auto increment with every read
Wire.endTransmission(); //End the transmission
Wire.requestFrom(address, 1); //Request 1 bytes from the gyro
while(Wire.available() < 1); //Wait until the 1 byte is received
Serial.print(F("Register 0x1B is set to:"));
Serial.println(Wire.read(),BIN);
Wire.beginTransmission(address); //Start communication with the address found during search.
Wire.write(0x1C); //We want to write to the ACCEL_CONFIG register (1A hex)
Wire.write(0x10); //Set the register bits as 00010000 (+/- 8g full scale range)
Wire.endTransmission(); //End the transmission with the gyro
Wire.beginTransmission(address); //Start communication with the gyro (adress 1101001)
Wire.write(0x1C); //Start reading @ register 28h and auto increment with every read
Wire.endTransmission(); //End the transmission
Wire.requestFrom(address, 1); //Request 1 bytes from the gyro
while(Wire.available() < 1); //Wait until the 1 byte is received
Serial.print(F("Register 0x1C is set to:"));
Serial.println(Wire.read(),BIN);
}
void gyro_signalen(){
Wire.beginTransmission(address); //Start communication with the gyro
Wire.write(0x3b); //Start reading @ register 43h and auto increment with every read
Wire.endTransmission(); //End the transmission
Wire.requestFrom(address,14); //Request 6 bytes from the gyro
while(Wire.available() < 14); //Wait until the 6 bytes are received
acc_x = Wire.read()<<8|Wire.read();
acc_y = Wire.read()<<8|Wire.read();
acc_z = Wire.read()<<8|Wire.read();
temperature = Wire.read()<<8|Wire.read(); //Add the low and high byte to the temperature variable
gyro_roll=Wire.read()<<8|Wire.read(); //Read high and low part of the angular data
if(cal_int == 2000)gyro_roll -= gyro_roll_cal; //Only compensate after the calibration
gyro_pitch=Wire.read()<<8|Wire.read(); //Read high and low part of the angular data
if(cal_int == 2000)gyro_pitch -= gyro_pitch_cal; //Only compensate after the calibration
gyro_yaw=Wire.read()<<8|Wire.read(); //Read high and low part of the angular data
if(cal_int == 2000)gyro_yaw -= gyro_yaw_cal; //Only compensate after the calibration
}
//Check if the angular position of a gyro axis is changing within 10 seconds
void check_gyro_axes(byte movement){
byte trigger_axis = 0;
float gyro_angle_roll, gyro_angle_pitch, gyro_angle_yaw;
//Reset all axes
gyro_angle_roll = 0;
gyro_angle_pitch = 0;
gyro_angle_yaw = 0;
gyro_signalen();
timer = millis() + 10000;
while(timer > millis() && gyro_angle_roll > -30 && gyro_angle_roll < 30 && gyro_angle_pitch > -30 && gyro_angle_pitch < 30 && gyro_angle_yaw > -30 && gyro_angle_yaw < 30){
gyro_signalen();
gyro_angle_roll += gyro_roll * 0.0000611; // 0.0000611 = 1 / 65.5 (LSB degr/s) / 250(Hz)
gyro_angle_pitch += gyro_pitch * 0.0000611;
gyro_angle_yaw += gyro_yaw * 0.0000611;
delayMicroseconds(3700); //Loop is running @ 250Hz. +/-300us is used for communication with the gyro
}
//Assign the moved axis to the orresponding function (pitch, roll, yaw)
if((gyro_angle_roll < -30 || gyro_angle_roll > 30) && gyro_angle_pitch > -30 && gyro_angle_pitch < 30 && gyro_angle_yaw > -30 && gyro_angle_yaw < 30){
gyro_check_byte |= 0b00000001;
if(gyro_angle_roll < 0)trigger_axis = 0b10000001;
else trigger_axis = 0b00000001;
}
if((gyro_angle_pitch < -30 || gyro_angle_pitch > 30) && gyro_angle_roll > -30 && gyro_angle_roll < 30 && gyro_angle_yaw > -30 && gyro_angle_yaw < 30){
gyro_check_byte |= 0b00000010;
if(gyro_angle_pitch < 0)trigger_axis = 0b10000010;
else trigger_axis = 0b00000010;
}
if((gyro_angle_yaw < -30 || gyro_angle_yaw > 30) && gyro_angle_roll > -30 && gyro_angle_roll < 30 && gyro_angle_pitch > -30 && gyro_angle_pitch < 30){
gyro_check_byte |= 0b00000100;
if(gyro_angle_yaw < 0)trigger_axis = 0b10000011;
else trigger_axis = 0b00000011;
}
if(trigger_axis == 0){
error = 1;
Serial.println(F("No angular motion is detected in the last 10 seconds!!! (ERROR 4)"));
}
else
if(movement == 1)roll_axis = trigger_axis;
if(movement == 2)pitch_axis = trigger_axis;
if(movement == 3)yaw_axis = trigger_axis;
}
void waitForSerial(){
Serial.println(F("Enter any symbol"));
while(Serial.available()){Serial.read();}
while (!Serial.available()) { }
Serial.read();
//Serial.println(Serial.read());
}
