LSM9DS1 and balancing math

Jwolf · 2018-12-04 03:06

Hello,
I have the LSM9 working with raw outputs... but im having trouble with the math.
How do I link the gyro + accel to create a stable orientation output?
I just want to get this to work so it can be used for any type of project, whether it be an rc aircraft, drone, balancing, or general heading output.

I'm using the Elev8 source from Jesse Burt.
Accel outputs at rest output +16,400 to -16,400, increased output due to g-forces... I 'think' its reading at 400hz
gyro is about 2x that when rotated quickly.
I guess I need a formula to deal with 3 different accelero axi's correlated with each other, minus g-forces, checked against 3 different gyro axi's, at the sampled rate.

I've been studying for a while now on basic concept... (((rate of change) - noise) / units)) = amount of correction needed to stay level in specified units

Cannot figure out how to make the LSM9 output usable.... please help!

amalfiCoast · 2018-12-04 04:02

Hello Jwolf,

There are two main filters you can use to fuse gyroscope and accelerometer measurements - the extended Kalman filter and the complementary filter. The complementary filter is far simpler to implement but does not account for system dynamics so it will not work well for fast moving vehicles such as fixed wing aircraft. For balancing applications, the complementary filter works fine. It can be implemented in code by:

accelAngle = atan2(ax,az);
 theta = 0.98*(theta+gy*Ts)+.02*accelAngle;

where accelAngle is the angle computed from the accelerometer, ax is the acceleration value in g's in the x direction, az is the acceleration value in g's in the z direction, theta is the pitch angle you are estimating, gy is the gyroscope output about the y axis, and Ts is the loop sample time. You may have to map your sensor outputs according to a north, east, down coordinate system where north is in the x direction and points out of the front of your vehicle, east is in the y direction and points out the right wing of your vehicle, and down is in the z direction and points out of the bottom of your vehicle towards earth. This way, if you are estimating theta (pitch), the gy measurement is about the the y axis. Once you are able to estimate theta, you can easily implement an estimation of phi (roll).

See my thread here: http://forums.parallax.com/discussion/169330/is-there-a-faster-way-to-read-sensor-data#latest
if you are interested in implementing the extended Kalman filter. Oh and make sure you are using the LMM main RAM memory model. Your code will run about 5 times faster than if you use the CMM RAM memory model. It took me a while to figure this out. If you have any questions, just ask and I'll try to help.

Thanks,
David

Jwolf · 2018-12-06 04:22

good info, thanks very much!
Will get back to ya when I have time to absorb that into the ol'melon

Jwolf · 2019-01-08 18:59

ok still working on trying to make use of the data from gyro/accel.
The kalman filter in your above post, which is in C, is a bit too wild for me to follow.

// Extended Kalman filter implmentation
    ///////////////////// 
    
    // Do Prediction Step, note: filter is not using accel measurements at this point, only gyro, so it drifts
    startTime = CNT; // Remember starting time
   
    int N = 1;
    for(int i = 0; i < N; i++){
      
    ////////////////////
    // State derivatives
    float f[2] = {
      {gy+gx*sin(phi)*tan(theta)+gz*cos(phi)*tan(theta)},
      {gx*cos(phi)-gz*sin(phi)}
    };  
   
    //////////////////////
    // Calculate xhat
    // xhat = xhat + (Ts/N)*f
    xhat[0]=xhat[0]+(delta/N)*f[0];
    xhat[1]=xhat[1]+(delta/N)*f[1];
    phi=xhat[0];
    theta=xhat[1];           
    
    delta = (CNT-startTime)/80000000; // Number of elapsed clock cycles divided by 80MHz clock
  } 
