Arduino Hanging/Crashing When using both LSM6DS3TRC and MCP2515

pkness · August 2, 2024, 2:26am

I am using an MCP2515 to control two motors via can, and an LSM6DS3TRC imu on a single arduino (Previously Mega, now Nano). I have encountered an issue, which is that after about 10-15 minutes, the arduino stops sending out CAN (The motors stop moving) and stops responding to serial commands. When I disable either the IMU or the CAN sending, this is not an issue (controlled by the #defines on the top)

Here is my code:

/*
  CAN Send Example

  This will setup the CAN controller(MCP2515) to send CAN frames.
  Transmitted frames will be printed to the Serial port.
  Transmits a CAN standard frame every 2 seconds.

  MIT License
  https://github.com/codeljo/AA_MCP2515
*/
#define MAX_VELO_RPM 7
#define ACCEL_RAD_S_S 0.2
#define DT_MS 300
#define ANGLE_OF_ATTACK 15
#define SEED 2132138

#define EN_CAN 1
#define EN_IMU 1

#include "AA_MCP2515.h"
#include <Adafruit_LSM6DS3TRC.h>

#define TIMING_CYCLE 100
#define TIMING_TOLERANCE 10

#define SERIAL_TIMING 0
#define COMMAND_TIMING 20
#define QUERY_TIMING 40
#define IMU_TIMING 60
#define PRINT_TIMING 80

#define DT_CYCLES DT_MS/TIMING_CYCLE

Adafruit_LSM6DS3TRC lsm6ds3trc;

struct Motor {
  int16_t angle;
  int16_t velocity;
  int16_t current;
  uint8_t temp;

  uint16_t encoder;
  uint16_t encoderRaw;
  int16_t encoderOffset;
};

void printMotor(Motor m, char c = '?');
void setVelocity(uint16_t canID, int32_t velocity_dps_hundreth);
void setAngle(uint16_t canID, int16_t velocity_dps, int16_t angle_deg_hundreth);

// TODO: modify CAN_BITRATE, CAN_PIN_CS(Chip Select) pin, and CAN_PIN_INT(Interrupt) pin as required.
const CANBitrate::Config CAN_BITRATE = CANBitrate::Config_8MHz_500kbps;
const uint8_t CAN_PIN_CS = 10;
const int8_t CAN_PIN_INT = 2;

const float xOffset = 0.6; //-9.89 9.77
const float yOffset = 0.295; //-10.09 9.50
const float zOffset = -0.325; //-9.50 10.15

float accelX = 0;
float accelY = 0;
float accelZ = 0;

CANConfig config(CAN_BITRATE, CAN_PIN_CS, CAN_PIN_INT);
CANController CAN(config);

Motor motorA;
Motor motorB;

Motor motors[2] = {motorA, motorB};

double heading = 0;

// String inst;

int commandLoop = 0;
int printLoopCount = 0;

const char endCharA = 'A';
const char endCharB = 'B';
const char purge = '_';

float valueA = 0;
float valueB = 0;

int pointCount = 0;

char mode = 'm';

sensors_event_t accel;
sensors_event_t gyro;
sensors_event_t temp;

bool serialRead = 0;
bool commandSent = 0;
bool querySent = 0;
bool imuRecv = 0;
bool printSent = 0;

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

  while(CAN.begin(CANController::Mode::Normal) != CANController::OK) {
    Serial.println("CAN begin FAIL - delaying for 1 second");
    delay(1000);
  }
  Serial.println("CAN begin OK");

  if (!lsm6ds3trc.begin_I2C()) {
    // if (!lsm6ds3trc.begin_SPI(LSM_CS)) {
    // if (!lsm6ds3trc.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) {
    Serial.println("Failed to find LSM6DS3TR-C chip");
    while (1) {
      delay(10);
    }
  }

  Serial.println("LSM6DS3TR-C Found!");

  resetMotor(0x141);
  resetMotor(0x142);

  pinMode(7, OUTPUT);

  lsm6ds3trc.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
  lsm6ds3trc.setAccelDataRate(LSM6DS_RATE_12_5_HZ);

  // lsm6ds3trc.configInt1(false, false, true); // accelerometer DRDY on INT1
  // lsm6ds3trc.configInt2(false, true, false); // gyro DRDY on INT2


  randomSeed(SEED);
}

