multi sensor program crash

Hello all,

As part of my final protect at the university I need to measure vehicle’s change in angle and speed simultaneously. inside the hub motor of the vehicle there is a hall sensor that is rising 14 times every revolution and for the angles change I use MPU6050 with complementary filter. I modified two codes I found on the internet to get the measurements I need. The code seems to work at the beginning (I get the speed and angles correct via serial port) but when I try to make the experiment the program crashes for no apparent reason. Trying to debug the problem I have connected an LED to check if the program crashes or the serial port stop working an it is defiantly the code because when the transmitting data get stacked the led also stops blinking.

i hope to find some help here…

that is the code:

/***** EXPERAMENT 1 : ANGLES VS. WHEELS SPEED *****

BOARD: ARDUINO UNO

SENSORS:

  1. MPU 6050 (GYROSCOPE & ACCELEROMETER)
  2. HALL SENSOR ENCODER (INSIDE THE HUB MOTOR)

CONECITONS:

  1. MPU 6050:
    VCC->5V
    GND->GND
    SCL->A5
    SDA->A4

  2. HALL SENSOR:
    VCC->5v
    GND->GND
    HALL 1->2
    */

//***************** includes ***************** //
#include “Wire.h” // This library allows you to communicate with I2C devices.
#include <TimerOne.h>

//***************** global variables HALL SENSOR ***************** //
volatile float t = 0;
volatile float t_prev = 0;
volatile float delta_t = 0;
volatile double RPS = 0;

//***************** global variables MPU6050 ***************** //
const int MPU_ADDR = 0x68; // I2C address of the MPU-6050. If AD0 pin is set to HIGH, the I2C address will be 0x69.

struct
{
volatile long accel_x;
volatile long accel_y;
volatile long accel_z;

volatile float gyro_x;
volatile float gyro_y;
volatile float gyro_z;
} sensor_data;

struct {
volatile long time;
float roll;
float pitch;
float yaw;
} curr_angle;

struct {
volatile long time;
float roll;
float pitch;
float yaw;
} last_angle;

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

pinMode(13, OUTPUT);

/* Extract result to Excell */
Serial.println(“CLEARDATA”);
Serial.println(“LABEL,Current time, Roll angle (deg) , Speed (RPS)”);

/* HALL SENSOR */
attachInterrupt(0, working, RISING);

/* MPU6050 */
Wire.begin();
setupMPU();
clibrate_sensor();
}

void loop() {

/* MPU6050 */
recordAccelRegisters();
recordGyroRegisters();

unsigned long t_now = millis();

// Convert gyro values to degrees/sec
float FS_SEL = 131;
volatile float gyro_x = sensor_data.gyro_x / FS_SEL;
volatile float gyro_y = sensor_data.gyro_y / FS_SEL;
volatile float gyro_z = sensor_data.gyro_z / FS_SEL;

// Get raw acceleration values
volatile float G_CONVERT = 16384;
volatile float accel_x = sensor_data.accel_x / G_CONVERT;
volatile float accel_y = sensor_data.accel_y / G_CONVERT;
volatile float accel_z = sensor_data.accel_z / G_CONVERT;

// Get angle values from accelerometer
float RADIANS_TO_DEGREES = 180 / 3.14159;
volatile float accel_angle_y = atan(-1 * accel_x / sqrt(pow(accel_y, 2) + pow(accel_z, 2))) * RADIANS_TO_DEGREES;
volatile float accel_angle_x = atan(accel_y / sqrt(pow(accel_x, 2) + pow(accel_z, 2))) * RADIANS_TO_DEGREES;
volatile float accel_angle_z = atan(sqrt(pow(accel_x, 2) + pow(accel_y, 2)) / accel_z) * RADIANS_TO_DEGREES;

// Compute the (filtered) gyro angles
volatile float dt = (t_now - last_angle.time) / 1000.0;
volatile float gyro_angle_x = gyro_x * dt + last_angle.roll;
volatile float gyro_angle_y = gyro_y * dt + last_angle.pitch;
volatile float gyro_angle_z = gyro_z * dt + last_angle.yaw;

// Apply the complementary filter
float alpha = 0.96;
volatile float roll = alpha * gyro_angle_x + (1.0 - alpha) * accel_angle_x;
volatile float pitch = alpha * gyro_angle_y + (1.0 - alpha) * accel_angle_y;
volatile float yaw = alpha * gyro_angle_z + (1.0 - alpha) * accel_angle_z;

// set current data to be the last
set_last_read_angle_data(t_now, roll, pitch, yaw);

Serial.print(“DATA,TIME,”);
Serial.print(roll);
Serial.print(" ");
Serial.println(RPS);

// Delay so we don’t swamp the serial port
delay(5);

digitalWrite(LED_BUILTIN, LOW);
}

//*********** functions declaration HALL SENSOR ************************//
void working() {
t = micros();
delta_t = t - t_prev;
t_prev = t;
if (delta_t > 1 ) {
RPS = ( (10 / 7) / delta_t ) * 100000;
digitalWrite(LED_BUILTIN, HIGH);
}
}

//pirnt result
//Serial.print(“DATA,TIME,”);
//Serial.println(roll);
//Serial.print(’,’);
//Serial.println(RPS);

//*********** functions declaration MPU6050 ************************//

void setupMPU() {
Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
Wire.write(0b00000000); //Setting the accel to +/- 2g
Wire.endTransmission();
}

void clibrate_sensor() {
set_last_read_angle_data(millis(), 0, 0, 0);
}

void recordAccelRegisters() {
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x3B); //Starting register for Accel Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000, 6); //Request Accel Registers (3B - 40)
while (Wire.available() < 6);
sensor_data.accel_x = Wire.read() << 8 | Wire.read(); //Store first two bytes into accelX
sensor_data.accel_y = Wire.read() << 8 | Wire.read(); //Store middle two bytes into accelY
sensor_data.accel_z = Wire.read() << 8 | Wire.read(); //Store last two bytes into accelZ
}

void recordGyroRegisters() {
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x43); //Starting register for Gyro Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000, 6); //Request Gyro Registers (43 - 48)
while (Wire.available() < 6);
sensor_data.gyro_x = Wire.read() << 8 | Wire.read(); //Store first two bytes into accelX
sensor_data.gyro_y = Wire.read() << 8 | Wire.read(); //Store middle two bytes into accelY
sensor_data.gyro_z = Wire.read() << 8 | Wire.read(); //Store last two bytes into accelZ
}

void set_last_read_angle_data(unsigned long time, float x, float y, float z) {
last_angle.time = time;
last_angle.roll = x;
last_angle.pitch = y;
last_angle.yaw = z;
}

Thanks for the help

Probably not the problem, but: why are you declaring local variables as volatile? Why using volatile at all? No ISR in sight.

"the transmitting data get stacked" - what does that mean?

"the transmitting data get stacked" - what does that mean?

I mean that the problem is not with the serial communication but in the program itself.