//    print("delta = %f\n",delta);

    // Do Update Step
    /////////////////////
    // Declare vector of actual measurements
    float y[3][1] = {
      {ax*9.81},
      {ay*9.81},
      {az*-9.81}
    };  
    ////////////////////
    // Define mathematical model of measurements based on xhat
    float h[3][1] = {
      {gx*Va*sin(theta)+gravity*sin(theta)},
      {gz*Va*cos(theta)-gy*Va*sin(theta)-gravity*cos(theta)*sin(phi)},
      {-gx*Va*cos(theta)-gravity*cos(theta)*cos(phi)}
    };
    
    ///////////////////
    //xhat=xhat+L*(y-h)
    xhat[0]=xhat[0]-L[0][0]*(h[0][0]-y[0][0])-L[0][1]*(h[1][0]-y[1][0])-L[0][2]*(h[2][0]-y[2][0]);
    xhat[1]=xhat[1]-L[1][0]*(h[0][0]-y[0][0])-L[1][1]*(h[1][0]-y[1][0])-L[1][2]*(h[2][0]-y[2][0]);          
    phi=xhat[0];   //Pitch
    theta=xhat[1]; //Roll
    
    float Error=theta*180/PI;
    float Error_int = Error_int+dt*Error;
    float servo=kp*Error+ki*Error_int+kd*gx*180/PI;
     
    if(servo<0){
      //Motor 1 (right motor)
      high(0);
      low(1);
      pwm_set(2,0,-servo+motorOffset+3);
      //Motor 2 (left motor)
      high(3);
      low(4);
      pwm_set(5,1,-servo+motorOffset);
    }
    else if(servo>0){
      //Motor 1 (right motor)
      high(1);
      low(0);
      pwm_set(2,0,servo+motorOffset+3);
      //Motor 2 (left motor)
      high(4);
      low(3);
      pwm_set(5,1,servo+motorOffset);
    }           
    
    dt = (CNT-start)/80000000; // Number of elapsed clock cycles divided by 80MHz clock
    print("%f\n",theta*180/PI);  
//    print("dt = %f\n",dt);  
  }

I cannot figure out how to port this to spin, nor can i figure out what in the world is goin on.
math like this in C, which is unusable in spin and shows no basic intuition, blows my mind: xhat[0]=xhat[0]-L[0][0]*(h[0][0]-y[0][0])-L[0][1]*(h[1][0]-y[1][0])-L[0][2]*(h[2][0]-y[2][0]);
please dont think i only need clarification on this one function... this is just a prime example to illustrate the level of confusion im dealin with... like why are you multiplying all axis by 9.81 in there?

I must have read a dozen or two in-depth kalman filter studies, but I was unable to extract any usable info on how to get the data output from accel/gyro, translated to degrees or a scaled output.
when I first started on this i thought the output would be a simple linear scale... like a ball in a bowl, tip or move in any direction an it would be a simple scale... but I found that its more like a ball in a box with only 3 points being truly discernible (0 for level, max output value equals completely vertical, and max output value times .71 equals 45deg angle).
I cannot find a math function to create an angle from these output values. Pythagorean doesnt work with the raw outputs, nor with output converted to milli g's

I'm lost right now..
Let say i just want to do something as simple as take these accel readings and turn them into a single angle to tell how far the Z-axis is from being level.
With the Y-axis being orientated up, raw output values at 'level': Y=16,400, X=0, Z=0
X = 0
Y = +15,000
Z = + 6,500

Exactly how many degrees is the Z-axis tipped? Assume Z-axis is 0deg at level/horizontal, 90deg when vertical, 180deg when flipped over... so a 360deg scale
*I may be mistaken on the max output value at rest... I think instead of 16,400 it may actually be 16,480?... i forgot how to calculate max on a 16bit chip

kwinn · 2019-01-09 02:43

Jwolf wrote: »

ok still working on trying to make use of the data from gyro/accel.
The kalman filter in your above post, which is in C, is a bit too wild for me to follow.

