For some reason the following equation isn't behaving as it should within my program:
gyroang_z= gyroang_z + gyro_z*(duration/1000000);
where gyroang_z and duration are stored as long and gyroz is an int.
Duration is the sampling time which has been worked out by:
now = micros();
duration=(now - last);
last= now;
where now and last are unsigned longs. So the variable duration is in microseconds, hence divided by 1,000,000 in the above equation. Gyro_z has typical values of 1-1000
None of the values i'm getting look right, I've checked the outputs of gyro_z and duration and they result in a fine answer when done manually, so I guess something must be overflowing, but how?
Edit: the entire code looks like this:
#include <Wire.h>
#define DEVICE_GYRO (0x68) //device address
#define DEVICE_ACC (0x53)
#define TO_READ (6) //num of bytes we are going to read each time (two bytes for each axis)
byte buff[TO_READ] ; //6 bytes buffer for saving data read from the device
char str[512]; //string buffer to transform data before sending it to the serial port
int gyro_x_off, gyro_y_off, gyro_z_off;
unsigned long now;
unsigned long last;
long duration;
long gyroang_z;
//Function Definitions
void gyroraw(int & x, int & y, int & z)
{
int regAddress = 0x1D; //first register
readFrom(DEVICE_GYRO, regAddress, TO_READ, buff);
x= (buff[0] << 8) | buff[1];
y= (buff[2] << 8) | buff[3];
z= (buff[4] << 8) | buff[5];
}
void accraw (int & x, int & y, int & z)
{
int regAddress = 0x32; //first register
readFrom(DEVICE_ACC, regAddress, TO_READ, buff);
x= (buff[1] << 8) | buff[0];
y= (buff[3] << 8) | buff[2];
z= (buff[5] << 8) | buff[4];
}
void writeTo(int device, byte address, byte val)
{
Wire.beginTransmission(device); //start transmission to device
Wire.write(address); // send register address
Wire.write(val); // send value to write
Wire.endTransmission(); //end transmission
} // end of writeTo
void readFrom(int device, byte address, int num, byte buff[])
{
Wire.beginTransmission(device); //start transmission to device
Wire.write(address); //sends address to read from
Wire.endTransmission(); //end transmission
Wire.requestFrom(device, num); // request 6 bytes from device
int i = 0;
while(Wire.available()) //device may send less than requested (abnormal)
{
buff[i] = Wire.read(); // receive a byte
i++;
}
} // end of readFrom
//Begin program
void setup()
{
Wire.begin(); // join i2c bus (address optional for master)
Serial.begin(9600); // start serial for output
//GYRO LAUNCH
writeTo(DEVICE_GYRO, 0x3E, 128);
writeTo(DEVICE_GYRO, 0x16, 24);
//ACC LAUNCH
writeTo(DEVICE_ACC, 0x2D, 0);
writeTo(DEVICE_ACC, 0x2D, 16);
writeTo(DEVICE_ACC, 0x2D, 8);
writeTo(DEVICE_ACC, 0x31, B00001011);
writeTo(DEVICE_ACC, 0x2C, B00001111);
readFrom(DEVICE_ACC,0x2C,1,buff);
int value;
value=(buff[0]);
Serial.print(value);
delay(1000);
//GYRO ZERO
for(int i=0;i<500;i++)
{
int x,y,z;
gyroraw(x,y,z);
gyro_x_off+=x;
gyro_y_off+=y;
gyro_z_off+=z;
delay(10);
}
gyro_x_off=abs(gyro_x_off/500);
gyro_y_off=abs(gyro_y_off/500);
gyro_z_off=abs(gyro_z_off/500);
sprintf(str, "%d, %d, %d",gyro_x_off,gyro_y_off,gyro_z_off);
Serial.write(str);
Serial.write(byte(10));
} // end of setup
void loop()
{
now = micros();
duration=(now - last);
last= now;
//Serial.print(duration);
int gyro_x, gyro_y, gyro_z;
int acc_x, acc_y, acc_z;
gyroraw(gyro_x, gyro_y, gyro_z);
accraw(acc_x, acc_y, acc_z);
gyro_x=(gyro_x-gyro_x_off)/14.375;
gyro_y=(gyro_y-gyro_y_off)/14.375;
gyro_z=(gyro_z-gyro_z_off)/14.375;
gyroang_z= gyroang_z + gyro_z*(duration/1000000);
//we send the x y z values as a string to the serial port
sprintf(str, "%d %d %d",gyro_x, gyro_y, gyro_z);
Serial.write(str);
Serial.write(byte(10));
Serial.print(duration);
Serial.write(byte(10));
Serial.print(" Z angle:");
Serial.print(gyroang_z);
Serial.write(byte(10));
//
// //It appears that delay is needed in order not to clog the port
delay(10);
} // end of loop