void loop() {
  
  long time = millis();
  int cyclePosition = time%TIMING_CYCLE;

  if(cyclePosition > SERIAL_TIMING && cyclePosition <= SERIAL_TIMING + TIMING_TOLERANCE){
    if(Serial.available()){
      char inChar = Serial.read();
      // Serial.println("--");
      // Serial.println(inChar);
      // Serial.println((int)inChar);
      // Serial.println("--");
      if(inChar >= 48 && inChar <= 57){
        // inst += inChar;
      }else if(inChar == 'p'){
        mode = 'p';
        resetMotor(0x141);
        resetMotor(0x142);
        // setCurrent(0x141,0);
        // setCurrent(0x142,0);
      }else if(inChar == 'a'){
        mode = 'a';
        resetMotor(0x141);
        resetMotor(0x142);
        // setCurrent(0x141,0);
        // setCurrent(0x142,0);

        accelX = 0;
        accelY = 0;
        accelZ = 0;
      }else if(inChar == 'v'){
        mode = 'v';
        resetMotor(0x141);
        resetMotor(0x142);
        // setCurrent(0x141,0);
        // setCurrent(0x142,0);
      }else if(inChar == 'r'){
        mode = ' ';
        resetMotor(0x141);
        resetMotor(0x142);
        // setCurrent(0x141,0);
        // setCurrent(0x142,0);
      }else if(mode == 'p' && inChar == endCharA){
        // valueA = atof(inst.c_str());
        // setAngleSingle(0x141,MAX_VELO_RPM,(int16_t)(valueA*100));
        // inst = "";
      }else if(mode == 'p' && inChar == endCharB){
        // valueB = atof(inst.c_str());
        // setAngleSingle(0x142,MAX_VELO_RPM,(int16_t)(valueB*100));
        // inst = "";
      }else if(mode == 'v' && inChar == endCharA){
        // valueA = atof(inst.c_str());
        // setVelocity(0x141,(int32_t)(valueA*100));
        // inst = "";
      }else if(mode == 'v' && inChar == endCharB){
        // valueB = atof(inst.c_str());
        // setVelocity(0x142,(int32_t)(valueB*100));
        // inst = "";
      }else if(inChar == purge){
        // inst = "";
      }else if(inChar == 'd'){
        Serial.print("+");
        Serial.print(accelX);
        Serial.print("=");
        Serial.print(accelY);
        Serial.print("=");
        Serial.print(accelZ);
        Serial.print("=");
        Serial.print(motors[0].encoder);
        Serial.print("=");
        Serial.print(motors[1].encoder);
        Serial.print("=");
      }
    }
  }else if(cyclePosition > COMMAND_TIMING && cyclePosition <= COMMAND_TIMING + TIMING_TOLERANCE){
    if(!commandSent){
      if(mode == 'a'){
        if(commandLoop > DT_CYCLES){
          heading += (random(65536)/65536.0) * ANGLE_OF_ATTACK - (ANGLE_OF_ATTACK/2);
          double heading_rad = heading * 3.14159 / 180;
          setVelocity(0x141,(int32_t)(sin(heading_rad)*MAX_VELO_RPM * 6 * 100));
          setVelocity(0x142,(int32_t)(cos(heading_rad)*MAX_VELO_RPM * 6 * 100));
          // printMotor(motorA,'a');
          // printMotor(motorB,'b');
          commandLoop = 0;
          pointCount ++;
          digitalWrite(7, HIGH);
        }else{
          queryMotor(0x141);
          queryMotor(0x142);
          digitalWrite(7, LOW);
        }
        commandLoop ++;

      }else{
        queryMotor(0x141);
        queryMotor(0x142);
      }
      commandSent = 1;
    }
  }else if(cyclePosition > QUERY_TIMING && cyclePosition <= QUERY_TIMING + TIMING_TOLERANCE){
    if(!querySent){
      queryMotorEncoder(0x141);
      queryMotorEncoder(0x142);
      querySent = 1;
    }
  }else if(cyclePosition > IMU_TIMING && cyclePosition <= IMU_TIMING + TIMING_TOLERANCE){
    #if EN_IMU
    if(!imuRecv){
      lsm6ds3trc.getEvent(&accel, &gyro, &temp);
      if(mode == 'a'){
        accelX = accel.acceleration.x + xOffset;
        accelY = accel.acceleration.y + yOffset;
        accelZ = accel.acceleration.z + zOffset;
      }
      imuRecv = 1;
    }
    #endif
  }else if(cyclePosition > PRINT_TIMING && cyclePosition <= PRINT_TIMING + TIMING_TOLERANCE){
    if(!printSent){
      if(printLoopCount > 1){

        double heading_rad = heading * 3.14159 / 180;
        // Serial.print("Heading:");
        // Serial.print(heading_rad);
        // Serial.print(" A Des:[");
        // Serial.print(sin(heading_rad)*MAX_VELO_RPM);
        // Serial.print("] Act:[");
        // Serial.print(motors[0].velocity/10.0);
        // Serial.print("] B Des:");
        // Serial.print(cos(heading_rad)*MAX_VELO_RPM);
        // Serial.print("] Act:[");
        // Serial.print(motors[1].velocity/10.0);

        // Serial.print("[Immediate] [");
        // Serial.print(accel.acceleration.x + xOffset);
        // Serial.print("] [");
        // Serial.print(accel.acceleration.y + yOffset);
        // Serial.print("] [");
        // Serial.print(accel.acceleration.z + zOffset);

        // Serial.print("] [Integral G] X:");
        // Serial.print(accelX);
        // Serial.print(" Y:");
        // Serial.print(accelY);
        // Serial.print(" Z:");
        // Serial.print(accelZ);

        // Serial.print(" | PC:");
        // Serial.println(pointCount);

        printLoopCount = 0;
      }
      printLoopCount ++;
      printSent = 1;
    }
  }else{
    serialRead = 0;
    commandSent = 0;
    querySent = 0;
    imuRecv = 0;
    printSent = 0;
  }

  getFeedback();
}