// Extended Kalman filter implmentation
    ///////////////////// 
    
    // Do Prediction Step, note: filter is not using accel measurements at this point, only gyro, so it drifts
    startTime = CNT; // Remember starting time
   
    int N = 1;
    for(int i = 0; i < N; i++){
      
    ////////////////////
    // State derivatives
    float f[2] = {
      {gy+gx*sin(phi)*tan(theta)+gz*cos(phi)*tan(theta)},
      {gx*cos(phi)-gz*sin(phi)}
    };  
   
    //////////////////////
    // Calculate xhat
    // xhat = xhat + (Ts/N)*f
    xhat[0]=xhat[0]+(delta/N)*f[0];
    xhat[1]=xhat[1]+(delta/N)*f[1];
    phi=xhat[0];
    theta=xhat[1];           
    
    delta = (CNT-startTime)/80000000; // Number of elapsed clock cycles divided by 80MHz clock
  } 
//    print("delta = %f\n",delta);

    // Do Update Step
    /////////////////////
    // Declare vector of actual measurements
    float y[3][1] = {
      {ax*9.81},
      {ay*9.81},
      {az*-9.81}
    };  
    ////////////////////
    // Define mathematical model of measurements based on xhat
    float h[3][1] = {
      {gx*Va*sin(theta)+gravity*sin(theta)},
      {gz*Va*cos(theta)-gy*Va*sin(theta)-gravity*cos(theta)*sin(phi)},
      {-gx*Va*cos(theta)-gravity*cos(theta)*cos(phi)}
    };
    
    ///////////////////
    //xhat=xhat+L*(y-h)
    xhat[0]=xhat[0]-L[0][0]*(h[0][0]-y[0][0])-L[0][1]*(h[1][0]-y[1][0])-L[0][2]*(h[2][0]-y[2][0]);
    xhat[1]=xhat[1]-L[1][0]*(h[0][0]-y[0][0])-L[1][1]*(h[1][0]-y[1][0])-L[1][2]*(h[2][0]-y[2][0]);          
    phi=xhat[0];   //Pitch
    theta=xhat[1]; //Roll
    
    float Error=theta*180/PI;
    float Error_int = Error_int+dt*Error;
    float servo=kp*Error+ki*Error_int+kd*gx*180/PI;
     
    if(servo<0){
      //Motor 1 (right motor)
      high(0);
      low(1);
      pwm_set(2,0,-servo+motorOffset+3);
      //Motor 2 (left motor)
      high(3);
      low(4);
      pwm_set(5,1,-servo+motorOffset);
    }
    else if(servo>0){
      //Motor 1 (right motor)
      high(1);
      low(0);
      pwm_set(2,0,servo+motorOffset+3);
      //Motor 2 (left motor)
      high(4);
      low(3);
      pwm_set(5,1,servo+motorOffset);
    }           
    
    dt = (CNT-start)/80000000; // Number of elapsed clock cycles divided by 80MHz clock
    print("%f\n",theta*180/PI);  
//    print("dt = %f\n",dt);  
  }

I cannot figure out how to port this to spin, nor can i figure out what in the world is goin on.
math like this in C, which is unusable in spin and shows no basic intuition, blows my mind: xhat[0]=xhat[0]-L[0][0]*(h[0][0]-y[0][0])-L[0][1]*(h[1][0]-y[1][0])-L[0][2]*(h[2][0]-y[2][0]);
please dont think i only need clarification on this one function... this is just a prime example to illustrate the level of confusion im dealin with... like why are you multiplying all axis by 9.81 in there?

I am not an expert on Kalman filters or self balancing robots I am pretty sure multiplying by 9.81 is a conversion of forces from Kilograms to Newtons. Makes sense to convert to the standard measurement for force.

I must have read a dozen or two in-depth kalman filter studies, but I was unable to extract any usable info on how to get the data output from accel/gyro, translated to degrees or a scaled output.
when I first started on this i thought the output would be a simple linear scale... like a ball in a bowl, tip or move in any direction an it would be a simple scale... but I found that its more like a ball in a box with only 3 points being truly discernible (0 for level, max output value equals completely vertical, and max output value times .71 equals 45deg angle).
I cannot find a math function to create an angle from these output values. Pythagorean doesnt work with the raw outputs, nor with output converted to milli g's

