Here is the code for the accelerometer coded alone:
#include <Wire.h>
unsigned long currentMillis;
short x,y,z;
float g_constant2 = 0.00241102; // if accel set to 2G
float g_constant4 = 0.00482204; // if accel set to 4G
float g_constant8 = 0.00964408; // if accel set to 8G
float gx,gy,gz,grav;
int period = 199;
unsigned long time_now = 0;
void setup() {
Wire.begin(4,5);
Serial.begin(115200);
Wire.beginTransmission(0x1c); // put in standby mode
Wire.write(0x2a);
Wire.write(0x10);
Wire.endTransmission();
Wire.beginTransmission(0x1c); // set scale to 8G
Wire.write(0x0e);
Wire.write(0x02); // 0x00 for 2G, 0x01 for 4G, 0x02 for 8G
Wire.endTransmission();
Wire.beginTransmission(0x1c); // put in active mode
Wire.write(0x2a);
Wire.write(0x11);
Wire.endTransmission();
}
void get_accel() {
currentMillis = millis();
int reg[6];
int i;
Wire.beginTransmission(0x1c);
Wire.write(0x01);
Wire.endTransmission(false);
Wire.requestFrom(0x1c,6);
// while(Wire.available() < 6) {
// delay(1);
// }
for(i=0;i<6;i++) reg[i] = Wire.read();
x = (reg[0] << 8) | reg[1];
y = (reg[2] << 8) | reg[3];
z = (reg[4] << 8) | reg[5];
x = x >> 2;
y = y >> 2;
z = z >> 2;
gx = x*g_constant8;
gy = y*g_constant8;
gz = z*g_constant8;
grav = sqrt(sq(gx)+sq(gy)+sq(gz));
Serial.print(gx,2); Serial.print(" ");
Serial.print(gy,2); Serial.print(" ");
Serial.print(gz,2); Serial.print(" ");
Serial.println(grav,2); Serial.print(" ");
}
void loop() {
if(millis() > time_now + period){
time_now = millis();
get_accel();
}
}
Here is the result I get:
-0.41 0.41 9.79 9.81
-0.35 0.42 9.80 9.81
-0.39 0.44 9.77 9.79
-0.38 0.42 9.80 9.81
-0.41 0.37 9.82 9.83
-0.35 0.37 9.83 9.84
-0.35 0.39 9.83 9.84
-0.39 0.42 9.78 9.80
-0.36 0.39 9.81 9.82
-0.37 0.43 9.80 9.81
-0.39 0.41 9.81 9.82
-0.37 0.43 9.73 9.75
.....continued
Here is the code for the everything together including magnetometer, accelerometer, GPS, and SD card with the GPS function in void loop commented out:
#include "NMEAGPS.h"
#include "SoftwareSerial.h"
#include <Wire.h>
#include <SPI.h>
#include <SD.h>
File fd;
SoftwareSerial serial_connection(0, 4); //TX_GPS to D0, RX_GPS to D4
NMEAGPS gps;
const unsigned char ubxRate1Hz[] PROGMEM =
{ 0x06,0x08,0x06,0x00,0xE8,0x03,0x01,0x00,0x01,0x00 };
const unsigned char ubxRate5Hz[] PROGMEM =
{ 0x06,0x08,0x06,0x00,200,0x00,0x01,0x00,0x01,0x00 };
const unsigned char ubxRate10Hz[] PROGMEM =
{ 0x06,0x08,0x06,0x00,100,0x00,0x01,0x00,0x01,0x00 };
const char baud38400 [] PROGMEM = "PUBX,41,1,3,3,38400,0";
int runoncevar = 0;
int maxX, maxY, maxZ;
int minX, minY, minZ;
int offX, offY, offZ;
short a,b,c;
unsigned long currentMillis;
short x,y,z;
float g_constant2 = 0.00241102; // if accel set to 2G
float g_constant4 = 0.00482204; // if accel set to 4G
float g_constant8 = 0.00964408; // if accel set to 8G
float gx,gy,gz,grav,an;
int period = 199;
unsigned long time_now = 0;
void setup()
{
Wire.begin(4,5);
Serial.begin(115200);
// if(!SD.begin(2)) {
// Serial.println("Error init");
// return;
// }
// SD.remove("data.csv");
//
// fd = SD.open("data.csv",FILE_WRITE);
// if(fd) {
// fd.println("Time (ms),X_accel (m/s2),Y_accel (m/s2),Z_accel (m/s2),Mag_accel (m/s2),Direction (degrees)");
// fd.close(); } else Serial.println("Error opening");
serial_connection.begin(9600);
gps.send_P( &serial_connection, (const __FlashStringHelper *) baud38400 );
serial_connection.flush(); // wait for the command to go out
delay( 100 ); // wait for the GPS device to change speeds
serial_connection.end(); // empty the input buffer, too
serial_connection.begin( 38400 ); // use the new baud rate
sendUBX( ubxRate5Hz, sizeof(ubxRate5Hz) ); // change both "ubxRate5Hz" to "ubxRate1Hz" or ubxRate10Hz" for different rates
Serial.println( F("GPS Start") ); // F macro saves RAM
Wire.beginTransmission(0x1c); // put in standby mode
Wire.write(0x2a);
Wire.write(0x10);
Wire.endTransmission();
Wire.beginTransmission(0x1c); // set scale to 8G
Wire.write(0x0e);
Wire.write(0x02); // 0x00 for 2G, 0x01 for 4G, 0x02 for 8G
Wire.endTransmission();
Wire.beginTransmission(0x1c); // put in active mode
Wire.write(0x2a);
Wire.write(0x11);
Wire.endTransmission();
Wire.beginTransmission(0x0d);
Wire.write(0x09);
Wire.write(0x05);
Wire.write(0x00);
Wire.write(0x01);
Wire.endTransmission();
maxX = -32767; maxY = -32767; maxZ = -32767;
minX = 32767; minY = 32767; minZ = 32767;
offX = 0; offY = 0; offZ = 0;
}
void get_accel() {
currentMillis = millis();
int reg[6];
int i;
Wire.beginTransmission(0x1c);
Wire.write(0x01);
Wire.endTransmission(false);
Wire.requestFrom(0x1c,6);
for(i=0;i<6;i++) reg[i] = Wire.read();
x = (reg[0] << 8) | reg[1];
y = (reg[2] << 8) | reg[3];
z = (reg[4] << 8) | reg[5];
x = x >> 2;
y = y >> 2;
z = z >> 2;
gx = x*g_constant8;
gy = y*g_constant8;
gz = z*g_constant8;
grav = sqrt(sq(gx)+sq(gy)+sq(gz));
if(millis() > time_now + period){
time_now = millis();
Serial.print(gx,2); Serial.print(" ");
Serial.print(gy,2); Serial.print(" ");
Serial.print(gz,2); Serial.print(" ");
Serial.println(grav,2); Serial.print(" "); }
}
void get_mag() {
int reg[6];
int k;
Wire.beginTransmission(0x0d);
Wire.write(0x00);
Wire.endTransmission(false);
Wire.requestFrom(0x0d,6);
for(k=0;k<6;k++) reg[k] = Wire.read();
a = (reg[1] << 8) | reg[0];
b = (reg[3] << 8) | reg[2];
c = (reg[5] << 8) | reg[4];
if (a > maxX) maxX = (int)a;
if (a < minX) minX = (int)a;
if (b > maxY) maxY = (int)b;
if (b < minY) minY = (int)b;
if (c > maxZ) maxZ = (int)c;
if (c < minZ) minZ = (int)c;
offX = (maxX + minX) / 2;
offY = (maxY + minY) / 2;
offZ = (maxZ + minZ) / 2;
int fx = (int)a - offX;
int fy = (int)b - offY;
int fz = (int)c - offZ;
an = atan2(fy,fz);
an += 0.19; //declination angle based in Annapolis, MD
if(an < 0)
an += 2*PI;
if(an > 2*PI)
an -= 2*PI;
an = an * 180.0 / PI;
Serial.println(an);
}
void write_sd() {
fd = SD.open("data.csv",FILE_WRITE);
if(fd) {
fd.print(currentMillis); fd.print(",");
fd.print(gx,2); fd.print(","); fd.print(gy,2); fd.print(","); fd.print(gz,2); fd.print(",");
fd.print(grav,2); fd.print(",");
fd.println(an);
fd.close(); } else Serial.println("Error opening");
}
void loop()
{
// if (gps.available( serial_connection ))
// {
// //Get the latest info from the gps object which it derived from the data sent by the GPS unit
// gps_fix fix = gps.read();
// float ms = 0.447*(fix.speed_mph());
//
// Serial.print( F("\nSpeed m/s:") );
// if (fix.valid.speed)
// Serial.print( ms );
//
// if (fix.valid.time){
// Serial.print( F("\nCentiSeconds:") );
// Serial.print( fix.dateTime_cs );
// Serial.println();
//
// }
// Serial.println();
// }
get_accel();
}
void sendUBX( const unsigned char *progmemBytes, size_t len )
{
serial_connection.write( 0xB5 ); // SYNC1
serial_connection.write( 0x62 ); // SYNC2
uint8_t s = 0, t = 0;
while (len-- > 0) {
uint8_t u = pgm_read_byte( progmemBytes++ );
s += u;
t += s;
serial_connection.write( u );
}
serial_connection.write( s ); // CHECKSUM A
serial_connection.write( t ); // CHECKSUM B
}
Here is the output:
-0.01 -0.01 -0.01 0.02
-0.01 -0.01 -0.01 0.02
-0.01 -0.01 -0.01 0.02
-0.01 -0.01 -0.01 0.02
-0.01 -0.01 -0.01 0.02
-0.01 -0.01 -0.01 0.02
-0.01 -0.01 -0.01 0.02
-0.01 -0.01 -0.01 0.02
-0.01 -0.01 -0.01 0.02
-0.01 -0.01 -0.01 0.02
-0.01 -0.01 -0.01 0.02
........continued
To me, it seems like the problem is in the initialization/void setup of the concatenated code.
I changed the baud rate to 115200, removing the indicated code out of the magnetometer made it operate incorrectly, and all of the Serial.print statements seem to not be affecting the code because it works in its own code.
Hopefully this helps. I'm not good at debugging and as I said, I'm definitely a beginner. I'm sorry if I'm being too difficult to help! I just don't know where to go from here.
