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.
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.
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.
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.
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
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.
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!
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.
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
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!
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
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.
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
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?
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.
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.
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?
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.
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.
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: Smile 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
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.
@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.
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.
Comments
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.
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.
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.
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: 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 degreesdgately
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.
doh nm... it was included with the mpu-9150 driver
Ill give it a try!
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
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
"Float Math Extended" http://obex.parallax.com/object/710
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.
Glad it worked!
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 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?
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.
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?
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
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
Maybe I can extract the formulas. Is there a way to port this to spin?
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
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: Smile 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
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 DisplayBut 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: 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)
Mike
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
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.
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
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.