Figuring out how to get the difference between 2 values

Projects Programming
1

Hi there,

Stupid simple title I know, but I'm having issues with finding the difference between 2 values that I am reading from 2 sensors. I am getting correct values for the angles of each sensor (with respect to gravity) but no matter the values, my code is outputting a 0 value and I'm not quite sure why or how to fix it.

I've tried moving things from different loops, and declaring variables in different places but something isn't working as I intend it to.

I've included below the code, the libraries for the 2 sensors (MPU9250 & MPU6050), as well as a screencap of the serial monitor output.

Thanks in advance for any help or tips,

Chris

#include "Wire.h"
#include "Math.h"
#include "I2Cdev.h"
#include "MPU9250.h"
#include "MPU6050.h"


MPU9250 One;
MPU6050 Two(0x69);

I2Cdev I2C_M;

uint8_t buffer_m[6];

//Initializes the variables for sensors 1 & 2
int16_t ax1, ay1, az1;
int16_t gx1, gy1, gz1;
int16_t mx1, my1, mz1;
int16_t ax2, ay2, az2;
int16_t gx2, gy2, gz2;
int16_t mx2, my2, mz2;
//Initializes the arrays for sensors 1 & 2
float OneAxyz[3];
float TwoAxyz[3];
float Gxyz[3];
float Mxyz[3];
float Oneangle[3];
float Twoangle[3];
float kneeangle[3];
//#define sample_num_mdate  5000 

//set up
void setup() 
{
  //initializes the 2 sensors
  One.initialize();
  Two.initialize();
  // join I2C bus
  Wire.begin();
  // initialise serial communication
  // sets baud rate
  Serial.begin(38400);
}

void loop() 
{
 // calls the values
 getAccel_DataOne();
 getAccel_DataTwo();
 getAngleOne();
 getAngleTwo();
 //prints the values
 Serial.println("Angle 1");
 Serial.println(Oneangle[1]);
 Serial.println(" ");
 Serial.println("Angle 2");
 Serial.println(Twoangle[1]);
 Serial.println(" ");
 Serial.println("Knee Angle");
 Serial.println(kneeangle[1]);
 Serial.println(" ");
 // delay between each value 
 delay(3000);
}

//Sensor One Address: 0x68

void getAccel_DataOne(void)
{
  One.getMotion9(&ax1, &ay1, &az1, &gx1, &gy1, &gz1, &mx1, &my1, &mz1);
  OneAxyz[1] = (double) ay1 / 16384;
}

void getAngleOne(void)
{
  getAccel_DataOne();
  float angleyone = acos(OneAxyz[1]);
  Oneangle[1] = (double) 180 * angleyone / PI;
}


//Sensor Two Address: 0x69

void getAccel_DataTwo(void)
{
  Two.getMotion9(&ax2, &ay2, &az2, &gx2, &gy2, &gz2, &mx2, &my2, &mz2);
  TwoAxyz[1] = (double) ay2 / 16384;
}
void getAngleTwo(void) //0x69
{
  getAccel_DataTwo();
  float angleytwo = acos(TwoAxyz[1]);
  Twoangle[1] = (double) 180 * angleytwo / PI;
}

void getKneeAngle(void)
{
  getKneeAngle();
  int kneeanglecalc = (Twoangle[1]-Oneangle[1]);
  kneeangle[1] = (double) kneeanglecalc;
}

Serial_monitor.PNG
Serial_monitor.PNG1040×789 22.2 KB

Grove_IMU_9DOF_9250.zip (65.8 KB)

MPU6050.zip (84.4 KB)

2

I can't see any attempt in your program to get the difference between any pair of numbers.

And there will be a big problem here if you call this function - because it then calls itself and will cause the Arduino to crash.

void getKneeAngle(void)
{
  getKneeAngle();
  int kneeanglecalc = (Twoangle[1]-Oneangle[1]);
  kneeangle[1] = (double) kneeanglecalc;
}

...R

3

Further to Robin2's comment, you have created a recursive function that calls itself over and over again until it the Arduino runs out of stack memory and then crashes/restarts.

You may have more success with

void getKneeAngle(void)
{
  int kneeanglecalc = (Twoangle[1]-Oneangle[1]);
  kneeangle[1] = (double) kneeanglecalc;
}
4 
  int kneeanglecalc = (Twoangle[1]-Oneangle[1]);
  kneeangle[1] = (double) kneeanglecalc;

I can't see any good reason for getting the difference between two floats, and storing that difference in an int variable that you then cast to a double to store in a float.

Paying attention to types IS important.

5

PaulS:

  int kneeanglecalc = (Twoangle[1]-Oneangle[1]);

kneeangle[1] = (double) kneeanglecalc;



I can't see any good reason for getting the difference between two floats, and storing that difference in an int variable that you then cast to a double to store in a float.

Paying attention to types IS important.

So write it as

  float kneeanglecalc = (Twoangle[1]-Oneangle[1]);
  kneeangle[1] = (double) kneeanglecalc;

?

6

What about not casting nor converting it....

7

septillion:
What about not casting nor converting it....

How do you mean? I'm pretty new to coding so don't really know how to correctly write it, i pretty much copied the formatting of the example sketch that came with the sensor library

8

From where did you get the idea to randomly insert word like "float" and "double"? Don't, do a little bit of reading. Look at some examples and tutorials. Granted, a lot are rubbish for most part but little "randomly" insert words without explaining it :wink:

9

So write it as

No.

kneeangle[1] = Twoangle[1] - Oneangle[1];

Yes.