I'm lost right now..
Let say i just want to do something as simple as take these accel readings and turn them into a single angle to tell how far the Z-axis is from being level.
With the Y-axis being orientated up, raw output values at 'level': Y=16,400, X=0, Z=0
X = 0
Y = +15,000
Z = + 6,500

Exactly how many degrees is the Z-axis tipped? Assume Z-axis is 0deg at level/horizontal, 90deg when vertical, 180deg when flipped over... so a 360deg scale
*I may be mistaken on the max output value at rest... I think instead of 16,400 it may actually be 16,480?... i forgot how to calculate max on a 16bit chip

The angle of tilt for any axis can be calculated using trig functions and Pythagoras Theorem using calibrated (raw x correction) values or values converted to newtons (or any other measurement of force). To calculate how far the Z axis is tipped in relation to vertical is also calculated using trig functions and Pythagoras.

Jwolf · 2019-01-09 09:07

The angle of tilt for any axis can be calculated using trig functions and Pythagoras Theorem using calibrated (raw x correction) values or values converted to newtons (or any other measurement of force). To calculate how far the Z axis is tipped in relation to vertical is also calculated using trig functions and Pythagoras.

Can you please provide proof by answering my question?
I have translated the raw output values from above, to G-force so you can elaborate with the Trig functions and Pythagoras theorem to provide an angle in degrees.

X= 0
Y= .910
Z= .400

kwinn · 2019-01-09 14:16

Jwolf wrote: »

The angle of tilt for any axis can be calculated using trig functions and Pythagoras Theorem using calibrated (raw x correction) values or values converted to newtons (or any other measurement of force). To calculate how far the Z axis is tipped in relation to vertical is also calculated using trig functions and Pythagoras.

Can you please provide proof by answering my question?
I have translated the raw output values from above, to G-force so you can elaborate with the Trig functions and Pythagoras theorem to provide an angle in degrees.

X= 0
Y= .910
Z= .400

I am assuming the Y axis is the vertical axis of the robot. Is that correct?

Edit to provide answer:

When using G force the Y reading is the sin() of the angle, and the Z is the cos()

Looking up 0.910 gives an angle of 66 degrees, as does the 0.400 cos() value.
Gets a little trickier when you have both x and y non-zero values since both affect the y angle.

Jwolf · 2019-01-14 10:19

Im sorry but that isnt even close.
Visibly, its more around 20deg

Mark_T · 2019-01-14 11:06

No Pythagorus, just use atan2() for this:

atan2(0.4, 0.91) = 26.4 degrees.

Looking up 0.910 gives an angle of 66 degrees, as does the 0.400 cos() value.

Nope, no combination of those values gives 66 degrees!

Jwolf · 2019-01-14 22:09

atan2 does not appear to be a valid spin command... and it doesnt appear to be in integer.spin either.
How do I go about using atan2 in SPIN?

dgately · 2019-01-14 22:21

The OBEX contains a floating point library for spin & PASM...

obex.parallax.com/object/233

IEEE-754 compliant Propeller floating point math package for functions callable by both Spin and assembler code. Contains Float32, Float32Full, Float32A and documentation. (V1.6 - updated Exp, Exp10, Pow, and added documentation to Spin code)

dgately

Jwolf · 2019-01-14 23:32

ok using atan2 from float32full....
atan2(0.4, 0.9) returns 1054220629

what is goin on here?

dgately · 2019-01-15 00:08

Jwolf wrote: »

ok using atan2 from float32full....
atan2(0.4, 0.9) returns 1054220629

what is goin on here?

Give an example of how you are using the library?

dgately · 2019-01-15 00:26

Remember, Spin's vars are 32-bit integers... The atan2 & Degrees functions return floating point responses, not integers. You'll need to read the Propeller Floating Point PDF that comes with the OBEX Floating Point Library object, to decide how to use the returned values.

CON
  x = 0.4
  y = 0.9
OBJ
  fp  : "Float32Full.spin"
  ser : "FullDuplexSerialPlus"
  FTS : "FloatString"
VAR
  long rad
  long deg
