Adafruit_ICM20X library causing void setup() code to loop

[carysmac]

Hi, I am using an adafruit ICM20948 and have successfully ran the example code from the github repository to receive the values. However when I copy the code from the repository into pre-existing (working code), the setup code loops all of the instructions before the:

 if (!icm.begin_I2C()) {
    Serial.println("Failed to find ICM20948 chip");
    while (1) {
      delay(10);
    }
  }

so if my code looks likes this simpler example:

void setup() {
    Serial.begin(9600);

    Serial.println("testing...");
    delay(500);

    if (!icm.begin_I2C()) {
      Serial.println("Failed to find ICM20948 chip");
      while (1) {
        delay(10);
      }
    }

    Serial.println("ICM20948 Found!");
}

my serial monitor will continuously receive testing... with 0.5 second delay, I have no idea why this is, has anyone experienced this before? to be clear I know the icm.begin_I2C() function works in isolation but it seems to be crashing when included in my code. I've considered this is due to the fact I'm using an Arduino UNO and my code uses the analogue pins 4 and 5 as outputs, which I'm aware are also capable of I2C? Since my ICM is connected with the dedicated SCL and SDA pins I assumed this wouldn't be an issue.

[jremington]

Always post complete code that compiles, runs and demonstrates the problem.

[carysmac]

I'm trying to modify the code for a small robot to include IMU sensor inputs.

the original code can be found from here: BEATRIX: An open source humanoid head platform for robotics teaching and research - ScienceDirect

Sorry in advance, I'm aware its quite long.

#include <AccelStepper.h>
#include <MultiStepper.h>

#include <Adafruit_ICM20X.h>
#include <Adafruit_ICM20948.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

// Define pins for stepper motor of robot neck
// motor X
#define X_STEP_PIN 2          
//#define X_DIR_PIN 5
//pinMode(D5, OUTPUT);

// motor Y
#define Y_STEP_PIN 3
#define Y_DIR_PIN 6

// motor Z
//#define Z_STEP_PIN 4
//pinMode(D4, OUTPUT);
#define Z_DIR_PIN 7

// pin to enable/disable all motors
#define ENABLE_PIN 8

// maximum number of characters for commands
#define MAX_SIZE_COMMAND 20
// maximum number of paramater for each command
#define MAX_NUM_PARAMETERS 20
// maximum motor speed
#define MAX_SPEED 200
// maximum motor acceleration
#define MAX_ACCEL 200


// creates object for motor X
AccelStepper stepperX(1, X_STEP_PIN, X_DIR_PIN); // (num. of motor, dir, step)
// creates object for motor Y
AccelStepper stepperY(1, Y_STEP_PIN, Y_DIR_PIN); // (num. of motor, dir, step)
// creates object for motor Z
AccelStepper stepperZ(1, Z_STEP_PIN, Z_DIR_PIN); // (num. of motor, dir, step)
// creates object to handle multiple motors
MultiStepper steppers;


// define the number of motors to handle simultaneously
long multiStepperPositions[3];

// array to store commands received
char commands_char[MAX_NUM_PARAMETERS][MAX_SIZE_COMMAND];
// counter for commands
int ncommand = 0;
// counter for parameters
int count = 0;
// gets characters
char current_char;
// stores the current status of a command
bool commandStatus = false;
// stores calibration status of the robot
int calibrationStatus = 0;

// temporary inital accelerometer readings for X for calibration of IMU
float FirstCalX = 0.0;
// temporary inital accelerometer readings for Y for calibration of IMU
float FirstCalY = 0.0;
// temporary inital accelerometer readings for Z for calibration of IMU
float FirstCalZ = 0.0;

//Global Frame Rate for all IMU values
float GlobalXAcc = 0.0;
float GlobalYAcc = 0.0;
float GlobalZAcc = 0.0;
float GlobalXGyro = 0.0;
float GlobalYGyro = 0.0;
float GlobalZGyro = 0.0;
float GlobalXMag = 0.0;
float GlobalYMag = 0.0;
float GlobalZMag = 0.0;

