Hey everyone,
I'm kind of new to Arduino so I'm still learning how everything works. My project is a autonomous quadcopter, currently I'm trying to take data from a gyro and convert it into Euler Angles. My code is at the end of the post.
The problem is that I will attach my gyro to pins A4 and A5 then open the serial monitor and none of my Serial.println commands will work. Then if I unplug my gyro everything works perfectly. Keep in mind that in the code at the end of my post I have commented out the command to extract data from the gyros and the gyro values are randomly generated numbers.
///////////////////////////////////
//Create variables for use later.//
///////////////////////////////////
float DCM[3][3]={{0,0,0},{0,0,0},{0,0,0}}; //inital matrix for DCM
float Gyro[3]={0,0,0}; //inital Gyro output {p,q,r} or {}
float timestamp_old;
float timestamp;
float pitch;
float roll;
float yaw;
///////////////
//Gyro set up//
///////////////
#include <Wire.h> //This code makes use of the wire library found at arduino.cc/en/Reference/WireAvailable
/////////////////////////////////////////////////////////////////////////////////////
//To attach the gyro to the arduino. Attach the SDA pin to A4 and the SCL pin to A5//
/////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Quick breakdown on how the 0xYY numbers work. They are addresses and values that we need to send to various places to get our results. All of the data //
//is in hexadecimal format. In the data sheet you will see that the address to CTRL_REG1 is 20h, the h just stands for hexadecimal. so the address is 20.//
//This applies to every signal value and address in this code, even when you see things like 0x2B, B is a number is the hexadecimal system. ///////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#define CTRL_REG1 0x20 //This is the address to CTRL_REG1 as defined in the specs found on the parallax website
#define CTRL_REG2 0x21 //This is the address to CTRL_REG2 as defined in the specs found on the parallax website
#define CTRL_REG3 0x22 //This is the address to CTRL_REG3 as defined in the specs found on the parallax website
#define CTRL_REG4 0x23 //This is the address to CTRL_REG4 as defined in the specs found on the parallax website
int Addr = 105; // I2C address of gyro, in the specs this is given in binary code
int x, y, z;
///////////////////////////
//Set up needed functions//
///////////////////////////
void getGyroValues () { //Here we will create the getGyroValues function. It is the most complex of the three created functions.
byte MSB, LSB;
MSB = readI2C(0x29); //here we are taking part of our x axis spin rate
LSB = readI2C(0x28); //The other part of the answer comes from 0x28
x = ((MSB << 8) | LSB); //and here we smash the two together in a xxxx.xxxx.yyyy.yyyy like format.
//WizenedEE in this forume explines it well. (forum.arduino.cc/index/php/topic,97477.0.html
MSB = readI2C(0x2B); //Same thing as above just for the y axis
LSB = readI2C(0x2A);
y = ((MSB << 8) | LSB);
MSB = readI2C(0x2D); //Again, same thing just for the z axis.
LSB = readI2C(0x2C);
z = ((MSB << 8) | LSB);
}
int readI2C (byte regAddr) { //Here we create the function readI2C
Wire.beginTransmission(Addr); //begin trasmission with the gyro
Wire.write(regAddr); // Register address to read
Wire.endTransmission(); // Terminate request
Wire.requestFrom(Addr, 1); // Read a byte
while(!Wire.available()) { }; // Wait for receipt
return(Wire.read()); // Get result
}
void writeI2C (byte regAddr, byte val) { //here we create the function writeI2C
Wire.beginTransmission(Addr); //begin trasmission with the gyro
Wire.write(regAddr); // Register address to write to
Wire.write(val); // value to send to selected register
Wire.endTransmission(); //Terminate Transmission
}
//////////////
//Void Setup//
//////////////
void setup() {
timestamp=0;
Wire.begin(Addr);
Serial.begin(9600);
writeI2C(CTRL_REG1, 0x1F); // Turn on all axes, disable power down
writeI2C(CTRL_REG3, 0x08); // Enable control ready signal
writeI2C(CTRL_REG4, 0x80); // Set scale (500 deg/sec)
delay(100); // Wait to synchronize
Serial.println("ready");
}
/////////////
//Void Loop//
/////////////
void loop() {
Serial.println("Start Loop");
//Get Gyro values
//////////////getGyroValues(); // Get new values. order: roll, pitch, yaw
//Populate Gyro vector with random numbers
//Define Gyro as (roll, pitch, yaw)
Gyro[0]=random(0,360);
Gyro[1]=random(0,360);
Gyro[2]=random(0,360);
Serial.println(Gyro[0]);
Serial.println(Gyro[1]);
Serial.println(Gyro[2]);
Serial.println("Populated Gyro");
//Determine time since last run through
timestamp_old=timestamp;
timestamp=millis(); //time since last run through in miliseconds
timestamp=timestamp*0.001; //converts from miliseconds to seconds
float Dt=timestamp-timestamp_old;
Serial.println("Time stamped");
//Construct DCM from Gyro
DCM[0][0]=0;
DCM[0][1]=-Dt*Gyro[2];
DCM[0][2]=Dt*Gyro[1];
DCM[1][0]=Dt*Gyro[2];
DCM[1][1]=0;
DCM[1][2]=-Dt*Gyro[0];
DCM[2][0]=-Dt*Gyro[1];
DCM[2][1]=Dt*Gyro[0];
DCM[2][2]=0;
Serial.println("Built DCM");
//Extract Euler Angles
pitch = -asin(DCM[2][0]);
roll = atan2(DCM[2][1],DCM[2][2]);
yaw = atan2(DCM[1][0],DCM[0][0]);
Serial.println("Found angles");
//Lets test it now
Serial.println(pitch);
Serial.println(roll);
Serial.println(yaw);
delay(10000);
}
In addition, I have this second code that extracts data from the gyros and prints them to the serial monitor. This code shows the same problem as the one above. When I have my gyro plugged into the A4 and A5 pins nothing works. Soon as I unplug them it works. I can't insert the code because it makes the post too long. But I can supply it if you wish.
Any help would be great. Thank you.
