I have an encoder that gives 4300 increments per rev. And I need at least 3 turns in either direction. (for a steering wheel)
However, when I turn it just a bit, it already hits the extremums. This is after a few degrees clockwise:
This is my descripor:
My code:
while (1)
{
steer.direction = position - position_p;
position_p = position;
USBD_CUSTOM_HID_SendReport(&hUsbDeviceFS, &steer, sizeof(steer));
HAL_Delay(5);
}
I have tried using an absolute value. With 8 bits it just overflows after a few degrees and comes back to the opposite extremum. Maybe 16 bits could solve that but I can't get it to work that way.
I managed to use a 16 bit absolute position.
I think it didn't work because the "send" function only took 8-bit values.
So I've split the 16 bit variable into a 2x8-bit array. (im using cubeIDE)
steer.direction[0] = position & 0x00FF;
steer.direction[1] = position >> 8;
Related
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.
I need the line I am drawing to be of specific length. I can detect when it reaches the required length but if the user moves the mouse faster it could make the line longer than intended.
Heres a video of the issue I am having, when I move the mouse slowly works great but speed gives issue: https://www.youtube.com/watch?v=4wkYcbG78TE
Here is the code where I create and detect the length of the line.
if Input.is_action_pressed("Left_click"): #This checks the distance between last vector and the mouse vector
#points_array[-1] = get_global_mouse_position() # Gets the last position of the array and sets the mouse cords
var length = clamp(points_array[-1].distance_to(get_global_mouse_position()),0,20)
if length == 20: # if its long enough create a new line and set it to mouse position
var cords = get_global_mouse_position()
print(cords)
points_array.append(cords)
When the mouse moved too much, you could add multiple points to fill the gap, always at the correct distance, of course.
That is, while the length from the last point to the position of the mouse is larger than the distance you want, add another point at the appropriate distance.
Thus, a while loop:
if Input.is_action_pressed("Left_click"):
var mouse_pos = get_global_mouse_position()
var distance = 20
while points_array[-1].distance_to(mouse_pos) > distance:
var cords = # ???
points_array.append(cords)
A little vector algebra will figure out where to place that point. Starting from the last added point, you want to go in the direction from it to the position of the mouse. What distance to go? well, it the length you want.
if Input.is_action_pressed("Left_click"):
var mouse_pos = get_global_mouse_position()
var distance = 20
while points_array[-1].distance_to(mouse_pos) > distance:
var last_point = points_array[-1]
var cords = last_point + last_point.direction_to(mouse_pos) * distance
points_array.append(cords)
I believe that should work.
I've started editing the RaspiStillYUV.c code. I eventually want to process the image I receive, but for now, I'm just working to understand it. Why am I working with YUV instead of RGB? So I can learn something new. I've made minor changes to the function camera_buffer_callback. All I am doing is the following:
fprintf(stderr, "GREAT SUCCESS! %d\n", buffer->length);
The line this is replacing:
bytes_written = fwrite(buffer->data, 1, buffer->length, pData->file_handle);
Now, the dimensions should be 2592 x 1944 (w x h) as set in the code. Working off of Wikipedia (YUV420) I have come to the conclusion that the file size should be w * h * 1.5. Since the Y component has 1 byte of data for each pixel and the U and V components have 1 byte of data for every 4 pixels (1 + 1/4 + 1/4 = 1.5). Great. Doing the math in Python:
>>> 2592 * 1944 * 1.5
7558272.0
Unfortunately, this does not line up with the output of my program:
GREAT SUCCESS! 7589376
That leaves a difference of 31104 bytes.
I figure that the buffer is allocated in fixed size chunks (the output size is evenly divisible by 512). While I would like to understand that mystery, I'm fine with the fixed size chunk explanation.
My question is if I am missing something. Are the extra bytes beyond the expected size meaningful in this format? Should they be ignored? Are my calculations off?
The documentation at this location supports your theory on padding: http://www.raspberrypi.org/wp-content/uploads/2013/07/RaspiCam-Documentation.pdf
Specifically:
Note that the image buffers saved in raspistillyuv are padded to a
horizontal size divisible by 16 (so there may be unused bytes at the
end of each line to made the width divisible by 16). Buffers are also
padded vertically to be divisible by 16, and in the YUV mode, each
plane of Y,U,V is padded in this way.
So my interpretation of this is the following.
The width is 2592 (divisible by 16 so this is ok).
The height is 1944 which is 8 short of being divisible by 16 so an extra 8*2592 are added (also multiplied by 1.5) thus giving your 31104 extra bytes.
Although this kindof helps with the size of the file, it doesn't explain the structure of the YUV output properly. I am having a look at this description to see if this provides a hint to start with: http://en.wikipedia.org/wiki/YUV#Y.27UV420p_.28and_Y.27V12_or_YV12.29_to_RGB888_conversion
From this I believe it is as follows:
Y Channel:
2592 * (1944+8) = 5059584
U Channel:
1296 * (972+4) = 1264896
V Channel:
1296 * (972+4) = 1264896
Giving a sum of :
5059584 + 2*1264896 = 7589376
This makes the numbers add up so only thing left is to confirm if this interpretation is correct.
I am also trying to do the YUV decode (for image comparisons) so if you can confirm if this actually does correspond to what you are reading in the YUV file this would be much appreciated.
You have to read the manual carefully. Buffers are padded to multiples of 16, but colour data is half-size, so your image size needs to be in multiples of 32 to avoid problems with padding breaking external software.
I've used Nick Vellios' tutorial to create radial gravity with a Box2D object. I am aware of Make a Vortex here on SO, but I couldn't figure out how to implement it in my project.
I have made a vortex object, which is a Box2D circleShape sensor that rotates with a consistent angular velocity. When other Box2D objects contact this vortex object I want them to rotate around at the same angular velocity as the vortex, gradually getting closer to the vortex's centre. At the moment the object is attracted to the vortex's centre but it will head straight for the centre of the vortex, rather than spinning around it slowly like I want it to. It will also travel in the opposite direction than the vortex as well as with the vortex's rotation.
Given a vortex and a box2D body, how can I set the box2d body to rotate with the vortex as it gets 'sucked in'.
I set the rotation of the vortex when I create it like this:
b2BodyDef bodyDef;
bodyDef.type = b2_dynamicBody;
bodyDef.angle = 2.0f;
bodyDef.angularVelocity = 2.0f;
Here is how I'm applying the radial gravity, as per Nick Vellios' sample code.
-(void)applyVortexForcesOnSprite:(CCSpriteSubclass*)sprite spriteBody:(b2Body*)spriteBody withVortex:(Vortex*)vortex VortexBody:(b2Body*)vortexBody vortexCircleShape:(b2CircleShape*)vortexCircleShape{
//From RadialGravity.xcodeproj
b2Body* ground = vortexBody;
b2CircleShape* circle = vortexCircleShape;
// Get position of our "Planet" - Nick
b2Vec2 center = ground->GetWorldPoint(circle->m_p);
// Get position of our current body in the iteration - Nick
b2Vec2 position = spriteBody->GetPosition();
// Get the distance between the two objects. - Nick
b2Vec2 d = center - position;
// The further away the objects are, the weaker the gravitational force is - Nick
float force = 1 / d.LengthSquared(); // 150 can be changed to adjust the amount of force - Nick
d.Normalize();
b2Vec2 F = force * d;
// Finally apply a force on the body in the direction of the "Planet" - Nick
spriteBody->ApplyForce(F, position);
//end radialGravity.xcodeproj
}
Update I think iForce2d has given me enough info to get on my way, now it's just tweaking. This is what I'm doing at the moment, in addition to the above code. What is happening is the body gains enough velocity to exit the vortex's gravity well - somewhere I'll need to check that the velocity stays below this figure. I'm a little concerned I'm not taking into account the object's mass at the moment.
b2Vec2 vortexVelocity = vortexBody->GetLinearVelocityFromWorldPoint(spriteBody->GetPosition() );
b2Vec2 vortexVelNormal = vortexVelocity;
vortexVelNormal.Normalize();
b2Vec2 bodyVelocity = b2Dot( vortexVelNormal, spriteBody->GetLinearVelocity() ) * vortexVelNormal;
//Using a force
b2Vec2 vel = bodyVelocity;
float forceCircleX = .6 * bodyVelocity.x;
float forceCircleY = .6 * bodyVelocity.y;
spriteBody->ApplyForce( b2Vec2(forceCircleX,forceCircleY), spriteBody->GetWorldCenter() );
It sounds like you just need to apply another force according to the direction of the vortex at the current point of the body. You can use b2Body::GetLinearVelocityFromWorldPoint to find the velocity of the vortex at any point in the world. From Box2D source:
/// Get the world linear velocity of a world point attached to this body.
/// #param a point in world coordinates.
/// #return the world velocity of a point.
b2Vec2 GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const;
So that would be:
b2Vec2 vortexVelocity = vortexBody->GetLinearVelocityFromWorldPoint( suckedInBody->GetPosition() );
Once you know the velocity you're aiming for, you can calculate how much force is needed to go from the current velocity, to the desired velocity. This might be helpful: http://www.iforce2d.net/b2dtut/constant-speed
The topic in that link only discusses a 1-dimensional situation. For your case it is also essentially 1-dimensional, if you project the current velocity of the sucked-in body onto the vortexVelocity vector:
b2Vec2 vortexVelNormal = vortexVelocity;
vortexVelNormal.Normalize();
b2Vec2 bodyVelocity = b2Dot( vortexVelNormal, suckedInBody->GetLinearVelocity() ) * vortexVelNormal;
Now bodyVelocity and vortexVelocity will be in the same direction and you can calculate how much force to apply. However, if you simply apply enough force to match the vortex velocity exactly, the sucked in body will probably go into orbit around the vortex and never actually get sucked in. I think you would want to make the force quite a bit less than that, and I would scale it down according to the gravity strength as well, otherwise the sucked-in body will be flung away sideways as soon as it contacts the outer edge of the vortex. It could take a lot of tweaking to get the effect you want.
EDIT:
The force you apply should be based on the difference between the current velocity (bodyVelocity) and the desired velocity (vortexVelocity), ie. if the body is already moving with the vortex then you don't need to apply any force. Take a look at the last code block in the sub-section titled 'Using forces' in the link I gave above. The last three lines there do pretty much what you need if you replace 'vel' and 'desiredVel' with the sizes of your bodyVelocity and vortexVelocity vectors:
float desiredVel = vortexVelocity.Length();
float currentVel = bodyVelocity.Length();
float velChange = desiredVel - currentVel;
float force = body->GetMass() * velChange / (1/60.0); //for a 1/60 sec timestep
body->ApplyForce( b2Vec2(force,0), body->GetWorldCenter() );
But remember this would probably put the body into orbit, so somewhere along the way you would want to reduce the size of the force you apply, eg. reduce 'desiredVel' by some percentage, reduce 'force' by some percentage etc. It would probably look better if you could also scale the force down so that it was zero at the outer edge of the vortex.
I had a project where I had asteroids swirling around a central point (there are things jumping between them...which is a different point).
They are connected to the "center" body via b2DistanceJoints.
You can control the joint length to make them slowly spiral inward (or outward). This gives you find grain control instead of balancing force control, which may be difficult.
You also apply tangential force to make them circle the center.
By applying different (or randomly changing) tangential forces, you can make the
crash into each other, etc.
I posted a more complete answer to this question here.
If you understand objective c very well, then just read the last 2 sentences. The rest of this just summarizes the last 2 sentances:
So I have two sprites, the lower arm and the upper arm. I set the anchor points to ccp(0.5f,0.0f) So lets say that the following dashes represent the lower arm, the anchorpoint is the dash in parenthesis: (-)------ . So the object is rotating around this point (the CGPoint at the moment is ccp(100,55)).
What I need is, if the lower arm is rotating around the dash in parenthesis: (-)-----o the circle represents the point I want. I'm basically connecting the two arms and trying to make the movement look nice... Both arms are 17 pixels long (which means that if the lower arm is pointing straight up, the CGPoint of the circle is ccp(100,72), and if the arm was pointing straight down, the circle would be ccp(100,38).
What equation would I use so that I could set the position of the upper arm equal to the position of the lower arm's rotating CGPoint, represented as a circle in the 2nd paragraph of this question. Like... _,/ the _ represents the lower arm, the comma represents the point I want, and the / represent the upper arm.
So lower and upper arm = 17 pixels long, anchor point for both is (0.5f,0.0f), how do I find the point opposite of the anchor point for the lower arm.
x = 100 + 17 * cos(θ)
y = 55 + 17 * sin(θ)
You need to find what the angle of rotation is. I'm not that familiar with objective c, but if you're using a rotation function there's most likely an angle component somewhere you can reference.
From there you can use trigonometry to find the components of your x and y change.
For x it will be: (anchor x) + (length of arm) * cosine(angle of rotation)
And for y it will be: (anchor y) + (length of arm) * sine(angle of rotation)
Also, make sure you know whether the angle is in radians or degrees, you might have to convert based on the sine/cosine functions.