LSM9DS1 and balancing math

24

Comments

  • I have been using a spreadsheet to verify the calculations so far. I will dig out one of my P1 boards when I get back from work, run the code you have posted, and list the results for each step to see where things go off the rails.
    In science there is no authority. There is only experiment.
    Life is unpredictable. Eat dessert first.
  • kwinn wrote: »
    I have been using a spreadsheet to verify the calculations so far. I will dig out one of my P1 boards when I get back from work, run the code you have posted, and list the results for each step to see where things go off the rails.

    Well, that was a bit optimistic. Got the board connected, copied your posted code, and found I was missing several of the objects you used. Downloaded the ones I needed and started modifying the code so I could substitute keyboard input for accelerometer readings. Looks like it will take another evening or two to finish.
    In science there is no authority. There is only experiment.
    Life is unpredictable. Eat dessert first.
  • Here is the current code with all accompanying files.
  • Thanks, that will help. Will get to it asap, but inevitably, as soon as you start a project other more urgent tasks come along. At least the weekend is not too far away.
    In science there is no authority. There is only experiment.
    Life is unpredictable. Eat dessert first.
  • I appreciate it, I'm at a wall trying to figure this out
  • Jwolf wrote: »
    I appreciate it, I'm at a wall trying to figure this out

    Unfortunately I am now in Edmonton trying to get a customer's equipment running, and it looks like I may be here for a few more days.
    In science there is no authority. There is only experiment.
    Life is unpredictable. Eat dessert first.
  • hate to bug ya, hopin youll have lil time soon tho :)
    I have tried everything... cannot find any other obex for atan2, the one I have does not produce the results it should... online atan2 calculators do produce correct results though.

  • dgatelydgately Posts: 1,095
    edited 2019-02-13 - 21:17:47
    The FME object in OBEX appears to give a good result (I think!)... Seems to almost match the result I get compiling a C program on my macOS system.
    CON
            _clkmode        = xtal1 + pll16x
            _xinfreq        = 5_000_000
    OBJ
            term    : "FullDuplexSerialPlus"
            fp      : "FloatString"
            ff      : "FME"
    VAR
      long ax, ay
    PUB start | i
      'start the terminal
      term.startPST(115200)
      'print startup 
      term.str(string("Float atan2 Demo...",13))
    
      fp.SetPrecision(3)
      term.str(string("PI Ratio (180 / PI): "))
      term.str(fp.FloatToString(ff.FDiv(180.00, PI)))
      term.Tx(13)
    
      ax := -7    ' test integer inputs, could be floats returned from other functions
      ay := 7
      
      term.str(string("Radians: "))
      term.str(fp.FloatToString(ff.atan2(ff.FFloat(ay), ff.FFloat(ax))))
      term.Tx(13)
     
      term.str(string("Degrees: "))
      ' must use FDiv to multiply PI ratio by atan2 result (i.e. don't use '*')
      term.str(fp.FloatToString(ff.FMul(ff.FDiv(180.0, PI),ff.atan2(ff.FFloat(ay), ff.FFloat(ax)))))
      term.Tx(13)
    
    Displays:
    Float atan2 Demo...
    PI Ratio (180 / PI): 57.3
    Radians: 2.36
    Degrees: 135
    
    C code:
    #include <stdio.h>
    #include <math.h>
    #define PI 3.14159265
    
    int main () {
       double x, y, ret, val;
       x = -7.0;
       y = 7.0;
       val = 180.0 / PI;
    
       ret = atan2 (y,x) * val;
       printf("The arc tangent of x = %lf, y = %lf ", x, y);
       printf("is %lf degrees\n", ret); 
       return(0);
    }
    Result:
    $ ./a.out 
    57.295780 ratio
    The arc tangent of x = -7.000000, y = 7.000000 is 2.356194 radians
    The arc tangent of x = -7.000000, y = 7.000000 is 135.000000 degrees
    

    dgately
    Livermore, CA (50 miles SE of San Francisco)
  • Jwolf wrote: »
    hate to bug ya, hopin youll have lil time soon tho :)
    I have tried everything... cannot find any other obex for atan2, the one I have does not produce the results it should... online atan2 calculators do produce correct results though.

    Sorry, still in Edmonton. Hate to start helping someone and then let them down. Hoping to resolve it soon and get back home, but it has already taken way more time than anticipated. Now trying to find a vacuum leak on the system that has 80+ potential leak prone areas and no quick way to locate the culprit.
    In science there is no authority. There is only experiment.
    Life is unpredictable. Eat dessert first.
  • JwolfJwolf Posts: 71
    edited 2019-02-16 - 04:27:11
    dgate... can you help with finding the FME obex? Im not sure what FME is an acronym for and nothing shows up when searching 'FME'
    doh nm... it was included with the mpu-9150 driver :)
    Ill give it a try!
  • JwolfJwolf Posts: 71
    edited 2019-02-16 - 04:51:47
    kwinn, u workin on a plc system?
    for a pressurized link, use soapy water and look for bubbles....
    for vacuum, use a rubber bulb with baby powder or chalk... if too windy / not poss... use a simple vacuum gauge on each tube till u find the leak.

    Im available for contract help :blush:
  • JwolfJwolf Posts: 71
    edited 2019-02-16 - 07:03:30
    dgately: It works! Beautifully!
    I'm getting a +/- 180deg result, and I was able to free up 2 cogs :)

    So what I'm seeing is: the result of atan2(x,y) needs to be multiplied by the results of (180/pi)
    Online Atan2 calculators seem to automatically include this
  • Jwolf wrote: »
    dgate... can you help with finding the FME obex? Im not sure what FME is an acronym for and nothing shows up when searching 'FME'
    doh nm... it was included with the mpu-9150 driver :)
    Ill give it a try!

    "Float Math Extended" http://obex.parallax.com/object/710
    Infernal Machine
  • Jwolf wrote: »
    kwinn, u workin on a plc system?
    for a pressurized link, use soapy water and look for bubbles....
    for vacuum, use a rubber bulb with baby powder or chalk... if too windy / not poss... use a simple vacuum gauge on each tube till u find the leak.

    Im available for contract help :blush:

    No, it's not a PLC system, it's an ARL Spectrometer (3460 metal analyzer). Customer left it on the foundry floor exposed to large temperature and humidity changes for several weeks. Not a good thing to do to an instrument with delicate optics and electronics.

    On top of that they replaced a plastic fitting with a brass one that created a short and damaged the source electronics. This is a 30 year old instrument so a lot of the parts are now obsolete, and I had to find suitable replacements for them. Took a lot of time and searching.

    Now I just have to find the leak(s) that are preventing the optical chamber from getting down to the 10-20 millitorr range it needs for proper operation. Replacing "O"rings on a total of 87 possible leak sites today, then pump down overnight and hope for a good vacuum.
    In science there is no authority. There is only experiment.
    Life is unpredictable. Eat dessert first.
  • Jwolf wrote: »
    So what I'm seeing is: the result of atan2(x,y) needs to be multiplied by the results of (180/pi)
    Online Atan2 calculators seem to automatically include this
    Yes, that calculates degrees from radians...

    Glad it worked!
    Livermore, CA (50 miles SE of San Francisco)
  • JwolfJwolf Posts: 71
    edited 2019-02-17 - 05:25:25
    kwinn, sounds like a fun job if u ask me :)
    I worked with lasers for years, got undercut by china tho and biz tapered so had to shut down.
    please let me know if theres any work or components you need outsourced, i still have stock of dichroics, splitters, collimators, drivers and at least 200 diodes (IR & visible)... i design and cnc my own psu's & ttl/analog modulation drivers.
  • So now that I'm getting a usable value, I need to simply control the flight surfaces, which I plan to do with servos.
    I need a servo with a decent response time and indexing... being able to accurately position the servo right on startup is important to me.
    What kind of servo do I need? I have a few analog servos, but they do not have any feedback on position (that i know of)
    do I need something like a digital coreless?
  • Jwolf wrote: »
    kwinn, sounds like a fun job if u ask me :)
    I worked with lasers for years, got undercut by china tho and biz tapered so had to shut down.
    please let me know if theres any work or components you need outsourced, i still have stock of dichroics, splitters, collimators, drivers and at least 200 diodes (IR & visible)... i design and cnc my own psu's & ttl/analog modulation drivers.

    I guess it is fun for some definitions of fun. Definitely a challenge. Good to see that you solved your challenge with the balancing math.

    Appreciate the offer of parts and help, but the optical and electronics problems have been repaired. All I need to do now is solve the poor vacuum problem. It could be a leak, bad pump, bad pressure gauge, or a final electronics failure. Will tackle that in the morning.
    In science there is no authority. There is only experiment.
    Life is unpredictable. Eat dessert first.
  • so would you say... that job sucks? waka waka ;)
  • LOL - Taking in to consideration that I am stuck in a motel room when I am not working, and that the local temperature varies from -17C and -30C, yes, this job sucks any way you look at it.
    In science there is no authority. There is only experiment.
    Life is unpredictable. Eat dessert first.
  • JwolfJwolf Posts: 71
    edited 2019-04-06 - 02:40:49
    Been having fun playing with the accel, provides great results for basic balancing on a slow moving unit.
    however, as I was previously warned, fast movements skew the accelerometer... a quick motion laterally makes it think it's being tipped when it is not.
    So now I need to integrate the gyro.

    I have not yet attempted this... but my initial thought is to simply subtract gyro output from accel output, then convert to degrees.
    Am I on the right track?
  • JwolfJwolf Posts: 71
    edited 2019-04-06 - 07:44:36
    well that didnt work at all lol.
    I was hoping the gyro data would show very little or no change on the axis that is being moved laterally since it does not experience pitch/roll.... but i get a good bit of change on all 3 gyro axis.
    I dont get how to go about linking accel & gyro data

    It seems to me that to combine these...
    Since the gyro reads rate of rotation per axis, and the accel reads amount of gravity per axis...
    the imu would have to begin stationary, accel data be used to calculate current orientation, then gyro data be the primary source of movement calculation, subtracted by accel data to get rid of false-positive roll/pitch.. or something like that lol
    However, since a simple lateral motion shows change on all axis for both gyro and accel... ???

    for clarification, by lateral movement I mean a forward motion on one axis... like a cd-rom closing/opening.
    I thought that lateral movement directly along one gyro axis, should not show any rate-of-change on that axis
  • Welcome to my world or not. Been there did that don't want to do that again.

    Well I guess the answer is that you want to fuse them such that when the object is not moving you use the accelerometer and when movement is detected that you use the gyro.

    If you use the gyro only and setup an accumulator of the changes in angle per second it is quite accurate. But after a while the angle will drift off and need to be re-centered. That's where the accelerometer comes in.

    So that where a complementary filter or a Kalman filter are used to build that relationship.

    Mike
  • JwolfJwolf Posts: 71
    edited 2019-04-07 - 10:47:22
    Well heres a nice find... however I lack the C skills :(
    Maybe I can extract the formulas. Is there a way to port this to spin?
  • ShawnaShawna Posts: 437
    edited 2019-04-07 - 14:16:15
    I think the complementary filter is the way to go. It is not the best way to fuse the gyro and accelerometer data together, but I think that it is the easiest to understand. For a balancing bot, I think it should work just fine.

    I am also working on a balancing bot.

    Here are a couple of links that explain the problems with using just a gyro, or just an accelerometer. The author of the video is using the arduino. I think that the arduino code is written in C or C++. In the first video he explains how to determine the angle from a gyro and its drawbacks. In the second video he talks about the accelerometer, its drawbacks, and how to fuse the two sensors together using a complementary filter. During the coarse of both videos he shows and explains the math to accomplish the fusion of the sensors. I learned a lot from them.

    He explains the concepts very well and makes the math easy to understand.





    The authors name is Joop Brokking, he has a website with his projects. He built a balancing bot using math from the previous two videos. His YouTube channel has two videos explaining most of the code required to control a balancing bot.

    brokking.net/yabr_main.html

    Hope it helps.

    Shawn
  • JwolfJwolf Posts: 71
    edited 2019-04-22 - 05:11:50
    great vid on how to go about converting gyro to a roll/pitch output and implementing a complimentary filter...
    However I'm having a bit of trouble with getting the gyro converted to a usable pitch/roll output due to timing on the propeller.

    I'm getting dps output from the imu, but I need to divide that by the amount of time passed.
    im running at 80mhz, so I planned to simply track time by the amount of cnt ticks passed divded by 80mil...
    when that went haywire, I realized that cnt was rolling through the limits of a long (-2,147,483,648 to 2,147,483,647).
    So it works fine when the cnt values are positive... but when it rolls over its a disaster

    So either theres a better way.... or
    I should record current cnt, compare to previous cnt, if less than, subtract previous cnt from 2,147,483,647, subtract current cnt from -2,147,483,648, multiply current cnt result by -1 to convert to positive value, add previous cnt result with current cnt result to get total amount of ticks passed, divide by 80,000,000 = time passed
    seems excessive... ideas?

    edit: crap actually that wont work if both cnt values are negative... so i'd have to add another check to see if both values are negative or only one.
    so, something like this...
    if currentCNT > 0 and previousCNT > 0
    domathA
    elseif currentCNT > 0 and previousCNT < 0
    domathB
    elseif currentCNT < 0 and previousCNT < 0
    domathC
  • JwolfJwolf Posts: 71
    edited 2019-04-22 - 08:33:59
    Figured out time with this:
    Time1 := cnt 'Record end of cycle
        
          'Determine if timers are pos or neg and perform math (cnt value ranges from -2,147,483,648 to 2,147,483,647)
          if Time1 => 0 and Time2 => 0        'If start time and end time pos
             Time3 := Time1 - Time2
          elseif Time1 > 0 and Time2 < 0    'If start time pos and end time neg
             Time1 -= 2147483647
             Time2 := ((-2147483648 - Time2) * -1)
             Time3 := Time1 + Time2
          elseif Time1 =< 0 and Time2 =< 0    'If start and end time neg
             Time3 := ((Time2 - Time1) * -1)
    
        Time2 := cnt 'Record start of cycle
             
        Time4 := ff.FDiv(Time3, 80000000)  'Find how much time has passed
        long[TimerG] := Time4     'Send to PST Display
    

    But I'm still not getting a usable output on gyro.
    One thing I just cant figure out... from that arduino video above, here is one of the timing calculations, but it doesnt add up:
    //Gyro angle calculations
      //0.0000611 = 1 / (250Hz / 65.5)
      angle_pitch += gyro_x * 0.0000611;                                   //Calculate the traveled pitch angle and add this to the angle_pitch variable
      angle_roll += gyro_y * 0.0000611;                                    //Calculate the traveled roll angle and add this to the angle_roll variable
    
    Im showing:
    1/(250/65.5) = 0.262
    so where is 0.0000611 coming from?

    I was going to try this method... the dps for the LSM9 is 17.5 (would replace the 65.5 in above calc)
  • The way the information is provided is miss leading. What he is saying is that the number is divided by 250 hz and then by 65.5 or 1/250 * 1/65.5. This is (250 * 65.5) or 1/16375.

    Mike

  • @Jwolf
    Your are right, you need to keep track of your time between gyro readings so that you can calculate how much rotation has occurred around each axis per reading. I like to keep track of the time by using loops that run at a constant speed or frequency.

    Like this
    while(1){ //Repeat forever loop.
    loop_start = CNT; //Preloads system counter value to keep track of loop time.

    //Code, Do something once every 10mS.

    waitcnt(loop_start + ((CLKFREQ / 100) - 144)); //Keeps track of while loop guaranteeing dt. 100hz
    }

    When the dT is constant you no longer have to calculate it every loop. The delta T (dT) of the loop above is .01mS.
    When using a loop like this you need to make sure you can get everything done within the time frame or the waitcnt pause will miss its exit point and the program will hang.
    Jwolf wrote: »
    1/(250/65.5) = 0.262
    so where is 0.0000611 coming from?

    I was going to try this method... the dps for the LSM9 is 17.5 (would replace the 65.5 in above calc)

    Yeah that is a little confusing.

    I haven't reworked my code since finding the video, too much to do outside, thank God spring is here! Last winter was getting old!

    This is what I think he is doing.

    He is using the MPU 6050 configured for 65.5 LSB(Degress/Second). 65.5 would be the multiplier if he was reading his gyro once every second. Since he is reading his gyro 250 times a second his new multiplier is .0000611.

    (1/250) / 65.5 = .0000611

    or

    .004mS / 65.5 = .0000611

    So his dT is 1/250 = .004mS

    or working it backwards

    .004/.0000611 = 65.5

    Since his loop is running at a constant 250Hz he is pre-calculating math that will never change, this way he can save processor cycles.

    You could replace the 65.5 with the 17.5 if and only if your loop is running at a constant 250hz.
    So far I have not been able to come even close to this kind of speed. It would require PASM. I am currently running at 100Hz syncing 3 cogs.

    Shawn

  • Shawna wrote: »
    You could replace the 65.5 with the 17.5 if and only if your loop is running at a constant 250hz.

    I misspoke, you cannot just replace the 65.5 with 17.5. The 65.5 is in degrees per second and the 17.5 is in millidegrees per second.

Sign In or Register to comment.