void printMotor(Motor m, char c = '?'){
  Serial.print("Motor ");
  Serial.print(c);
  Serial.print("| Angle: ");
  Serial.print(m.angle);
  Serial.print("| Velocity");
  Serial.print(m.velocity);
  Serial.print("| Temp");
  Serial.println(m.temp);
}

void setVelocity(uint16_t canID, int32_t velocity_dps_hundreth){
    uint8_t data[8] = { 
        0xA2, 
        0x00, 
        0x00, 
        0x00, 
        (uint8_t)(velocity_dps_hundreth), 
        (uint8_t)(velocity_dps_hundreth>>8), 
        (uint8_t)(velocity_dps_hundreth>>16), 
        (uint8_t)(velocity_dps_hundreth>>24)};

    // uint8_t data[8] = {0xA2, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00};
    CANFrame frame(canID, data, sizeof(data));
    // frame.print("TX");
    CAN.write(frame);
}

void setAngleSingle(uint16_t canID, int16_t velocity_dps, int16_t angle_deg_hundreth){
    uint8_t data[8] = { 
        0xA6, 
        0x00, 
        (uint8_t)(velocity_dps), 
        (uint8_t)(velocity_dps>>8), 
        (uint8_t)(angle_deg_hundreth), 
        (uint8_t)(angle_deg_hundreth>>8), 
        0x00, 
        0x00};

    // uint8_t data[8] = {0xA2, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00};
    CANFrame frame(canID, data, sizeof(data));
    // frame.print("TX");
    CAN.write(frame);
}

void setCurrent(uint16_t canID, int16_t current_hundreth){
    uint8_t data[8] = { 
        0xA1, 
        0x00, 
        0x00, 
        0x00, 
        (uint8_t)(current_hundreth), 
        (uint8_t)(current_hundreth>>8), 
        0x00, 
        0x00};

    // uint8_t data[8] = {0xA2, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00};
    CANFrame frame(canID, data, sizeof(data));
    // frame.print("TX");
    CAN.write(frame);
}

void queryMotor(uint16_t canID){
  sendCommand(canID, 0x9C);
}

void queryMotorEncoder(uint16_t canID){
  sendCommand(canID, 0x90);
}

void resetMotor(uint16_t canID){
  sendCommand(canID, 0x76);
}