PUB Main
  ser.StartPST(115_200)
  fp.Start
  ser.Str(string("radians: "))
  rad := fp.atan2(x, y)
  ser.Str(FTS.FloatToString(fp.atan2(x, y)))  
  ser.Str(string(" var x radians: "))
  ser.Str(FTS.FloatToString(rad))  
  ser.CarriageReturn
  
  ser.Str(string("degrees: "))
  deg := fp.Degrees(fp.atan2(x, y))
  ser.Str(FTS.FloatToString(fp.Degrees(fp.atan2(x, y))))
  ser.Str(string(" var y degrees: "))
  ser.Str(FTS.FloatToString(deg))
  ser.CarriageReturn

Simple IDE Terminal returns:    radians: 0.4182231 var x radians: 0.4182231
degrees: 23.96242 var y degrees: 23.96242

Mark_T · 2019-01-15 00:32

1054220629 = $3ED62155 which is the floating point representation of 0.418223 which is the
arctan of 0.4/0.9

Jwolf · 2019-01-15 03:51

First let me reiterate that I am simply trying to make use of the LSM9 imu to get usable data in the form of degrees for use with balancing and/or a flight control system.
So right now im trying to get the current angle in degrees that it is tipped fwd/bck/lft/rght from the accelerometer.... thats it.
unfortunately there are no good resources to learn how to do this... every knowledge source I have read, is in individual pieces of interest which always use unrelated and unusable examples.
So I unfortunately have to do this from scratch. I really wish there was a single formula for use with IMU's to get results in deg from an IMU with accel/gyro output.
Seems to me the best place to start is with simply getting angles based on 'stable' orientation (0 g's representing 'stable' except for the one axis on vertical (Y in this case))...
then later on, use gyroscope outputs to increase accuracy.
The accelerometer outputs raw values from +16,480 to -16,480. included in the LSM9 IMU object from 'Jesse Burt' in OBEX is a way to translate to milli-g's.
The output from the milli-g routine gives a result in one thousandths of a G-force... ie 1000 = 1G

So just to try and get some working math, I have provided the raw outputs along with converted milli-g outputs of a static position.
RAW
X = 0
Y = +15,000
Z = + 6,500

Milli-G
X= 0
Y= .910
Z= .400

So far, Mark_T you have been the closest to providing an actual step forward... but, I am completely unfamiliar with atan2, and have been unable to duplicate your results.
Here is my current code... what am I doing wrong?
Thanks for the help, this is a real lonely project

CON
  _clkmode = xtal1 + pll16x
  _xinfreq = 5_000_000
  SCL_PIN       = 1
  SDIO_PIN      = 2
  CS_AG_PIN     = 3
  CS_M_PIN      = 4
  INT_AG_PIN    = 5
  INT_M_PIN     = 6
  DISP_Balance  = 1
   
OBJ      
  ser       : "com.serial.terminal"
  time      : "time"
  imu       : "sensor.imu.tri.lsm9ds1"
  int       : "string.integer"
  fp        : "float32full"
  
VAR
  long  _keyDaemon_stack[100]
  byte  _keyDaemon_cog, _imu_cog, _ser_cog
  byte  _demo_state
  byte  _max_cols
  long  RollDeg, PitchDeg, ax, ay, az


PUB Main
 _max_cols := 4
 fp.start                               ' Start Floating point math cogs
 waitcnt(clkfreq * 3 + cnt)             ' Delay to allow terminal to be loaded
 _ser_cog := ser.Start (115_200)        ' Start Serial Terminal
 ser.Clear                              ' Clear Screen
  
 _imu_cog := imu.Start (SCL_PIN, SDIO_PIN, CS_AG_PIN, CS_M_PIN, INT_AG_PIN, INT_M_PIN)  ' Launch LS9 Driver into new cog (launched through imu OBJ)
 VerifyImu              ' Verify IMU Started Successful
 imu.setMagScale (16)   ' Set Magnetomoter Scale
 DisplayBalance         ' Start Balancing Routine                      

PUB VerifyImu

   if imu.whoAmI <> $683D               ' Verify LSM9DS1 Startup Successful
      ser.Str (string("LSM9DS1 not found - halting!", ser#NL))
      imu.Stop     ' Stop IMU cog
      ser.Stop     ' Stop Serial cog
      Repeat       ' Dead loop

PUB DisplayBalance  |  gx, gy, gz
  Repeat
    ser.Clear

    ' Raw Output
    ser.Str (string("Display RAW Accel data:", ser#NL))
    imu.readAccel (@ax, @ay, @az)
    ser.Str (string("AX: "))
    ser.Str (int.DecPadded(ax, 6))
    ser.Str (string("  AY: "))
    ser.Str (int.DecPadded(ay, 6))
    ser.Str (string("  AZ: "))
    ser.Str (int.DecPadded(az, 6))

    ' G-Force Output
    ser.Str (string(ser#NL))
    ser.Str (string("Display Calculated Accel data (milli-g's):", ser#NL))  ' Y-Axis orientated up
    imu.readAccelCalculated (@ax, @ay, @az)
    ser.Str (string("AX: "))
    ser.Str (int.DecPadded (ax, 6))
    ser.Str (string("  AY: "))
    ser.Str (int.DecPadded (ay, 6))
    ser.Str (string("  AZ: "))
    ser.Str (int.DecPadded (az, 6))

    ' Adjust to degrees
    ser.Str (string(ser#NL))
    ser.Str (string("Roll: "))
    RollDeg := fp.Degrees(fp.atan2(ax, ay))
    ser.Str (int.DecPadded (RollDeg, 6))
    ser.Str (string("deg    ")) 
    
    ser.Str (string(ser#NL))
    ser.Str (string("Pitch: "))
    PitchDeg := fp.Degrees(fp.atan2(az, ay))
    ser.Str (int.DecPadded (PitchDeg, 6))
    ser.Str (string("deg    "))
    
    time.MSleep (100)

DAT
  
  ag_regmap  byte 54
  byte $04, "ACT_THS         ", 0
  byte $05, "ACT_DUR         ", 0
  byte $06, "INT_GEN_CFG_XL  ", 0
  byte $07, "INT_GEN_THS_X_XL", 0
  byte $08, "INT_GEN_THS_Y_XL", 0
  byte $09, "INT_GEN_THS_Z_XL", 0
  byte $0A, "INT_GEN_DUR_XL  ", 0
  byte $0B, "REFERENCE_G     ", 0
  byte $0C, "INT1_CTRL       ", 0
  byte $0D, "INT2_CTRL       ", 0
  byte $0F, "WHO_AM_I_XG     ", 0
  byte $10, "CTRL_REG1_G     ", 0
  byte $11, "CTRL_REG2_G     ", 0
  byte $12, "CTRL_REG3_G     ", 0
  byte $13, "ORIENT_CFG_G    ", 0
  byte $14, "INT_GEN_SRC_G   ", 0
  byte $15, "OUT_TEMP_L      ", 0
  byte $16, "OUT_TEMP_H      ", 0
  byte $17, "STATUS_REG_0    ", 0
  byte $18, "OUT_X_L_G       ", 0
  byte $19, "OUT_X_H_G       ", 0
  byte $1A, "OUT_Y_L_G       ", 0
  byte $1B, "OUT_Y_H_G       ", 0
  byte $1C, "OUT_Z_L_G       ", 0
  byte $1D, "OUT_Z_H_G       ", 0
  byte $1E, "CTRL_REG4       ", 0
  byte $1F, "CTRL_REG5_XL    ", 0
  byte $20, "CTRL_REG6_XL    ", 0
  byte $21, "CTRL_REG7_XL    ", 0
  byte $22, "CTRL_REG8       ", 0
  byte $23, "CTRL_REG9       ", 0
  byte $24, "CTRL_REG10      ", 0       
  byte $26, "INT_GEN_SRC_XL  ", 0
  byte $27, "STATUS_REG_1    ", 0     
  byte $28, "OUT_X_L_XL      ", 0       
  byte $29, "OUT_X_H_XL      ", 0       
  byte $2A, "OUT_Y_L_XL      ", 0       
  byte $2B, "OUT_Y_H_XL      ", 0       
  byte $2C, "OUT_Z_L_XL      ", 0       
  byte $2D, "OUT_Z_H_XL      ", 0       
  byte $2E, "FIFO_CTRL       ", 0        
  byte $2F, "FIFO_SRC        ", 0         
  byte $30, "INT_GEN_CFG_G   ", 0    
  byte $31, "INT_GEN_THS_XH_G", 0 
  byte $32, "INT_GEN_THS_XL_G", 0 
  byte $33, "INT_GEN_THS_YH_G", 0 
  byte $34, "INT_GEN_THS_YL_G", 0 
  byte $35, "INT_GEN_THS_ZH_G", 0
  byte $36, "INT_GEN_THS_ZL_G", 0 
  byte $37, "INT_GEN_DUR_G   ", 0    
  byte $00, "                ", 0

  m_regmap  byte 24
  byte $05, "OFFSET_X_REG_L_M", 0 
  byte $06, "OFFSET_X_REG_H_M", 0 
  byte $07, "OFFSET_Y_REG_L_M", 0 
  byte $08, "OFFSET_Y_REG_H_M", 0 
  byte $09, "OFFSET_Z_REG_L_M", 0 
  byte $0A, "OFFSET_Z_REG_H_M", 0 
  byte $0F, "WHO_AM_I_M      ", 0       
  byte $20, "CTRL_REG1_M     ", 0      
  byte $21, "CTRL_REG2_M     ", 0      
  byte $22, "CTRL_REG3_M     ", 0      
  byte $23, "CTRL_REG4_M     ", 0      
  byte $24, "CTRL_REG5_M     ", 0      
  byte $27, "STATUS_REG_M    ", 0     
  byte $28, "OUT_X_L_M       ", 0        
  byte $29, "OUT_X_H_M       ", 0        
  byte $2A, "OUT_Y_L_M       ", 0        
  byte $2B, "OUT_Y_H_M       ", 0        
  byte $2C, "OUT_Z_L_M       ", 0        
  byte $2D, "OUT_Z_H_M       ", 0        
  byte $30, "INT_CFG_M       ", 0        
  byte $30, "INT_SRC_M       ", 0        
  byte $32, "INT_THS_L_M     ", 0      
  byte $33, "INT_THS_H_M     ", 0      
  byte $00, "                ", 0

Jwolf · 2019-01-15 03:58

Here is a screenshot of the current output while using that code

Display RAW Accel data:
AX:    -73  AY:  15632  AZ:   5836
Display Calculated Accel data (milli-g's):
AX:     -5  AY:    946  AZ:    350
Roll: 2147483647deg
Pitch: 1101163885deg

iseries · 2019-01-15 12:10

I think you have a lot of work ahead of you.

None of the solutions are gong to be written in SPIN.

Balancing math is much simpler that copter math. In copter math they use Quaternions to represent the position of the copter in 3D space. Using that model they can determine the Roll, Pitch and Yaw of the copter to make adjustments.

You can use Quaternions to balance a robot, and some examples do that.

Simply stated if you use the accelerometer and the object is not moving the math works and you can determine the angle that the robot is at. If however the robot is moving then you need to use the gyro's to determine at what rate the object is moving at and subtract it for the accelerometer to get the corrected angle.

I for one have not been able to get this to works using a number of different builds and I used C examples from a number of these robots.

There are some chips out there that have the math built into the chip so that all you need to do is read the values and determine the angle. I have a BNO055 chip but have not used it yet. I'm kind of burnt out on this right now.

Mike

iseries · 2019-01-15 12:36

Also the best balancing robot that I have used is the one from JJrobots. It's fast and works great. It will even pick itself up when it falls over. All open source and very inexpensive. I even put snow tires on it and drove it around in the snow. I 3D printed the frame. Snow tires extra.

Here is a video link of it in action
Mike

kwinn · 2019-01-15 20:04

Mark_T wrote: »

No Pythagorus, just use atan2() for this:

atan2(0.4, 0.91) = 26.4 degrees.

Looking up 0.910 gives an angle of 66 degrees, as does the 0.400 cos() value.

Nope, no combination of those values gives 66 degrees!

You are right, that angle is wrong. I am getting the vertical/horizontal axis mixed up. Also , I am not combining the two readings, just using the cos() of one and sin() of the other to look up the angle. With the correct axis cos of 0.910 is between 24 and 25 degrees, while sin of 0.400 is between 23 and 24 degrees, which is close to what using the atan2 result produces.

While it is possible to do these types of calculations using trig I have to admit that using the atan2 function makes them so much easier. Thanks for the correction and the enlightenment.

Jwolf · 2019-01-15 21:17

how come when i use atan2, it returns like a 12 digit number?

iseries · 2019-01-15 22:33

Most of those functions are coded for radians and not degrees. You need to multiple by PI and divide by 180

Mike

Mark_T · 2019-01-16 14:17

Mathematical functions are always in radians, never degrees, note. You are expected to know the 180/π
conversion factor by heart once you use trig.

Jwolf · 2019-01-19 07:43

so... take the raw result, convert to milli-G, use atan2, multiply by 180, then divide by pi?
That gives = 123041749546.46190664773073918307

Jwolf · 2019-01-19 07:55

Is there anyone who can show how to convert either the raw output or milli-G, to degrees?

iseries · 2019-01-19 11:18

Ok, reading the specs on an LSM9DS1 it output an accelerometer reading of +-16393.

Now if you set the accel to 1g scale that means that a reading of 16393 = 1g of force.

Doing the ATAN2(AX,AZ) gives you the angle of the accelerometer. bearing in mind that you need to calibrate the accelerometer on a flat surface before you use it.
So with the unit sitting flat on the table the AX should return close to 0 and AZ will be close 16393 unless you have it upside down then it would be -16393.

So if AX = 5 and AZ = 16393 then ATAN2 returns 1.570491 or 89.98 degrees.

Mike

kwinn · 2019-01-19 17:58

I have attached a spreadsheet with a brief explanation and the calculations from one of your earlier posts. Just change the extension from txt to xls to use it.

Jwolf · 2019-01-20 08:31

kwinn, i dont have excel installed

iseries:
"ATAN2 returns 1.570491 or 89.98 degrees."
When I use atan2 like this (atan2(5,16393)), i get a result of 966840320

I am using atan2 from obex: http://obex.parallax.com/object/233 in file 'Float32Full.spin'
how come you get a valid result when I'm a few hundred million degrees off?
Is that a bad obex?

Jwolf · 2019-01-20 08:55

oh interesting... when i use an online calculator, I also get 1.57deg... also, when I put 966840320 in an ATAN calculator... it returns 1.57deg
but of course when I try to apply 966840320 to ATAN in spin from that obex... I once again get a multi-hundred million result.
I'm guessing at this point I need to look for another Atan2 function obex... its as if this one only performs half of the calculations

kwinn · 2019-01-20 14:45

Jwolf wrote: »

kwinn, i dont have excel installed

I have attached a PDF of the spreadsheet.

PS - You could install LibreOffice. Open source, free, and an excellent office suite. It's what I use.

"ATAN2 returns 1.570491 or 89.98 degrees."
When I use atan2 like this (atan2(5,16393)), i get a result of 966840320

I am using atan2 from obex: http://obex.parallax.com/object/233 in file 'Float32Full.spin'
how come you get a valid result when I'm a few hundred million degrees off?
Is that a bad obex?

Are you converting the integer values from the accelerometers to floating point before calling the atan2 subroutine?

Jwolf · 2019-01-21 05:40

all variables declared as longs... used FloatMath to convert before calling atan2, still get same results.
another thing that make me think atan2 isnt working properly... if the accel value is a negative, atan2 returns -1

LSM9DS1 and balancing math

Comments