//time delay between calibration readings and error in sensor difference
int ErrorCal = 1;
int TimeDelay = 500;


// temporary used for @MOVALL command
boolean inPositionX = false;
boolean inPositionY = false;   
boolean inPositionZ = false;

Adafruit_ICM20948 icm;
uint16_t measurement_delay_us = 65535;


void setup()
{

    // set baudrate for communication with Arduino board
    Serial.begin(9600);

    delay(500);


    Serial.println("Adafruit ICM20948 test!");

    Serial.println("Pre if statement");


    
    if (icm.begin_I2C()) {
      Serial.println("ICM20948 Found!");
    } else {

      
      Serial.println("Failed to find ICM20948 chip");
      while (1) {
        delay(10);
      }
      
    }
    

    Serial.println("ICM20948 Found!");

    
    
    //set gyro and acc range aat highest limit
    icm.setGyroRange(ICM20948_GYRO_RANGE_2000_DPS);
    icm.setAccelRange(ICM20948_ACCEL_RANGE_16_G);

    //set divisor for acc reading
    icm.setAccelRateDivisor(4095);
    uint16_t accel_divisor = icm.getAccelRateDivisor();
    float accel_rate = 1125 / (1.0 + accel_divisor);

    //set divisor for gyro reading
    icm.setGyroRateDivisor(255);
    uint8_t gyro_divisor = icm.getGyroRateDivisor();
    float gyro_rate = 1100 / (1.0 + gyro_divisor);
    
    //set Mag data rate
    icm.setMagDataRate(AK09916_MAG_DATARATE_10_HZ);


    // initialise motors to move zero steps
    stepperX.move(0);
    stepperY.move(0);
    stepperZ.move(0);

    // initialise speed and acceleration for motor X
    stepperX.setMaxSpeed(MAX_SPEED);
    stepperX.setAcceleration(MAX_ACCEL);

    // initialise speed and acceleration for motor Y
    stepperY.setMaxSpeed(MAX_SPEED);
    stepperY.setAcceleration(MAX_ACCEL);

    // initialise speed and acceleration for motor Z
    stepperZ.setMaxSpeed(MAX_SPEED);
    stepperZ.setAcceleration(MAX_ACCEL);
  
    // Allocates all motors to MultiStepper to manage in corresponding commands
    steppers.addStepper(stepperX);
    steppers.addStepper(stepperY);
    steppers.addStepper(stepperZ);

    // configures the enable_pin as output
    pinMode(ENABLE_PIN, OUTPUT);
    // initialise enable_pin as HIGH to disable the motors
    // HIGH: disable robot motors
    // LOW: enable robot motors
    digitalWrite(ENABLE_PIN, HIGH);


    
}


void loop()
{
    // if data is received, then it is stored in commands_char
    if( Serial.available() > 0 )
    {      
        for( int i = 0; i < MAX_NUM_PARAMETERS; i++ )
        {
            for( int j = 0; j < MAX_SIZE_COMMAND; j++ )
                commands_char[i][j] = '\0';
        }

        count = 0;
        ncommand = 0;

        // stores commmands and parameters in commands_char
        do
        {
            current_char = Serial.read();
            
            delay(3);

            if( current_char != ' ' )
            {
                commands_char[ncommand][count] = current_char;
                count++;
            }
            else
            {
                commands_char[ncommand][count] = '\0';
                count = 0;
                ncommand++;                
            }            
        }while( current_char != '\r' );

        // check if the command received is correct
        commandStatus = commandList(commands_char[0]);
        replyAcknowledge(commandStatus);

        // if the command is correct, then it is executed by the corresponding function
        if( commandStatus == true )
            replyAcknowledge(executeCommand(commands_char));

        // cleans the serial pipe
        Serial.flush();
    }

}    