void sendCommand(uint16_t canID, uint8_t command){
  uint8_t data[8] = { 
      command, 
      0x00, 
      0x00, 
      0x00, 
      0x00, 
      0x00, 
      0x00, 
      0x00};
  CANFrame frame(canID, data, sizeof(data));
  // frame.print("TX");
  CAN.write(frame);
}

void getFeedback(){
  uint8_t data[8] = {0,0,0,0,0,0,0,0};
  CANFrame frame(0x140,data,8);
  while (CAN.read(frame) == CANController::IOResult::OK) {
      frame.getData(data, 8);
      uint16_t id = frame.getId()-0x241;
      if(id == 0 || id == 1){
        if(data[0] == 0xA1 || data[0] == 0xA2 || data[0] == 0xA6 || data[0] == 0x9C){
            motors[id].angle = (int16_t)(data[6] | (data[7]<<8)); 
            motors[id].velocity = (int16_t)(data[4] | (data[5]<<8));
            motors[id].current = (int16_t)(data[2] | (data[3]<<8));
            motors[id].temp = data[1];
            // if(data[0] == 0x9C){
            //   Serial.println("9C");
            // }
        }else if(data[0] == 0x90){
            motors[id].encoder = (int16_t)(data[2] | (data[3]<<8)); 
            motors[id].encoderRaw = (int16_t)(data[4] | (data[5]<<8));
            motors[id].encoderOffset = (int16_t)(data[6] | (data[7]<<8));
            // frame.print("90");
        }else{
            // frame.print("WUT");
        }
      }
      digitalWrite(7,HIGH);
  }
}

What am I doing wrong? Is there a way for me to run the IMU on a second Nano extending the UART line from the first Nano?

horace · August 2, 2024, 5:33am

could you upload a schematic showing wiring and how you power the system?
was it working on the Mega and now failing on the Nano or failing on both?
is the time to fail consistent or random?
does the output to the serial monitor give some indication of where the problem occurs?
add extra Serial.println() statements to display variable values and the flow of the code
you can always remove them when the code is working
upload a photo of the project?

pkness · August 2, 2024, 11:39pm

Was happening on Mega as well, I moved to nano because its easier to breadboard and I was hoping it was a Mega issue, it was not, the issue persists.

Time to fail is random, I've had it as low as about 2 minutes and as high as around 10, but usually its around 6-7.
The system is currently being powered by usb from my laptop, however I don't believe this is a power issue, as I know the system can run for longer periods of time (I've tested up to two hours iirc), and if it was a power issue I don't think the system would just stop, it would restart, right?

I don't know how to generate a wiring diagram, but basically its
Nano D10 - MCP2515 CS
Nano D11 - MCP2515 MOSI
Nano D12 - MCP2515 MISO
Nano D13 - MCP2515 CLK
Nano 5V - MCP2515 VIN
Nano GND - MCP2515 GND

Nano 3.3V - LMS6DS3 VIN
Nano GND - LMS6DS3 GND
Nano A4 - LMS6DS3 SDA
Nano A5 - LMS6DS3 SCL

And here's a photo

pkness · August 2, 2024, 11:40pm

And yeah, the CAN is not plugged into the MCP in the photo, I was testing something else out, but usually the can is plugged in (and works and everything)

The IMU also works

gilshultz · August 3, 2024, 3:20am

Post an annotated schematic, not a wiring diagram showing exactly how you have wired it. Be sure to show all power sources, grounds, external parts such as caps etc. Post links to each of the hardware items. If you can post a clear picture of it set up.

paulpaulson · August 3, 2024, 3:30am

Hello pkness

I have done a quick check of the code:

Do the array indexes always match the size of the arrays?

system · January 30, 2025, 3:30am

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Arduino Nano stürzt immer wieder ab Deutsch	116	799	July 26, 2025
T-motor Cubemars AK 80-64 arduino Spakfun CAN bus communication Programming	36	8212	January 15, 2026
ESP32 with MPU6500 Sensors	25	1185	January 8, 2026
CAN Bus : MCP2515 with Mega Showing Configuration Mode Failure Programming	15	370	January 26, 2026
MPU-6050 Accelerometer Values Stuck Sensors	11	82	November 26, 2025

Arduino Hanging/Crashing When using both LSM6DS3TRC and MCP2515

Related topics