I'm working with a Gyroscope (L3GD20) with a 2000DPS
Correct me if their is a mistake,
I start by reading the values High and Low for the 3 axes and concatenate them. Then I multiply every value by 0.07 to convert them into DPS.
My main goal is to track the angle over time, so I simply implemented a Timer which reads the data every dt = 10 ms
to integrate ValueInDPS * 10ms, here is the code line I'm using :
angleX += (resultGyroX)*dt*0.001; //0.001 to get dt in [seconds]
This should give us the value of the angle in [degree] am I right ?
The problem is that the values I'm getting are a little bit weird, for example when I make a rotation of 90°, I get something like 70°...
Your method is a recipe for imprecision and accumulated error.
You should avoid using floating point (especially if there is no FPU), and especially also if this code is in the timer interrupt handler.
you should avoid unnecessarily converting to degrees/sec on every sample - that conversion is needed only for presentation, so you should perform it only when you need to need the value - internally the integrator should work in gyro sample units.
Additionally, if you are doing floating point in both an ISR and in a normal thread and you have an FPU, you may also encounter unrelated errors, because FPU registers are not preserved and restored in an interrupt handler. All in all floating point should only be used advisedly.
So let us assume you have a function gyroIntegrate() called precisely every 10ms:
static int32_t ax = 0
static int32_t ay = 0
static int32_t az = 0
void gyroIntegrate( int32_t sample_x, int32_t sample_y, int32_t sample_z)
{
ax += samplex ;
ay += sampley ;
az += samplez ;
}
Not ax etc. are the integration of the raw sample values and so proportional to the angle relative to the starting position.
To convert ax to degrees:
degrees = ax × r-1 × s
Where:
r is the gyro resolution in degrees per second (0.07)
s is the sample rate (100).
Now you would do well to avoid floating point and here it is entirely unnecessary; r-1 x s is a constant (1428.571 in this case). So to read the current angle represented by the integrator, you might have a function:
#define GYRO_SIGMA_TO_DEGREESx10 14286
void getAngleXYZ( int32_t* int32_t, int32_t* ydeg, int32_t* zdeg )
{
*xdeg = (ax * 10) / GYRO_SIGMA_TO_DEGREESx10 ;
*ydeg = (ax * 10) / GYRO_SIGMA_TO_DEGREESx10 ;
*zdeg = (ax * 10) / GYRO_SIGMA_TO_DEGREESx10 ;
}
getAngleXYZ() should be called from the application layer when you need a result - not from the integrator - you do the math at the point of need and have CPU cycles left to do more useful stuff.
Note that in the above I have ignored the possibility of arithmetic overflow of the integrator. As it is it is good for approximately +/-1.5 million degrees +/-4175 rotations), so it may not be a problem in some applications. You could use an int64_t or if you are not interested in the number of rotations, just the absolute angle then, in the integrator:
ax += samplex ;
ax %= GYRO_SIGMA_360 ;
Where GYRO_SIGMA_360 equals 514286 (360 x s / r).
Unfortunately, MEMs sensor math is quite complicated.
I would personally use ready libraries provided by the STM https://www.st.com/en/embedded-software/x-cube-mems1.html.
I actually use them, and the results are very good.
Related
I need to be able to find x, y coordinates at any length down an Archimedean spiral arm, given a specific distance between each loop of the arm.
I have researched previous questions on Stackoverflow, and across the Internet, and I have found three methods, which are each different, and each plot me a spiral. (I call them the first, second and third method, here.)
The first method, does plot equidistant points, with the pointdist variable = 1, but as this is increased, there is also an aberration of point distances (supposed to be equidistant) near the center of the spiral.
The third method and second method, do not correctly plot equidistant points near the center of the spiral. (See graphs below)
The third method, though, allows me to input any length down the arm and obtain x, y coordinates.
The first and second methods plot equidistant points by a process where they additively sum a variable, each cycle of a loop to plot the equidistant points. Because of how this value is built up instead of calculated from scratch using only the distance along the spiral arm variable, I can't use these two methods to find coordinates at any arbitrary length along the arm [Proofreading this, I just thought, perhaps if I initialize the length to calculate one point each time. However, all three methods have problems even with equidistant points.]
Here is output from the third method:
Here is the code of the "third method". This method uses what an answer to another sprial-related question on Stackoverflow (Placing points equidistantly along an Archimedean spiral) calls the "Clackson scroll formula", which is said to be possibly inaccurate in some ranges.
double thetamax = 10 * Math.PI;
double b = armbandwidth / (2 * Math.PI);
// "armbandwidth” value influences distance between the spiral arms, usually kept between 1 and 20
AddPoint(0,0); // Mark the origin of the spiral
// “pointdist” is the length between points to plot along the spiral, I use 0.1 to 2+
// but it doesn't reveal spiral shape with increasing values
for (double s = pointdist; s < spirallength; s += pointdist)
{
double thetai = Math.Sqrt(2 * s / b);
double xx = b * thetai * Math.Cos(thetai);
double yy = b * thetai * Math.Sin(thetai);
AddPoint(xx, yy);
}
I need to both:
Use a method that does not have aberrations in the equidistance of points along the spiral arm, given equally spaced values of lengths down the spiral arms.
Use a method that allows me to specify the width between the spiral arms (in terms of the same units used for the length along spiral arm between the points, as well).
In case it helps, and to be clear about what I've tried, here are the code and output from the other methods I've found and tested (here called "second method" and "first method") for calculating the coordinates of equidistant points along an Archimedean spiral:
Here is output from the second method (note the uneven distanced points near center):
Here is the code for the second method:
AddPoint(0,0); // Mark the origin of the spiral
double arclength = 0.8; // This value (kept between 0.1 and 20 or so) sets the distance between points to calculate coordinates for along the spiral curve
double r = arclength;
double b = armbandwidth / (2 * Math.PI); // "armbandwidth" value influences distance between the spiral arms, usually kept between 3.5 to 10
double phi = r / b;
double xx = r * Math.Cos(phi);
double yy = r * Math.Sin(phi);
AddPoint(xx, yy);
while( r <= spirallength ) // spirallength determines roughly how many iterations of points to draw
{
phi += arclength / r;
r = b * phi;
xx = r * Math.Cos(phi);
yy = r * Math.Sin(phi);
AddPoint(xx, yy);
}
Because the variable phi is additively increased each iteration, I can't pass in any length down the spiral arm to find coordinates for. (Maybe if I initialized the whole method only to a single arclength each time. - In any case, the points near center are not evenly spaced.)
Here is output from the first method (Equidistant points throughout with pointdist = 1):
Here is the code of the first method:
double separation = 4; // Value influences distance between the spiral arms, usually kept 3.5 to 10+
double angle = 0;
double r;
AddPoint(0,0); // Mark the origin of the spiral
for (double i=0; i <= spirallength; i+=pointdist) // spirallength determines pointdist spaced points to plot
{
r = Math.Sqrt(i+1);
angle += Math.Asin(1/r);
double xx = Math.Cos(angle) * r*separation;
double yy = Math.Sin(angle) * r*separation;
AddPoint(xx, yy);
}
However, when "pointdist" is increased above 1, there are aberrations in equidistance between points near the center of the spiral, even by this method. Here is the output of a graph using the "first method" and pointdist = 9:
Can anyone help me calculate x, y coordinates for any length down the spiral arm from center, for an Archimedes spiral defined by a specified width between loops of the arm?
(It should be able to have equidistant points, accurate even near the center, and be able to take a width between loops of the arm in units the same scale as those used for the distance to a point along the arm passed to the coordinates equation.)
Much appreciated!
I believe this last piece of code is the best (most simple and straightforward) approach:
constant angle variation
For a given angle
calculate the radius
convert Polar coordinates to Cartesian.
However, I understand the Archimedean spiral is defined by the formula: r = a + b * angle (where a=0 to simplify, and b controls the distance between loops.
In short, the position of particle is proportional to the angle θ as time elapses.
So what's up with that? That's not linear at all!
r = Math.Sqrt(i+1);
angle += Math.Asin(1/r);
Here's a simple way to make it.
It's not a running code. But I believe it's very easy to understand.
So just understand the logic and then code it with your own variables:
for (some incrementing t)
{
radius = t/100; //Start at radius = 0. When t = 100, radius=1.
angle = t*2*Pi/200; //It takes 200 points to make a full 360 (2Pi) turn
//Now converting from polar coordinates (r,angle) to (x,y):
x = cos(angle) * r;
y = sin(angle) * r;
AddPoint(x, y);
}
This code generates the following image:
This generates the right trajectory. However, as noticed, this does not produce equidistant points, because each angle increment is multiplied by the radius. So I suggest you to find a correction function and apply it to t. Like so:
function correction(i)
{
// I actually don't know the exact relationship between t and i
t = 1/i
// But I think it converges to t=1/i for very small increments
return t
}
for (some incrementing i)
{
t = correction(i)
...
}
I am using naudio with SineWaveProvider32 code directly from http://mark-dot-net.blogspot.com/2009/10/playback-of-sine-wave-in-naudio.html to generate
sine wave tones. The relevant code in the SineWaveProvider32 class:
public override int Read(float[] buffer, int offset, int sampleCount)
{
int sampleRate = WaveFormat.SampleRate;
for (int n = 0; n < sampleCount; n++)
{
buffer[n + offset] =
(float)(Amplitude * Math.Sin((2 * Math.PI * sample * Frequency) / sampleRate));
sample++;
if (sample >= sampleRate) sample = 0;
}
return sampleCount;
}
I was getting clicks/beats every second, so I changed
if (sample >= sampleRate) sample = 0;
to
if (sample >= (int)(sampleRate / Frequency)) sample = 0;
This fixed the clicks every second (so that "sample" was always relative to a zero-crossing, not the sample rate).
However, whenever I set the Amplitude variable, I get a click. I tried setting it only when the buffer[] was at a zero-crossing,
thinking that a sudden jump in amplitude might be causing the problem. That did not solve the problem. I am setting the Amplitude to values between
0.25 and 0.0
I tried adusting the latency and number of buffers as suggested in NAudio change volume in runtime but that
had no effect either.
My code that changes the Amplitude:
public async void play(int durationMS, float amplitude = .25f)
{
PitchPlayer pPlayer = new PitchPlayer(this.frequency, amplitude);
pPlayer.play();
await Task.Delay(durationMS/2);
pPlayer.provider.Amplitude = .15f;
await Task.Delay(durationMS /2);
pPlayer.stop();
}
the clicks are caused by a discontinuity in the waveform. This is hard to fix in a class like this because ideally you would slowly ramp the volume from one value to the other. This can be done by modifying the code to have a target amplitude, and then if the current amplitude is not equal to the target amplitude then you move towards it by a small delta amount calculated each time through the loop. So over a period of say 10ms, you move from the old to new amplitude. But you'd need to write this yourself unfortunately.
For a similar concept where the frequency is being changed gradually rather than the amplitude, take a look at my blog post on portamento in NAudio.
Angular speed
Instead of frequency it is easier to think in terms of angular speed. How much to increase the angular argument of a sin() function for each sample.
When using radians for angle, one periode completing a full circle is 2*pi so the angular velocity of one Hz is (2*pi)/T = (2*pi)/1/f = f*2*pi = 1*2*pi [rad/s]
The sample rate is in [samples per second] and the angular velocity is in [radians per second] so to get the [angle per sample] you simply divide angular velocity by sample rate to get [radians/second]/[samples/second] = [radians/sample].
That is the number to continuously increase the angle of the sin() function for each sample - no multiplication is needed.
To sweep from one frequency to another you simply move from one angular increment to another in small steps over a number of samples.
By sweeping between frequencies there will be a continuous chain of adjacent samples and transient spread out smoothly over time.
Moving from one amplitude to another could also be spread out over multiple samples to avoid sharp transients.
Fade in and fade out incrementally adjusting the amplitude at the start and end of a sound is more graceful than stepping the output from one level to another in one sample.
Sharp steps produce rings on the water that propagate out in the world.
About sin() calculations
For speedy calculations it may be better to rotate a vector of the length of the amplitude and calculate sn=sin(delta), cs=cos(delta) only when angular speed changes:
Wikipedia Link to theory
where amplitude^2 = x^2 + y^2, each new sample can be calculated as:
px = x * cs - y * sn;
py = x * sn + y * cs;
To increase the amplitude you simply multiply px and py by a factor say 1.01. To make the next sample you set x=px, y=py and run the px, py calculation again with cs and sn the same all the time.
py or px can be used as the signal output and will be 90 deg out of phase.
On the first sample you can set x=amplitude and y=0.
Consider a line from point A (x,y) to B (p,q).
The method CGContextMoveToPoint(context, x, y); moves to the point x,y and the method CGContextAddLineToPoint(context, p, q); will draw the line from point A to B.
My question is, can I find the all points that the line cover?
Actually I need to know the exact point which is x points before the end point B.
Refer this image..
The line above is just for reference. This line may have in any angle. I needed the 5th point which is in the line before the point B.
Thank you
You should not think in terms of pixels. Coordinates are floating point values. The geometric point at (x,y) does not need to be a pixel at all. In fact you should think of pixels as being rectangles in your coordinate system.
This means that "x pixels before the end point" does not really makes sense. If a pixel is a rectangle, "x pixels" is a different quantity if you move horizontally than it is if you move vertically. And if you move in any other direction it's even harder to decide what it means.
Depending on what you are trying to do it may or may not be easy to translate your concepts in pixel terms. It's probably better, however, to do the opposite and stop thinking in terms of pixels and translate all you are currently expressing in pixel terms into non pixel terms.
Also remember that exactly what a pixel is is system dependent and you may or may not, in general, be able to query the system about it (especially if you take into consideration things like retina displays and all resolution independent functionality).
Edit:
I see you edited your question, but "points" is not more precise than "pixels".
However I'll try to give you a workable solution. At least it will be workable once you reformulate your problem in the right terms.
Your question, correctly formulated, should be:
Given two points A and B in a cartesian space and a distance delta, what are the coordinates of a point C such that C is on the line passing through A and B and the length of the segment BC is delta?
Here's a solution to that question:
// Assuming point A has coordinates (x,y) and point B has coordinates (p,q).
// Also assuming the distance from B to C is delta. We want to find the
// coordinates of C.
// I'll rename the coordinates for legibility.
double ax = x;
double ay = y;
double bx = p;
double by = q;
// this is what we want to find
double cx, cy;
// we need to establish a limit to acceptable computational precision
double epsilon = 0.000001;
if ( bx - ax < epsilon && by - ay < epsilon ) {
// the two points are too close to compute a reliable result
// this is an error condition. handle the error here (throw
// an exception or whatever).
} else {
// compute the vector from B to A and its length
double bax = bx - ax;
double bay = by - ay;
double balen = sqrt( pow(bax, 2) + pow(bay, 2) );
// compute the vector from B to C (same direction of the vector from
// B to A but with lenght delta)
double bcx = bax * delta / balen;
double bcy = bay * delta / balen;
// and now add that vector to the vector OB (with O being the origin)
// to find the solution
cx = bx + bcx;
cy = by + bcy;
}
You need to make sure that points A and B are not too close or the computations will be imprecise and the result will be different than you expect. That's what epsilon is supposed to do (you may or may not want to change the value of epsilon).
Ideally a suitable value for epsilon is not related to the smallest number representable in a double but to the level of precision that a double gives you for values in the order of magnitude of the coordinates.
I have hardcoded epsilon, which is a common way to define it's value as you generally know in advance the order of magnitude of your data, but there are also 'adaptive' techniques to compute an epsilon from the actual values of the arguments (the coordinates of A and B and the delta, in this case).
Also note that I have coded for legibility (the compiler should be able to optimize anyway). Feel free to recode if you wish.
It's not so hard, translate your segment into a math line expression, x pixels may be translated into radius of a circe with center in B, make a system to find where they intercept, you get two solutions, take the point that is closer to A.
This is the code you can use
float distanceFromPx2toP3 = 1300.0;
float mag = sqrt(pow((px2.x - px1.x),2) + pow((px2.y - px1.y),2));
float P3x = px2.x + distanceFromPx2toP3 * (px2.x - px1.x) / mag;
float P3y = px2.y + distanceFromPx2toP3 * (px2.y - px1.y) / mag;
CGPoint P3 = CGPointMake(P3x, P3y);
Either you can follow this link also it will give you the detail description -
How to find a third point using two other points and their angle.
You can find out number of points whichever you want to find.
I am trying to calculate the forces that will act on circular objects in the event of a collision. Unfortunately, my mechanics is slightly rusty so i'm having a bit of trouble.
I have an agent class with members
vector position // (x,y)
vector velocity // (x,y)
vector forward // (x,y)
float radius // radius of the agent (all circles)
float mass
So if we have A,B:Agent, and in the next time step the velocity is going to change the position. If a collision is going to occur I want to work out the force that will act on the objects.
I know Line1 = (B.position-A.position) is needed to work out the angle of the resultant force but how to calculate it is baffling me when I have to take into account current velocity of the vehicle along with the angle of collision.
arctan(L1.y,L1.x) is am angle for the force (direction can be determined)
sin/cos are height/width of the components
Also I know to calculate the rotated axis I need to use
x = cos(T)*vel.x + sin(T)*vel.y
y = cos(T)*vel.y + sin(T)*vel.x
This is where my brain can't cope anymore.. Any help would be appreciated.
As I say, the aim is to work out the vector force applied to the objects as I have already taken into account basic physics.
Added a little psudocode to show where I was starting to go with it..
A,B:Agent
Agent {
vector position, velocity, front;
float radius,mass;
}
vector dist = B.position - A.position;
float distMag = dist.magnitude();
if (distMag < A.radius + B.radius) { // collision
float theta = arctan(dist.y,dist.x);
flost sine = sin(theta);
float cosine = cos(theta);
vector newAxis = new vector;
newAxis.x = cosine * dist .x + sine * dist .y;
newAxis.y = cosine * dist .y - sine * dist .x;
// Converted velocities
vector[] vTemp = {
new vector(), new vector() };
vTemp[0].x = cosine * agent.velocity.x + sine * agent.velocity.y;
vTemp[0].y = cosine * agent.velocity.y - sine * agent.velocity.x;
vTemp[1].x = cosine * current.velocity.x + sine * current.velocity.y;
vTemp[1].y = cosine * current.velocity.y - sine * current.velocity.x;
Here's to hoping there's a curious maths geek on stack..
Let us assume, without loss of generality, that we are in the second object's reference frame before the collision.
Conservation of momentum:
m1*vx1 = m1*vx1' + m2*vx2'
m1*vy1 = m1*vy1' + m2*vy2'
Solving for vx1', vy1':
vx1' = vx1 - (m2/m1)*vx2'
vy1' = vy1 - (m2/m1)*vy2'
Secretly, I will remember the fact that vx1'*vx1' + vy1'*vy1' = v1'*v1'.
Conservation of energy (one of the things elastic collisions give us is that angle of incidence is angle of reflection):
m1*v1*v1 = m1*v1'*v1' + m2*v2'+v2'
Solving for v1' squared:
v1'*v1' = v1*v1 - (m2/m1)v2'*v2'
Combine to eliminate v1':
(1-m2/m1)*v2'*v2' = 2*(vx2'*vx1+vy2'*vy1)
Now, if you've ever seen a stationary poolball hit, you know that it flies off in the direction of the contact normal (this is the same as your theta).
v2x' = v2'cos(theta)
v2y' = v2'sin(theta)
Therefore:
v2' = 2/(1-m2/m1)*(vx1*sin(theta)+vy1*cos(theta))
Now you can solve for v1' (either use v1'=sqrt(v1*v1-(m2/m1)*v2'*v2') or solve the whole thing in terms of the input variables).
Let's call phi = arctan(vy1/vx1). The angle of incidence relative to the tangent line to the circle at the point of intersection is 90-phi-theta (pi/2-phi-theta if you prefer). Add that again for the reflection, then convert back to an angle relative to the horizontal. Let's call the angle of incidence psi = 180-phi-2*theta (pi-phi-2*theta). Or,
psi = (180 or pi) - (arctan(vy1/vx1))-2*(arctan(dy/dx))
So:
vx1' = v1'sin(psi)
vy1' = v1'cos(psi)
Consider: if these circles are supposed to be solid 3D spheres, then use a mass proportional to radius-cubed for each one (note that the proportionality constant cancels out). If they are supposed to be disklike, use mass proportional to radius-squared. If they are rings, just use radius.
Next point to consider: Since the computer updates at discrete time events, you actually have overlapping objects. You should back out the objects so that they don't overlap before computing the new location of each object. For extra credit, figure out the time that they should have intersected, then move them in the new direction for that amount of time. Note that this time is just the overlap / old velocity. The reason that this is important is that you might imagine a collision that is computed that causes the objects to still overlap (causing them to collide again).
Next point to consider: to translate the original problem into this problem, just subtract object 2's velocity from object 1 (component-wise). After the computation, remember to add it back.
Final point to consider: I probably made an algebra error somewhere along the line. You should seriously consider checking my work.
I want to simulate a free fall and a collision with the ground (for example a bouncing ball). The object will fall in a vacuum - an air resistance can be omitted. A collision with the ground should causes some energy loss so finally the object will stop moving. I use JOGL to render a point which is my falling object. A gravity is constant (-9.8 m/s^2).
I found an euler method to calculate a new position of the point:
deltaTime = currentTime - previousTime;
vel += acc * deltaTime;
pos += vel * deltaTime;
but I'm doing something wrong. The point bounces a few times and then it's moving down (very slow).
Here is a pseudocode (initial pos = (0.0f, 2.0f, 0.0f), initial vel(0.0f, 0.0f, 0.0f), gravity = -9.8f):
display()
{
calculateDeltaTime();
velocity.y += gravity * deltaTime;
pos.y += velocity.y * deltaTime;
if(pos.y < -2.0f) //a collision with the ground
{
velocity.y = velocity.y * energyLoss * -1.0f;
}
}
What is the best way to achieve a realistic effect ? How the euler method refer to the constant acceleration equations ?
Because floating points dont round-up nicely, you'll never get at a velocity that's actually 0. You'd probably get something like -0.00000000000001 or something.
you need to to make it 0.0 when it's close enough. (define some delta.)
To expand upon my comment above, and to answer Tobias, I'll add a complete answer here.
Upon initial inspection, I determined that you were bleeding off velocity to fast. Simply put, the relationship between kinetic energy and velocity is E = m v^2 /2, so after taking the derivative with respect to velocity you get
delta_E = m v delta_v
Then, depending on how energyloss is defined, you can establish the relationship between delta_E and energyloss. For instance, in most cases energyloss = delta_E/E_initial, then the above relationship can be simplified as
delta_v = energyloss*v_initial / 2
This is assuming that the time interval is small allowing you to replace v in the first equation with v_initial, so you should be able to get away with it for what your doing. To be clear, delta_v is subtracted from velocity.y inside your collision block instead of what you have.
As to the question of adding air-resistance or not, the answer is it depends. For small initial drop heights, it won't matter, but it can start to matter with smaller energy losses due to bounce and higher drop points. For a 1 gram, 1 inch (2.54 cm) diameter, smooth sphere, I plotted time difference between with and without air friction vs. drop height:
For low energy loss materials (80 - 90+ % energy retained), I'd consider adding it in for 10 meter, and higher, drop heights. But, if the drops are under 2 - 3 meters, I wouldn't bother.
If anyone wants the calculations, I'll share them.