/* Function for execution of commands */
bool executeCommand(char cmdReceived[][MAX_SIZE_COMMAND])
{
    int step_size_int[20];
    int abs_step_size_int[20];
    int speed_int[20];
    int xMotorPos = 0;
    int yMotorPos = 0;
    int zMotorPos = 0;

    /* Enable/disable motors */
    if( !strcmp(cmdReceived[0],"@ENMOTORS") )
    {
      if( !strcmp(cmdReceived[1],"ON\r") )
        digitalWrite(ENABLE_PIN, LOW);
      else if( !strcmp(cmdReceived[1],"OFF\r") )
        digitalWrite(ENABLE_PIN, HIGH);
      else
        return false;

      calibrationStatus = 0;

      return true;
    }
    /* Calibration of X axis */
    if( !strcmp(cmdReceived[0],"@CALX") )
    {
      return true;
      // TBD
    }
    /* Calibration of Y axis */
    else if( !strcmp(cmdReceived[0],"@CALY") )
    {
      return true;
      // TBD
    }
    /* Calibration of Z axis */
    else if( !strcmp(cmdReceived[0],"@CALZ") )
    {
      return true;
      // TBD
    }
    /* Calibration - OK*/ 
    else if( !strcmp(cmdReceived[0],"@CALNOW\r") )
    {
        stepperX.setCurrentPosition(0);        
        stepperY.setCurrentPosition(0);        
        stepperZ.setCurrentPosition(0);

        //find IMU values
        sensors_event_t accel;
        sensors_event_t gyro;
        sensors_event_t mag;
        sensors_event_t temp;
        icm.getEvent(&accel, &gyro, &temp, &mag);

        //define the first readings as the current
        FirstCalX = gyro.gyro.x;
        FirstCalY = gyro.gyro.y;
        FirstCalZ = gyro.gyro.z;

        //loop the check to ensure the beatrix head is still while taking Global frame readings
        int i = 1;
        while (i == 1) {
          if (((FirstCalX-ErrorCal)<GlobalXGyro<(FirstCalX+ErrorCal)) && ((FirstCalY-ErrorCal)<GlobalYGyro<(FirstCalY+ErrorCal)) && ((FirstCalZ-ErrorCal)<GlobalZGyro<(FirstCalZ+ErrorCal))){
            //set all global readings (gyro already correct)
            float GlobalXAcc = 0.0;
            float GlobalYAcc = 0.0;
            float GlobalZAcc = 0.0;
            float GlobalXMag = 0.0;
            float GlobalYMag = 0.0;
            float GlobalZMag = 0.0;

            //end loop
            i = 5;
          } else {

            //reset to loop again
            GlobalXGyro = FirstCalX;
            GlobalYGyro = FirstCalY;
            GlobalZGyro = FirstCalZ;
            FirstCalX = gyro.gyro.x;
            FirstCalY = gyro.gyro.y;
            FirstCalZ = gyro.gyro.z;

            //delay to ensure readings have time to change
            delay(TimeDelay);
          }
        }

        calibrationStatus = 1;

        return true;
    }
    /* Move all axes to home position - OK*/
    else if( !strcmp(cmdReceived[0], "@MOVHOME\r") )
    {
        if( calibrationStatus == 1 )
        {
            multiStepperPositions[0] = 0;
            multiStepperPositions[1] = 0;
            multiStepperPositions[2] = 0;

            steppers.moveTo(multiStepperPositions);
            steppers.runSpeedToPosition();
            delay(1000);

            stepperX.setCurrentPosition(stepperX.currentPosition());
            stepperY.setCurrentPosition(stepperY.currentPosition());
            stepperZ.setCurrentPosition(stepperZ.currentPosition());

            //check sensors are in home position

            return true;
        }
        else
            return false;    
    }
    /* Stop all motors - OK*/
    else if( !strcmp(cmdReceived[0],"@STOPALL\r") )
    {
        stepperX.stop();
        stepperY.stop();
        stepperZ.stop();
    }
    /* Get position from X axis - OK*/
    else if( !strcmp(cmdReceived[0],"@GETXPOS\r") )
    {
        Serial.println(stepperX.currentPosition());
    }
    /* Get position from Y axis - OK*/
    else if( !strcmp(cmdReceived[0],"@GETYPOS\r") )
    {
        Serial.println(stepperY.currentPosition());
    }
    /* Get position from Z axis - OK*/
    else if( !strcmp(cmdReceived[0],"@GETZPOS\r") )
    {
        Serial.println(stepperZ.currentPosition());
    }
    /* Get position from all axis - OK*/
    else if( !strcmp(cmdReceived[0],"@GETALLPOS\r") )
    {
        xMotorPos = stepperX.currentPosition();
        yMotorPos = stepperY.currentPosition();
        zMotorPos = stepperZ.currentPosition();
        Serial.print("\n");
        Serial.print(xMotorPos);
        Serial.print(" ");
        Serial.print(yMotorPos);
        Serial.print(" ");
        Serial.print(zMotorPos);
        Serial.print("\n");
    }
    /* Relative movement of X axis - OK*/
    else if( !strcmp(cmdReceived[0],"@MOVRX") )
    {
        if( calibrationStatus == 1 )
        {
          if( strcmp(cmdReceived[1]," ") && strcmp(cmdReceived[2]," ") )
          {
              step_size_int[0] = atoi(cmdReceived[1]);           
              speed_int[0] = atoi(cmdReceived[2]);
                              
              if( speed_int[0] > MAX_SPEED )
                  speed_int[0] = MAX_SPEED;
                  
              stepperX.move(step_size_int[0]);
        
              while( stepperX.distanceToGo() != 0 )
                  stepperX.run();
                  
              stepperX.stop();
              stepperX.runToPosition();

              return true;
          }
          else
              return false;       
        }
        else       
            return false;
    }
    /* Relative movement of Y axis - OK*/
    else if( !strcmp(cmdReceived[0],"@MOVRY") )
    {
        if( calibrationStatus == 1 )
        {
          if( strcmp(cmdReceived[1]," ") && strcmp(cmdReceived[2]," ") )
          {
              step_size_int[0] = atoi(cmdReceived[1]);
              speed_int[0] = atoi(cmdReceived[2]);
               
              if( speed_int[0] > MAX_SPEED )
                  speed_int[0] = MAX_SPEED;

              stepperY.move(step_size_int[0]);
        
              while( stepperY.distanceToGo() != 0 )
                  stepperY.run();
                  
              stepperY.stop();
              stepperY.runToPosition();
    
              return true;
          }
          else
              return false;       
        }
        else       
            return false;
    }
    /* Relative movement of Z axis - OK*/
    else if( !strcmp(cmdReceived[0],"@MOVRZ") )
    {
        if( calibrationStatus == 1 )
        {
          if( strcmp(cmdReceived[1]," ") && strcmp(cmdReceived[2]," ") )
          {
              step_size_int[0] = atoi(cmdReceived[1]);
              speed_int[0] = atoi(cmdReceived[2]);
               
              if( speed_int[0] > MAX_SPEED )
                  speed_int[0] = MAX_SPEED;

              stepperZ.move(step_size_int[0]);
        
              while( stepperZ.distanceToGo() != 0 )
                  stepperZ.run();
                  
              stepperZ.stop();
              stepperZ.runToPosition();
    
              return true;
          }
          else
              return false;       
        }
        else       
            return false;
    }
    /* Absolute movement of X axis - OK */
    else if( !strcmp(cmdReceived[0],"@MOVAX") )
    {
        if( calibrationStatus == 1 )
        {
          if( strcmp(cmdReceived[1]," ") && strcmp(cmdReceived[2]," ") )
          {
              step_size_int[0] = atoi(cmdReceived[1]);
              speed_int[0] = atoi(cmdReceived[2]);
              
              if( speed_int[0] > MAX_SPEED )
                  speed_int[0] = MAX_SPEED;

              stepperX.moveTo(step_size_int[0]);

              while( stepperX.currentPosition() != step_size_int[0] )
                  stepperX.run();
                  
              stepperX.stop();
              stepperX.runToPosition();
    
              return true;
          }
          else
              return false;       
        }
        else       
            return false;
    }
    /* Absolute movement of Y axis - OK */
    else if( !strcmp(cmdReceived[0],"@MOVAY") )
    {
        if( calibrationStatus == 1 )
        {
          if( strcmp(cmdReceived[1]," ") && strcmp(cmdReceived[2]," ") )
          {
              step_size_int[0] = atoi(cmdReceived[1]);
              speed_int[0] = atoi(cmdReceived[2]);
              
              if( speed_int[0] > MAX_SPEED )
                  speed_int[0] = MAX_SPEED;

              stepperY.moveTo(step_size_int[0]);

              while( stepperY.currentPosition() != step_size_int[0] )
                  stepperY.run();
                  
              stepperY.stop();
              stepperY.runToPosition();
    
              return true;
          }
          else
              return false;       
        }
        else       
            return false;
    }
    /* Absolute movement of Z axis - OK */
    else if( !strcmp(cmdReceived[0],"@MOVAZ") )
    {
        if( calibrationStatus == 1 )
        {
          if( strcmp(cmdReceived[1]," ") && strcmp(cmdReceived[2]," ") )
          {
              step_size_int[0] = atoi(cmdReceived[1]);
              speed_int[0] = atoi(cmdReceived[2]);
              
              if( speed_int[0] > MAX_SPEED )
                  speed_int[0] = MAX_SPEED;

              stepperZ.moveTo(step_size_int[0]);

              while( stepperZ.currentPosition() != step_size_int[0] )
                  stepperZ.run();
                  
              stepperZ.stop();
              stepperZ.runToPosition();
    
              return true;
          }
          else
              return false;       
        }
        else       
            return false;
    }
    /* Relative movement of all axes - OK*/
    else if( !strcmp(cmdReceived[0],"@MOVRALL") )
    {
        if( calibrationStatus == 1 )
        {
          if( strcmp(cmdReceived[1]," ") && strcmp(cmdReceived[2]," ") && strcmp(cmdReceived[3]," ") && strcmp(cmdReceived[4]," ") && 
              strcmp(cmdReceived[5]," ") && strcmp(cmdReceived[6]," ") && strcmp(cmdReceived[7]," ") && strcmp(cmdReceived[8]," "))
          {

              inPositionX = false;
              inPositionY = false;   
              inPositionZ = false;

              step_size_int[0] = atoi(cmdReceived[1]);
              step_size_int[1] = atoi(cmdReceived[2]);
              step_size_int[2] = atoi(cmdReceived[3]);
              speed_int[0] = atoi(cmdReceived[5]);
              speed_int[1] = atoi(cmdReceived[6]);
              speed_int[2] = atoi(cmdReceived[7]);
               
              if( speed_int[0] > MAX_SPEED )
                  speed_int[0] = MAX_SPEED;

              if( speed_int[1] > MAX_SPEED )
                  speed_int[1] = MAX_SPEED;

              if( speed_int[2] > MAX_SPEED )
                  speed_int[2] = MAX_SPEED;

              stepperX.move(step_size_int[0]);
              stepperY.move(step_size_int[1]);
              stepperZ.move(step_size_int[2]);
        
              do
              {          
                  if( stepperX.distanceToGo() != 0 )
                    stepperX.run();
                  else
                  {
                    stepperX.stop();
                    stepperX.runToPosition();
                    inPositionX = true;
                  }
        
                  if( stepperY.distanceToGo() != 0 )
                    stepperY.run();
                  else
                  {
                    stepperY.stop();
                    stepperY.runToPosition();
                    inPositionY = true;
                  }
    
                  if( stepperZ.distanceToGo() != 0 )
                    stepperZ.run();
                  else
                  {
                    stepperZ.stop();
                    stepperZ.runToPosition();
                    inPositionZ = true;
                  }

              }while( ( inPositionX != true ) || ( inPositionY != true ) || ( inPositionZ != true ) );

              delay(1000);

              stepperX.setCurrentPosition(stepperX.currentPosition());
              stepperY.setCurrentPosition(stepperY.currentPosition());
              stepperZ.setCurrentPosition(stepperZ.currentPosition());

              return true;
          }
          else
              return false;       
        }
        else       
            return false;
    }
    /* Absolute movement of all axes - OK*/
    else if( !strcmp(cmdReceived[0],"@MOVAALL") )
    {
        if( calibrationStatus == 1 )
        {
          if( strcmp(cmdReceived[1]," ") && strcmp(cmdReceived[2]," ") && strcmp(cmdReceived[3]," ") && strcmp(cmdReceived[4]," ") && 
              strcmp(cmdReceived[5]," ") && strcmp(cmdReceived[6]," ") && strcmp(cmdReceived[7]," ") && strcmp(cmdReceived[8]," "))
          {
              step_size_int[0] = atoi(cmdReceived[1]);
              step_size_int[1] = atoi(cmdReceived[2]);
              step_size_int[2] = atoi(cmdReceived[3]);
              speed_int[0] = atoi(cmdReceived[5]);
              speed_int[1] = atoi(cmdReceived[6]);
              speed_int[2] = atoi(cmdReceived[7]);
               
              if( speed_int[0] > MAX_SPEED )
                  speed_int[0] = MAX_SPEED;

              if( speed_int[1] > MAX_SPEED )
                  speed_int[1] = MAX_SPEED;

              if( speed_int[2] > MAX_SPEED )
                  speed_int[2] = MAX_SPEED;

              if( speed_int[3] > MAX_SPEED )
                  speed_int[3] = MAX_SPEED;

              multiStepperPositions[0] = step_size_int[0];
              multiStepperPositions[1] = step_size_int[1];
              multiStepperPositions[2] = step_size_int[2];

              steppers.moveTo(multiStepperPositions);
              steppers.runSpeedToPosition();
              delay(1000);

              stepperX.setCurrentPosition(stepperX.currentPosition());
              stepperY.setCurrentPosition(stepperY.currentPosition());
              stepperZ.setCurrentPosition(stepperZ.currentPosition());
    
              return true;
          }
          else
              return false;       
        }
        else       
            return false;
    }
    /* Check the status of the current command*/
    else if( !strcmp(cmdReceived[0],"@COMSTATUS\r") )
    {
        return true;
      // TBD
    }
    /* Check the status of the robot calibration*/
    else if( !strcmp(cmdReceived[0],"@CALSTATUS\r") )
    {
        if( calibrationStatus == 1 )
            return true;
        else
            return false;
    }
    /* Check the status of the IMU accel*/
    else if( !strcmp(cmdReceived[0],"@GETALLACCEL\r") )
    {
      sensors_event_t accel;
      sensors_event_t gyro;
      sensors_event_t mag;
      sensors_event_t temp;
      icm.getEvent(&accel, &gyro, &temp, &mag);

      Serial.print("\n");
      Serial.print(accel.acceleration.x);
      Serial.print(" ");
      Serial.print(accel.acceleration.y);
      Serial.print(" ");
      Serial.print(accel.acceleration.z);
      Serial.print("\n");

    }
    /* Check the status of the IMU mag*/
    else if( !strcmp(cmdReceived[0],"@GETALLMAG\r") )
    {
      sensors_event_t accel;
      sensors_event_t gyro;
      sensors_event_t mag;
      sensors_event_t temp;
      icm.getEvent(&accel, &gyro, &temp, &mag);

      Serial.print("\n");
      Serial.print(mag.magnetic.x);
      Serial.print(" ");
      Serial.print(mag.magnetic.y);
      Serial.print(" ");
      Serial.print(mag.magnetic.z);
      Serial.print("\n");

    }
    /* Check the status of the IMU gryo*/
    else if( !strcmp(cmdReceived[0],"@GETALLGRYO\r") )
    {
      sensors_event_t accel;
      sensors_event_t gyro;
      sensors_event_t mag;
      sensors_event_t temp;
      icm.getEvent(&accel, &gyro, &temp, &mag);

      Serial.print("\n");
      Serial.print(gyro.gyro.x);
      Serial.print(" ");
      Serial.print(gyro.gyro.y);
      Serial.print(" ");
      Serial.print(gyro.gyro.z);
      Serial.print("\n");
      
    }

    else
        return false;
}

