Shop OBEX P1 Docs P2 Docs Learn Events
LSM9DS1 and balancing math — Parallax Forums

LSM9DS1 and balancing math

JwolfJwolf Posts: 74
edited 2018-12-06 12:05 in Robotics
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!
«1345

Comments

  • 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



  • good info, thanks very much!
    Will get back to ya when I have time to absorb that into the ol'melon :smile:
  • JwolfJwolf Posts: 74
    edited 2019-01-08 19:28
    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
  • kwinnkwinn Posts: 8,697
    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.
  • JwolfJwolf Posts: 74
    edited 2019-01-09 10:22
    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
  • kwinnkwinn Posts: 8,697
    edited 2019-01-09 23:53
    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.
  • JwolfJwolf Posts: 74
    edited 2019-01-14 10:20
    Im sorry but that isnt even close.
    Visibly, its more around 20deg
  • 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!
  • JwolfJwolf Posts: 74
    edited 2019-01-14 22:10
    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?
  • 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
  • JwolfJwolf Posts: 74
    edited 2019-01-14 23:35
    ok using atan2 from float32full....
    atan2(0.4, 0.9) returns 1054220629

    what is goin on here?
  • 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?

  • dgatelydgately Posts: 1,621
    edited 2019-01-15 00:46
    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
    
  • 1054220629 = $3ED62155 which is the floating point representation of 0.418223 which is the
    arctan of 0.4/0.9
  • JwolfJwolf Posts: 74
    edited 2019-01-15 04:08
    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
    
  • JwolfJwolf Posts: 74
    edited 2019-01-15 04:01
    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
    
  • 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
  • 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

  • kwinnkwinn Posts: 8,697
    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.
  • how come when i use atan2, it returns like a 12 digit number?
  • Most of those functions are coded for radians and not degrees. You need to multiple by PI and divide by 180

    Mike
  • Mathematical functions are always in radians, never degrees, note. You are expected to know the 180/π
    conversion factor by heart once you use trig.
  • JwolfJwolf Posts: 74
    edited 2019-01-19 07:53
    so... take the raw result, convert to milli-G, use atan2, multiply by 180, then divide by pi?
    That gives = 123041749546.46190664773073918307
  • Is there anyone who can show how to convert either the raw output or milli-G, to degrees?
  • 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

  • kwinnkwinn Posts: 8,697
    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.
  • JwolfJwolf Posts: 74
    edited 2019-01-20 08:36
    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?
  • JwolfJwolf Posts: 74
    edited 2019-01-20 12:15
    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
  • kwinnkwinn Posts: 8,697
    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?
  • JwolfJwolf Posts: 74
    edited 2019-01-21 12:25
    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
Sign In or Register to comment.