The set speed being positive or negative shouldn't matter - you'll need to be able to output positive or negative values to balance properly, so I'd check to see if you have an overflow somewhere that's causing a sign flip or something like that. Could also be a >> instead of a ~> (signed vs unsigned, in Spin).
Scaling will likely be necessary, but you could also just try limiting the output numbers to be within range of the servo speeds. You need to react quickly to small changes, so you can't scale down too much, so you want small changes to produce movement. Limit the values sent to the motor driver so they can't exceed the min/max servo range, and then play with the scale before you clamp the numbers to tweak sensitivity.
It's also possible that your servos just aren't responsive enough, or that the main loop + servo lag is too slow. The default servo update rate is only 50hz, and if the servos themselves are slow you might need to try real motors.
I am using high speed servos from Parallax and want the bot to made from the parts in the ActivityBot kit.
It is also being programmed in Blocklyprop so there are some limitations on how much I can do in the
program. It is really close to balancing. I am pretty sure it can be done, I have to teach myself how to do it!
Ah, ok - the high speed ones might be enough to do the job. Steppers aren't known for being speed demons, and that's what I was using. I did have the (benefit?) of it being all in Spin or C++, so most of the limits were my own.
Is it worth measuring two accelerometer axes together to increase the accuracy of the resulting angle a bit? Right now I'm only measuring Y (which is parallel to ground), but I could compute an angle from both Y and X and then average those two together. It would introduce longer delay but should increase accuracy.
Accelerometer only measures force - you could, in theory, compute the angle from only one force measurement if you assume that gravity == 1.0 and use ACos / ASin, but that won't tell you which direction you're leaning in, and it'll break if you are ever subject to a force greater than gravity, which you will be.
The way it's usually done is using Atan2(forward, down), which will get you an angle with sign, and handle arbitrary force values.
You're welcome. You should look into doing a complimentary filter - that will likely help a lot too.
The basic idea is that an accelerometer is extremely noisy, so it can't be trusted in the short term, but over the long term, the average angle will always be "down" because of gravity. Gyroscopes aren't subject to nearly as much noise, so in the short term they're very accurate, but over the long term they drift, and they don't give you an absolute angle, just a relative one.
The complimentary filter combines the two, and lets you choose which one you trust more (usually the gyro).
To use it, you have to have both measurements in the same units (or compatible units at least). In pseudo code, it'd look like this (the code below assumes everything is in floating point, but fixed point can work too):
float runningAngle = 0.0f;
const float AccelMix = 0.02f; // 2% accelerometer - only trust it a little
const float GyroMix = (1.0f - AccelMix); // 98% gyro
// Read once on startup to get a good zero reading - probably want to take an average of a bunch of readings
gyroZero = ReadGyroX();
while(1)
{
gyro = ReadGyroX() - gyroZero;
accy = ReadAccelY();
accz = ReadAccelZ();
gyroDegrees = (gyro / UpdatesPerSecond) * GyroUnitsToDegrees;
accelDegrees = Atan2(accy, accz) * RadiansToDegrees;
// GyroDegrees is a relative measure of how many degrees we've rotated since the last gyro update
runningAngle = runningAngle + gyroDegrees;
// Mix in a little bit of the accelerometer angle to keep it from drifting
runningAngle = (runningAngle * GyroMix) + (accelAngle * AccelMix);
waitcnt( CNT + ClockFreq / UpdatesPerSecond);
}
You're welcome. You should look into doing a complimentary filter - that will likely help a lot too.
The basic idea is that an accelerometer is extremely noisy, so it can't be trusted in the short term, but over the long term, the average angle will always be "down" because of gravity. Gyroscopes aren't subject to nearly as much noise, so in the short term they're very accurate, but over the long term they drift, and they don't give you an absolute angle, just a relative one.
The complimentary filter combines the two, and lets you choose which one you trust more (usually the gyro).
To use it, you have to have both measurements in the same units (or compatible units at least). In pseudo code, it'd look like this (the code below assumes everything is in floating point, but fixed point can work too):
float runningAngle = 0.0f;
const float AccelMix = 0.02f; // 2% accelerometer - only trust it a little
const float GyroMix = (1.0f - AccelMix); // 98% gyro
// Read once on startup to get a good zero reading - probably want to take an average of a bunch of readings
gyroZero = ReadGyroX();
while(1)
{
gyro = ReadGyroX() - gyroZero;
accy = ReadAccelY();
accz = ReadAccelZ();
gyroDegrees = (gyro / UpdatesPerSecond) * GyroUnitsToDegrees;
accelDegrees = Atan2(accy, accz) * RadiansToDegrees;
// GyroDegrees is a relative measure of how many degrees we've rotated since the last gyro update
runningAngle = runningAngle + gyroDegrees;
// Mix in a little bit of the accelerometer angle to keep it from drifting
runningAngle = (runningAngle * GyroMix) + (accelAngle * AccelMix);
waitcnt( CNT + ClockFreq / UpdatesPerSecond);
}
Thanks. Already doing that though . If you'd like though, I would love a code review of my sensor reader and angle computer cogs (SensorReader.h and AngleComputer.h).
The first thing that jumps out is this:
int16_t individualReadings[ACCEL_AVERAGING_BUFFER_LENGTH][3];
By putting the [3] on the end, you're setting your memory layout like this:
[x0, y0, z0], [x1, y1, z1], [x2, y2, z2], ...
That means to access x0, x1, x2, ... you need to multiply your index by 3, and the Prop doesn't have hardware multiply.
If you change it to:
int16_t individualReadings[3][ACCEL_AVERAGING_BUFFER_LENGTH];
...it means that accessing a sequence of x, y, or z values is just your index plus an offset, which will probably be faster. (3 is a special case of multiply, because (X*3) == (X<<1) + X, which is pretty quick anyway)
Also, your ADXL::Scale() function, I'm assuming, returns a double (or float?). That means you're doing two floating point scales per value, but you could fold them together:
(X / Y) == X * (1/Y)
(X / Y) / Z == (X * (1/Y) * (1/Z) == X * (1/(Y*Z)) or X / (Y*Z)
Basically if you fold your scale factors together, you only have to do one floating point multiply (or divide) instead of two, per accel axis that you're scaling. If ADXL345::scale() and the factor are const / inline the compiler might be doing this for you already.
I would try with and without the high pass filter. I didn't like the output I was getting with it enabled.
You shouldn't use doubles on the Prop unless you have the "doubles are actually floats" flag set in the compiler. (I'm not even sure if the Prop supports doubles, but check it - if it does, you'd be spending a TON of extra time on precision you don't need). All functions like atan2, sin, cos, etc have versions with 'f' post-fixed to the name - atan2f, sinf, cosf - those are float precision versions of them and they cost less. Again, not sure if the Prop supports doubles, but if it does...
I'd reduce the accel weight - 20% is really high and will make you subject to a lot of noise. 2% is closer to the sweet spot.
Your accel buffer being 16 in length is probably overkill. It means that your accel readings are going to lag by almost 1/5th of a second, and the complimentary filter is doing a chunk of that work for you anyway. I run my accelerometer / gyro readings at 500hz (or higher) and do the signal filtering in that space, so the lag is lower. I'd try 4 or 8, but with the accel weight set to 2% - I'd expect the result to be more responsive.
static double scale (const int value, const Range range) {
return value * (1 << range) * FULL_RESOLUTION_SCALE;
}
...is going to convert to everything to float or double because of promotion. The optimizer might do a better job, but you should check to make sure it's doing the right thing, IE:
int temp = value * (1<<range);
return temp * FULL_RESOLUTION_SCALE;
I'd just verify that if you implement it the second way it doesn't change anything in the timings or code size of your result. If you can generate a code listing, have a look to see that the two routines end up the same (or really close). It *might* be doing this:
Oohh - I just noticed that you're doing rapid reads in a loop of the accelerometer, and averaging *those*. You may be getting 10 identical readings, depending on how fast your loop is. I'd time that to see.
The way my code has historically worked is I run the entire loop at 250hz, and each iteration through that loop does one reading from accel, gyro, etc, puts them at the next index in the avg buffer, computes the current average, moves on. If your read loop is reasonably fast, you may be exceeding the ability of the accel to feed you unique readings.
I set the accelerometer for a 3,200 Hz sampling frequency and put into FIFO mode. Therefore, reading 16 samples every 10ms should give me completely unique results for each and every read.
Re: doubles on prop
I tried both with and without the -m32-bit-doubles option enabled and actually saw very little difference in code size (300 bytes) and I don't remember seeing any performance difference (I log the time it takes for each iteration of the loop). I'll quadruple check that after dinner tonight though.
Re: fancy math
That will take more brain cells to comprehend than I have to spare before dinner
David and Jason, you two keep this conversation going I am learning a lot of stuff from this!
Had to put my balancer aside for some other obligations but will be back to it soon and will be
using some of what you guys are doing.
David and Jason, you two keep this conversation going I am learning a lot of stuff from this!
Had to put my balancer aside for some other obligations but will be back to it soon and will be
using some of what you guys are doing.
Glad you're enjoying it. I was a bit worried because I didn't want to hijack your thread with my own project.
Jason,
I dropped the accelerometer weight to 2% and then graphed my angle value over time as it is sitting on the desk in front of me. It's depressing!!!
So that's a peak-to-peak range of 0.2 degrees, which doesn't seem reasonable to me at all. Each point on the x-axis is approximately 30ms apart. I zoomed in on the graph to find out the frequency and its around 1.6 Hz. I compared that with the DPS value coming out of the gyro (post averaging) and then compared the two graphs and the waves were a perfect match. This variation is clearly the gyro's fault. With the jitter at such a low frequency, it does seem like any amount of averaging or filter will be capable of resolving this because it's way out of the scope of the loop, even if I did a rolling filter like you.
Is this normal? Is it possible to get the kind of stability you demonstrated in your video with this kind of error? Or do I need to continue tweaking the L3G settings? Or a different gyro altogether?
I've double checked the float v. double performance. There is indeed a significant difference (as I'm sure you knew ). Here's some numbers (in microseconds):
Each data point is the time it takes for a single loop in a cog. I'm getting data on three different cogs:
1) Sensor reader. This guy reads from the gyro and accel over 900kHz SPI bus, does some averaging of multiple values, and spits out three numbers (raw data from two accel. axes and DPS from the gyro)
2) Angle computer. This guy takes the three inputs from the sensor reader and computes an angle
3) Logger. This guy logs a done of data to the serial console at 460,800 baud, including the four values listed above
Response rate certainly does improve dramatically with both the bit-width and memory model! Ranging from 15ms at the worst to 3.3ms at best (ignoring the Logger cog). Interestingly, the computed angle actually changes with the memory model too. The graph above is based on data from 64-bit, cmm code. However, 64-bit lmm hits an average angle of more like 1.5! I haven't done a deep-dive into those numbers yet to find out what is causing the discrepancy.
0.2 degrees isn't a lot, but it does seem higher than it should be. Do you have anything mechanical on this thing, or near it? Is it sitting on a table near anything that hums? From the graph it looks like it's resonant with something, but I'd expect that more from the accelerometer than the gyro.
Try turning off *all* filtering, and just emit raw readings. Graph those and see how they compare to what you're seeing now. If those are reasonably stable then the problem isn't the device.
As for the computed angle changing, that might make sense - if you expect your loop to run at 100 Hz, but it actually runs slower than that, you won't be accumulating angle readings often enough.
This is what you expect:
1 degree per second * 0.01sec * 100 updates gives 1 degree.
...but if you're running half as fast as you think you are, this is what you'll get:
1 degree per second * 0.01sec * 50 updates is only 0.5 degrees.
If your accumulation loop runs slow, your readings will be low.
I'll try turning off the averaging in software and high-pass filter in hardware. I can't turn off the low-pass filter though - I can only change the frequency.
So long as each cog is running faster than 10ms, I'm okay. Even in the worst case of 64-bit cmm, it has 100 us left to spin in its waitcnt2 routine in the SensorReader cog. That's why I referenced "response rate" instead of "refresh rate" when referring to the performance.
The high-pass one is the one I think needs to go - I've never enabled it, and suspect it'll cause
You suspect it will cause incomplete sentences?
I can't remember exactly which graph that is that I posted last night, but I did try turning the high pass filter off and it didn't help any. However, setting it to 3.5 Hz did help a bit. I realize though that these results could have very different end results once mounted on a chassis that is moving about, so I'll play more with it later. And as you suggested, tonight I'll try turning off the high-pass filter and removing the averaging buffer so that I can inspect the raw data and see what kind of results I get.
I haven't looked at outputs from the FC for a while, and I don't know that I ever graphed "degrees" specifically to see how stable it was, but I'll see if I can do that over the next week or so. Within the last month we got a puppy, moved to a new house, my mom just visited for a week, and I'm attending a wedding this weekend, so things have been a little nuts.
If you whip up a quick test that does nothing more than dump raw gyro values and graph those it should point to where the issue is - it could be the physical hardware, though that seems unlikely. It could potentially be a power issue if you don't have enough decoupling on the chips, though that also seems unlikely. Running from a battery instead of a power supply would eliminate that pretty quickly.
Quite the exciting life! I appreciate your time on this even more now!
So I've written up a test that reads 500 samples over the course of two seconds (uses waitcnt2 to get the precise 250 Hz frequency). After all 500 have been read, it dumps them to an SD card. I ran the test twice: once powered by my computer, and second time powered by an 8-cell battery pack full of brand new Eneloop AAs (multimeter measuring 10.6V). The board is a DNA-RTC. I did try a third time with a 100uF capacitor between ground and 3V3 on the DNA's header and a second .1uF capacitor between ground and VCC on the L3G breakout board's header... it didn't help so I didn't bother with a screenshot.
I can't graph it yet (installing office) but my raw gyro readings when sitting on my bench are between -16 to -28. I've attached a dump of about 5 seconds worth (at 200/sec). There should be (1000/70) units per degree, or about 14.28, so I'm wandering a little less than a degree overall at rest, but it looks reasonably similar to yours.
Jason, you're results are significantly better than mine when graphed out. I set the scale so that there is a 4 DPS range on all three graphs to make sure the visual comparison is reasonable. It's immediately obvious that you're getting much cleaner results:
WOWZER! I just checked the datasheet and the L3G has a zero-rate level @ the 250dps range of ±10dps!!! I guess I should be pretty happy with the graph I'm seeing
Comments
Scaling will likely be necessary, but you could also just try limiting the output numbers to be within range of the servo speeds. You need to react quickly to small changes, so you can't scale down too much, so you want small changes to produce movement. Limit the values sent to the motor driver so they can't exceed the min/max servo range, and then play with the scale before you clamp the numbers to tweak sensitivity.
It's also possible that your servos just aren't responsive enough, or that the main loop + servo lag is too slow. The default servo update rate is only 50hz, and if the servos themselves are slow you might need to try real motors.
It is also being programmed in Blocklyprop so there are some limitations on how much I can do in the
program. It is really close to balancing. I am pretty sure it can be done, I have to teach myself how to do it!
Is it worth measuring two accelerometer axes together to increase the accuracy of the resulting angle a bit? Right now I'm only measuring Y (which is parallel to ground), but I could compute an angle from both Y and X and then average those two together. It would introduce longer delay but should increase accuracy.
The way it's usually done is using Atan2(forward, down), which will get you an angle with sign, and handle arbitrary force values.
The basic idea is that an accelerometer is extremely noisy, so it can't be trusted in the short term, but over the long term, the average angle will always be "down" because of gravity. Gyroscopes aren't subject to nearly as much noise, so in the short term they're very accurate, but over the long term they drift, and they don't give you an absolute angle, just a relative one.
The complimentary filter combines the two, and lets you choose which one you trust more (usually the gyro).
To use it, you have to have both measurements in the same units (or compatible units at least). In pseudo code, it'd look like this (the code below assumes everything is in floating point, but fixed point can work too):
float runningAngle = 0.0f; const float AccelMix = 0.02f; // 2% accelerometer - only trust it a little const float GyroMix = (1.0f - AccelMix); // 98% gyro // Read once on startup to get a good zero reading - probably want to take an average of a bunch of readings gyroZero = ReadGyroX(); while(1) { gyro = ReadGyroX() - gyroZero; accy = ReadAccelY(); accz = ReadAccelZ(); gyroDegrees = (gyro / UpdatesPerSecond) * GyroUnitsToDegrees; accelDegrees = Atan2(accy, accz) * RadiansToDegrees; // GyroDegrees is a relative measure of how many degrees we've rotated since the last gyro update runningAngle = runningAngle + gyroDegrees; // Mix in a little bit of the accelerometer angle to keep it from drifting runningAngle = (runningAngle * GyroMix) + (accelAngle * AccelMix); waitcnt( CNT + ClockFreq / UpdatesPerSecond); }Thanks. Already doing that though
int16_t individualReadings[ACCEL_AVERAGING_BUFFER_LENGTH][3];
By putting the [3] on the end, you're setting your memory layout like this:
[x0, y0, z0], [x1, y1, z1], [x2, y2, z2], ...
That means to access x0, x1, x2, ... you need to multiply your index by 3, and the Prop doesn't have hardware multiply.
If you change it to:
int16_t individualReadings[3][ACCEL_AVERAGING_BUFFER_LENGTH];
...it means that accessing a sequence of x, y, or z values is just your index plus an offset, which will probably be faster. (3 is a special case of multiply, because (X*3) == (X<<1) + X, which is pretty quick anyway)
Also, your ADXL::Scale() function, I'm assuming, returns a double (or float?). That means you're doing two floating point scales per value, but you could fold them together:
(X / Y) == X * (1/Y)
(X / Y) / Z == (X * (1/Y) * (1/Z) == X * (1/(Y*Z)) or X / (Y*Z)
Basically if you fold your scale factors together, you only have to do one floating point multiply (or divide) instead of two, per accel axis that you're scaling. If ADXL345::scale() and the factor are const / inline the compiler might be doing this for you already.
I would try with and without the high pass filter. I didn't like the output I was getting with it enabled.
You shouldn't use doubles on the Prop unless you have the "doubles are actually floats" flag set in the compiler. (I'm not even sure if the Prop supports doubles, but check it - if it does, you'd be spending a TON of extra time on precision you don't need). All functions like atan2, sin, cos, etc have versions with 'f' post-fixed to the name - atan2f, sinf, cosf - those are float precision versions of them and they cost less. Again, not sure if the Prop supports doubles, but if it does...
I'd reduce the accel weight - 20% is really high and will make you subject to a lot of noise. 2% is closer to the sweet spot.
Your accel buffer being 16 in length is probably overkill. It means that your accel readings are going to lag by almost 1/5th of a second, and the complimentary filter is doing a chunk of that work for you anyway. I run my accelerometer / gyro readings at 500hz (or higher) and do the signal filtering in that space, so the lag is lower. I'd try 4 or 8, but with the accel weight set to 2% - I'd expect the result to be more responsive.
static double scale (const int value, const Range range) { return value * (1 << range) * FULL_RESOLUTION_SCALE; }...is going to convert to everything to float or double because of promotion. The optimizer might do a better job, but you should check to make sure it's doing the right thing, IE:I'd just verify that if you implement it the second way it doesn't change anything in the timings or code size of your result. If you can generate a code listing, have a look to see that the two routines end up the same (or really close). It *might* be doing this:
The way my code has historically worked is I run the entire loop at 250hz, and each iteration through that loop does one reading from accel, gyro, etc, puts them at the next index in the avg buffer, computes the current average, moves on. If your read loop is reasonably fast, you may be exceeding the ability of the accel to feed you unique readings.
I set the accelerometer for a 3,200 Hz sampling frequency and put into FIFO mode. Therefore, reading 16 samples every 10ms should give me completely unique results for each and every read.
Re: doubles on prop
I tried both with and without the -m32-bit-doubles option enabled and actually saw very little difference in code size (300 bytes) and I don't remember seeing any performance difference (I log the time it takes for each iteration of the loop). I'll quadruple check that after dinner tonight though.
Re: fancy math
That will take more brain cells to comprehend than I have to spare before dinner
Had to put my balancer aside for some other obligations but will be back to it soon and will be
using some of what you guys are doing.
Glad you're enjoying it. I was a bit worried because I didn't want to hijack your thread with my own project.
Jason,
I dropped the accelerometer weight to 2% and then graphed my angle value over time as it is sitting on the desk in front of me. It's depressing!!!
So that's a peak-to-peak range of 0.2 degrees, which doesn't seem reasonable to me at all. Each point on the x-axis is approximately 30ms apart. I zoomed in on the graph to find out the frequency and its around 1.6 Hz. I compared that with the DPS value coming out of the gyro (post averaging) and then compared the two graphs and the waves were a perfect match. This variation is clearly the gyro's fault. With the jitter at such a low frequency, it does seem like any amount of averaging or filter will be capable of resolving this because it's way out of the scope of the loop, even if I did a rolling filter like you.
Is this normal? Is it possible to get the kind of stability you demonstrated in your video with this kind of error? Or do I need to continue tweaking the L3G settings? Or a different gyro altogether?
Each data point is the time it takes for a single loop in a cog. I'm getting data on three different cogs:
1) Sensor reader. This guy reads from the gyro and accel over 900kHz SPI bus, does some averaging of multiple values, and spits out three numbers (raw data from two accel. axes and DPS from the gyro)
2) Angle computer. This guy takes the three inputs from the sensor reader and computes an angle
3) Logger. This guy logs a done of data to the serial console at 460,800 baud, including the four values listed above
64-bit, cmm
Size: 13,932 bytes
Sensor reader: 9,905
Angle computer: 5,102
Logger: 28,932
32-bit, cmm
Size: 12,060 bytes
Sensor reader: 9,007
Angle computer: 1,543
Logger: 16,322
64-bit, lmm
Size: 20,668 bytes
Sensor reader: 3,152
Angle computer: 1,457
Logger: 9,082
32-bit, lmm
Size: 17,724 bytes
Sensor reader: 2,924
Angle computer: 495
Logger: 5,095
Response rate certainly does improve dramatically with both the bit-width and memory model! Ranging from 15ms at the worst to 3.3ms at best (ignoring the Logger cog). Interestingly, the computed angle actually changes with the memory model too. The graph above is based on data from 64-bit, cmm code. However, 64-bit lmm hits an average angle of more like 1.5! I haven't done a deep-dive into those numbers yet to find out what is causing the discrepancy.
Try turning off *all* filtering, and just emit raw readings. Graph those and see how they compare to what you're seeing now. If those are reasonably stable then the problem isn't the device.
This is what you expect:
1 degree per second * 0.01sec * 100 updates gives 1 degree.
...but if you're running half as fast as you think you are, this is what you'll get:
1 degree per second * 0.01sec * 50 updates is only 0.5 degrees.
If your accumulation loop runs slow, your readings will be low.
So long as each cog is running faster than 10ms, I'm okay. Even in the worst case of 64-bit cmm, it has 100 us left to spin in its waitcnt2 routine in the SensorReader cog. That's why I referenced "response rate" instead of "refresh rate" when referring to the performance.
You suspect it will cause incomplete sentences?
I can't remember exactly which graph that is that I posted last night, but I did try turning the high pass filter off and it didn't help any. However, setting it to 3.5 Hz did help a bit. I realize though that these results could have very different end results once mounted on a chassis that is moving about, so I'll play more with it later. And as you suggested, tonight I'll try turning off the high-pass filter and removing the averaging buffer so that I can inspect the raw data and see what kind of results I get.
If you whip up a quick test that does nothing more than dump raw gyro values and graph those it should point to where the issue is - it could be the physical hardware, though that seems unlikely. It could potentially be a power issue if you don't have enough decoupling on the chips, though that also seems unlikely. Running from a battery instead of a power supply would eliminate that pretty quickly.
So I've written up a test that reads 500 samples over the course of two seconds (uses waitcnt2 to get the precise 250 Hz frequency). After all 500 have been read, it dumps them to an SD card. I ran the test twice: once powered by my computer, and second time powered by an 8-cell battery pack full of brand new Eneloop AAs (multimeter measuring 10.6V). The board is a DNA-RTC. I did try a third time with a 100uF capacitor between ground and 3V3 on the DNA's header and a second .1uF capacitor between ground and VCC on the L3G breakout board's header... it didn't help so I didn't bother with a screenshot.
Here's the results:
It IS isn't it!?
Jason, you're results are significantly better than mine when graphed out. I set the scale so that there is a 4 DPS range on all three graphs to make sure the visual comparison is reasonable. It's immediately obvious that you're getting much cleaner results:
What gyro is being used in your hardware?