/* Send reply ACK/NACK to client */
void replyAcknowledge(bool cmdStatus)
{
    if( cmdStatus == true )
        sendACK();
    else
        sendNACK();

    Serial.flush();
}

/* Print ACK message */
void sendACK()
{
    Serial.print("ACK\n");
}

/* Print NACK message */
void sendNACK()
{
    Serial.print("NACK\n");
}

/* Check the command received */
bool commandList(char *cmdReceived)
{
    char *commandArray[] = {"@CALSTART\r",    // TBD: Start calibration automatically
                            "@CALX\r",        // TBD: Calibrate motor X only
                            "@CALY\r",        // TBD: Calibrate motor Y only
                            "@CALZ\r",        // TBD: Calibrate motor Z only
                            "@CALSTATUS\r",   // Check the current calibration status of all motors
                            "@CALNOW\r",      // Calibrate the robot initial/home position (x=0, y=0, z=0)
                            "@CALEND\r",      // TBD: End robot calibration
                            "@MOVHOME\r",     // Move all motors to initial/home position
                            "@MOVRX",         // Move motor X relative to current X motor position
                            "@MOVRY",         // Move motor Y relative to current Y motor position
                            "@MOVRZ",         // Move motor Z relative to current Z motor position
                            "@MOVAX",         // Move motor X from home position
                            "@MOVAY",         // Move motor Y from home position
                            "@MOVAZ",         // Move motor Z from home position
                            "@MOVRALL",       // Move all motors relative to their current motor positions
                            "@MOVAALL",       // Move all motors from their home positions
                            "@STOPALL\r",     // Stop all motor movements
                            "@GETALLPOS\r",   // Get the current positions of all motors
                            "@GETXPOS\r",     // Get the current position of motor X
                            "@GETYPOS\r",     // Get the current position of motor Y
                            "@GETZPOS\r",     // Get the current position of motor Z
                            "@COMSTATUS\r",   // TBD: Check the status of a command
                            "@ENMOTORS"       // Enable/disable motors to be actuated
                            "@GETALLACCEL\r"  // Get all axis accel data from IMU
                            "@GETALLMAG\r"    // Get all axis mag data from IMU
                            "@GETALLGYRO\r"   // Get all axis gyro data from IMU
                            };   
    int ncommands = 26;
    
    // Search whether the command receive exist in the commandArray list
    for( int i = 0; i < ncommands; i++ )
    {
        if( !strcmp(commandArray[i], cmdReceived) )
            return true;
    }
    
    return false;
}

[jremington]

@carysmac

my serial monitor will continuously receive testing... with 0.5 second delay

That is almost certainly due to the Arduino constantly rebooting, perhaps due to a power supply or wiring problem.

The code you are testing is complex, with several motors, so for further help, please post a complete schematic of the setup, along with links to specifications for all the parts, including the power supplies. A photo of the setup would be helpful.

A memory problem can also cause rebooting, so post the final memory usage summary published by the Arduino IDE when the code is compiled and linked.

[carysmac]

Ah rebooting is a good idea, this is the code running with nothing but the IMU connected to the I2C pins, the original code can run with nothing connected without this problem.