Well, I figured I would provide the general outline of the prog to prevent visual clutter, but here it is:
//AUTHOR: Jaz S
#define BAUD 115200
#define ANGLE_SCALE_FACTOR 0.0109863 // Convert register data to Degrees
boolean IMU_flag = false;
struct IMU{
int data;
} phi, theta, psi;
void setup(){
Serial.begin(BAUD); //Sets baud rate for PC USB
Serial1.begin(BAUD); //Sets baud rate for Serial port 1
Serial.println("Setup complete");
}
void loop(){
Serial.println("Starting loop"); //Makes no difference if I leave this in or take it out.
POLL_AGAIN:
read_UM6();
if (IMU_flag == true){
Serial.println(psi.data*ANGLE_SCALE_FACTOR);/*This is the data I want printed, from this very line of code. It doesn't happen if I don't have the Serial.print() in the read_UM6() function.*/
delay(250);
}
else {
goto POLL_AGAIN;
}
}//end VOID
void read_UM6(){
Serial.print("Reading IMU... "); /*This is the line that creates trouble! If I take it out, loop() the whole program stops.*/
phi.data = 0;
theta.data = 0;
psi.data = 0;
unsigned int c = 0;
unsigned int data[5] = {0};
unsigned long data_sum = 0;
byte blank = 0, temp = 0;
byte chksum1 = 0, chksum0 = 0;
unsigned int chksum = 0;
//If there's data in the serial temp register and we haven't finished retrieving data...
if ((Serial1.available() > 0)) {
c = Serial1.read();
if ((c == 's')){
c = Serial1.read();
if ((c == 'n')){
c = Serial1.read();
if ((c == 'p')) {
c = Serial1.read();
if (c == (PT_HAS_DATA | PT_IS_BATCH | PT_BATCH_LEN)) {
c = Serial1.read();
if (c == REG_EULER_PHI_THETA) {
for (byte i = 0; i < 6; i++){
data[i] = Serial1.read();
data_sum += data[i];
}
blank = Serial1.read();
blank = Serial1.read();
chksum1 = Serial1.read();
chksum0 = Serial1.read();
chksum = (chksum1 << 8) | chksum0;
if (chksum == ('s' + 'n' + 'p' + (PT_HAS_DATA | PT_IS_BATCH | PT_BATCH_LEN) + REG_EULER_PHI_THETA + data_sum)){ /*If checksum is correct...*/
phi.data = (data[1] | (data[0] << 8));
theta.data = (data[3] | (data[2] << 8));
psi.data = (data[5] | (data[4] << 8));
IMU_flag = true;
return;
}
else {
FLUSH:
while ((Serial1.available() > 0)){
blank = Serial1.read();
}
IMU_flag = false;
return;
}
}
else {
goto FLUSH;
}
}
else
{
goto FLUSH;
}
}
else
{
goto FLUSH;
}
}
else
{
goto FLUSH;
}
}
else
{
goto FLUSH;
}
}//end if
//If no data is available and we have not retrieved data...
else if (Serial1.available() == 0){
IMU_flag = false;
return;
} //end else
else {
IMU_flag = false;
return;
}
} //end void