I am trying to use a PID controller to stop a quadcopter at a specific location while travelling horizontally, however currently it overshoots/undershoots depending on the max velocity. I have tried manually tuning the P,I and D gains with limited success. Essentially the velocity needs to go from maxSpeed to 0 at the very end of the flight path.
I run a loop that executes every 0.1 of a second.
The pitch input to the quadcopter is in m/s and I recalculate the distance to the target on each iteration.
some pseudo code
kP = 0.25
kI = 0.50
kD = 90
timeStep = 0.1
maxSpeed = 10
currentError = initialDistanceToLocation - currentDistanceToLocation
derivativeError = (currentError - previousError) / timeStep
previousError = currentError
output = kP * currentError + kI * integralError + kD * derivativeError
integralError = integralError + currentError * timeStep
if >= maxSpeed {
output = maxSpeed
} else if output <= 0 {
output = 0
}
return output
Is there a way to reliably tune this PID controller to this system that will work for different max velocities, and/or are there other factors I need to consider?
A few ideas:
check in your code if you calculate the integralError after you calculate the output, too. This can, of course, lead to undefined behavior since in the output calculation, integralError could be anything depending on the programming language.
From my experience, you should get rid of the D part entirely, since in a digital environment and due to measurement noise, it could have destabilizing effects
I highly recommend looking at this tutorial which guides you through important aspects (even though it concentrates on fixed-point PI controllers)
Summary
To stop on a point, one easy way is to switch from a velocity PID controller to a position PID controller whenever you want to stop on a point, and turn up D (which dampens velocity in a position controller) to prevent position overshoot of your target point. See below for details.
Reminder: D in a velocity controller = useless. D in a position controller = essential. See my comment here, and notes below.
Details
I've done this before. I didn't spend a ton of time optimizing it, but what I found to work well was to switch from a velocity controller which tries to overfly waypoints, to a position controller which tries to stop on them, once you are near the desired stopping point.
Note that your position controller can also be forced into a velocity controller by scaling your gains to produce desired velocities, or even by scaling your position error to arbitrarily produce a desired velocity, and by moving the desired position point continually to keep the vehicle following a path. The video below demos that last part too.
The position controller was a PID controller on position: the vehicle's pitch angle was directly proportional to the distance from the target in the y-axis, and the vehicle's roll angle was directly proportional to the distance from the target in the x-axis. That's it. The integral would take out steady-state error to ultimately stop perfectly on the desired position, despite disturbances, wind, imperfections, etc., and the derivative was a dampening term which would reduce the pitch and roll angles as velocity increased, so as to not overshoot the target. In other words: P and D fight against each other, and this is good and intended! Increasing P tries harder to make the vehicle tilt when the vehicle has no velocity, and increasing D causes the vehicle to tilt less and therefore overshoot less the more velocity (derivative of position) that it has! P gets it moving quickly (increases acceleration), and D stops it from moving at too high of a velocity (decreases velocity).
For any controller where neutralizing the driving force (pitch and roll in this case) instantly stops a change in the derivative of the error, D is NOT needed. Ex: for velocity controllers, the second you remove throttle on a car for instance, velocity stops changing, assuming small drag. So, D is NOT needed. BUT, for position controllers, the second you remove throttle on a car (or tilt on a quadcopter), position keeps changing rapidly due to inertia and existing velocity! So, D very much is needed. That's it! That's the rule! For position-based controllers, D very much is needed, since it dampens velocity and acts as the dampening term to prevent overshoot of the target.
Watch this video here, from 0:49 to 1:10 to see the vehicle quickly take off autonomously in this PID position control mode, and auto-position itself at the center of the room. This is exactly what I'm talking about.
At approximately 3:00 in the video I say, "[the target waypoint] is leading the vehicle by two-and-one-half meters down the waypoint path." You should know that that 2.5 meters is the forced distance error (position error) that I am enforcing in the position PID controller in order to keep the vehicle moving at the shown fixed velocity at that point in the video. So...I'm basically using my position controller as a crude sort of velocity controller with a natural filter on the commanded path shape due to my "pure pursuit" or "vector flow-field" waypoint-following algorithm.
Anyway, I explain a lot of the above concepts, and much more, in my more-thorough answer here, so study this too: Physics-based controls, and control systems: the many layers of control.
Related
I am working on a project where I should implement object tracking technique using the camera of Parrot AR Drone 2.0. So the main idea is, a drone should be able to identify a specified colour and then follow it by keeping some distance.
I am using the opencv API to establish communication with the drone. This API provides function:
ARDrone::move3D(double vx, double vy, double vz, double vr)
which moves the AR.Drone in 3D space and where
vx: X velocity [m/s]
vy: Y velocity [m/s]
vz: Z velocity [m/s]
vr: Rotational speed [rad/s]
I have written an application which does simple image processing on the images obtained from the camera of the drone using OpenCV and finds needed contours of the object to be tracked. See the example below:
Now the part I am struggling is finding the technique using which I should find the velocities to be sent to the move3D function. I have read that common way of doing controlling is by using PID controlling. However, I have read about that and could not get how it could be related to this problem.
To summarise, my question is how to move a robot towards an object detected in its camera? How to find coordinates of certain objects from the camera?
EDIT:
So, I just realized you are using a drone and your cordinate system WRT the drone is likely x forward into image, y left of image (image columns), z up vertically(image rows). My answer has coordinates WRT the camera x = columns, y = rows, z = depth (into image) Keep that n mind when you read my outline. Also everything I wrote is psuedo-code, it won't run without many modifications
Original Post:
A PID controller is a Proportional–integral–derivative controller. It decides an action sequenced based on the your specific error.
For your problem lets assume the optimal tracking means the rectangle is in the center of the image, and it takes up ~30% of the pixel space. This means that you move your camera/bot until these conditions are met. We will call these goal parameters
x_ideal = image_width / 2
y_ideal = image_height / 2
area_ideal = image_width * image_height * 0.3
Now lets say your bounding box is characterized by 4 parameters
(x_bounding, y_bounding, width_bounding_box, height_bounding_box)
Your error would be something along the lines of:
x_err = x_bounding - x_ideal;
y_err = y_bounding - y_ideal;
z_err = area_ideal - (width_bounding_box * height_bounding_box)
Notice I have tied the z distance (depth) to the size of the object. This assumes that the object being tracked is rigid and doesn't change size. Any change in size is due to the objects distance to the camera (a bigger bounding box means object is close, a small one means object is far). This is a bit of an estimation, but without having any parameters on the camera or the object itself we can only make these general statements.
We need to keep the sign in mind when creating our control sequences, this is why the order matters when doing subtraction. Lets think about this logically. x_err determines how far off the bounding box is horizontally from the desired position. In our case this should be positive, meaning the bot should move to the left so the object moves closer to the center of the image. The box is too small, meaning the object is too far away, etc.
z_err < 0 : means bot is too close and needs to slow down, Vz should be reduced
z_err = 0 : keep the speed command the same, no change
z_err > 0 : we need to get closer, Vz should increase
x_err < 0 : means bot is to the right and needs to turn left(decreasing x), Vx should be reduced
x_err = 0 : keep the speed in X the same, no change to Vx
x_err > 0 : means bot is to the left and needs to turn right(increasing x), Vx should be increased
We can do the same for each y axis. Now we use this error to create a command sequence for the bot.
That description sounds a lot like a PID controller. Observe a state, figure out an error, create a control sequence to reduce the error, then repeat the process over and over. In your case the velocity would be the actions output by your algorithm. You will essentially have 3 PIDs running
PID for X
PID for Y
PID for Z
Because these are orthogonal by nature, we can say each system is independent (and ideally it is), moving in the x direction shouldn't affect the Y direction. This example also completely ignores the bearing information (Vr), but it's meant to be a thought exercise, not a complete solution
The exact velocity of the corrections is determined by your PID coefficients and this is where things get a little tricky. Here is a easy to read (almost no math) overview or PID control. You will have to play with your system (aka "Tune" your parameters) through a bit of experimentation. This is made even more difficult because the camera is not a full 3d sensor, so we can't extract true measurements from the environment. It hard to convert an error of ~30 pixels into m/s without knowing more information about your sensor/environment, But I hope this gave you a general idea of how to proceed
anybody who know how to calculate the speed of hand gesture from hand tracking ?
(i'm using processing 1.5.1 with simpleopenNI 0.27)
Thanks for your attention
Speed is distance per unit time. Therefore, speed is:
distance_between_hand_in_consecutive_frames/(seconds_per_frame)
To find distance in 3d, use Euclidean distance with the position of the hand in consecutive frames.
EDIT: A psuedocode example.
f1 = get_current_frame_hand_coordinates()
f0 = get_previous_frame_hand_coordinates()
Then you need a function to calculate distance. Your input should be two tuples, here a and b, of size three, i.e. (x,y,z)
e_distance(a,b):
d = square_root( (a[0]-b[0])^2 + (a[1]-b[1])^2 + (a[2]-b[2])^2 )
return d
dist = e_distance(f0,f1)
Basically, here you just plug the tuple values into the equation. I'm not sure how your code is laid out, this is designed for a single set of tuples.
Now that you have the distance, then we just need to calculate the speed.
speed = distance/seconds_per_frame
Wikipedia says that the framerate of a Kinect is between 9 and 30 Hz. This means that your seconds_per_frame is between 1/9 and 1/30 of a second.
This will only give you the speed. Your question asks about speed (which only has magnitude), but you can also get velocity (which has both magnitude and direction) fairly easily with a little trig.
Do you know the app "motion ruler"? It can be used to get the length of an object by moving an iPhone.
How can I get the velocity from UIAcceleration when moving an iPhone?
Velocity is just acceleration * time. I.e. you can use the formula
v = u + acceleration * time.
As a starter you could look at assuming the initial speed is 0, 'u' can be ignored to get accelration * time.
if you want distance just use
distance = initial velocity * time + 0.5 * acceleration * time * time.
or better still use dead reckoning and keep adding your velocity vectors as frequently as you get updates from the accelerometers.
To get acceleration as a scalar you can do a = sqrt(x*x+y*y+z*z). You will want to remove gravity from this before though! Also this will only give you distance in straight line, you may prefer to measure velocity in x,y,z independently and use vector analysis to measure the path for a more accurate reading. Be warned though, you may also need to take into account any rotations to be super accurate and this feature is only on iPhone4s. This can get tricky and you'll probably want to use some matrix maths to orientate the 'space' the phone is in.
You will also need to do a low pass filter on the accelerometer to get rid of the 'shakes' and this is where a significant error margin will creep in and why I'd expect all rulers in appstore have spent a long time refining this.
to do a filter, a very simple one is filteredValue = ((lastValue * n) + accelerationValue )/(n+1) where a larger n value gives a smoother result but less responsive.
I have a rotating sphere that the user rotates by applying a virtual force, like a virtual accelerator. I want to be able to simulate a nice momentum effect so that when they lift off the accelerator the ball winds down in speed in a natural and realistic way, as if due to friction and/or gravity. I don't want to get into any deep physics equations. I'd like to do this quick so if I could find a code sample that shows how to do this, or even a page of formulas clear enough that I could encode, that would be great.
I'd like a formula that has one or two adjustable coefficients that I could tune to make the ball decelerate faster or slower depending on my needs. I don't want to get into anything heavy like an open source physics library or the like. Just something simple.
I'm using Delphi 6 Pro but I also know C/C++, Basic, Java, and Javascript.
Velocity is change in displacement. Acceleration is change in velocity.
Gravity or friction just causes an acceleration (possibly negative).
So all you need to do is apply a negative acceleration when they do not activate the accelerator.
So lets assume you have an angle that is changing. Applying the accelerator increases the amount the angle changes by each iteration or time step. If your angle is t and your change in angle is called dt (angular velocity) then when the accelerator is applied you'll have:
t = t + dt
dt = dt + a
where a depends on how much force you're applying, or how much they've 'pressed' the accelerator (i.e. this is the acceleration).
You'll probably want to limit dt (i.e. speed of rotation) - if you only want to spin in one direction you'll have an upper positive limit and a lower limit of 0. If you want both directions, you can have a lower negative limit and an upper positive limit.
All you need to do is make a some negative number when the accelerator is not applied (if dt is positive - make a positive if dt is negative), and ensure you don't 'wrap' (i.e. make dt 0 when it gets near 0).
It has been some time ago, but according to my dynamics study books the Mass Moment of Inertia for a sphere is defined as I=(2/5)m*r^2. m is the mass, r is the radius of the sphere (all in SI untis). On this page you'll find some examples to use the mass moment of inertia to calculate deceleration of the sphere as a result of the applied negative torque. This toqrue is a result of the friction. Since you are not defining the material of the surface of the sphere and the surroudings you cannot calculate the friction and will have to choose a good force yourself.
As long as you are not solving stellar problems, I don't see how gravity has much to do with decelerating the rotation.
Friction is almost proportional to the current rotation speed (actually the speed on the surface of the sphere).
So a formula for the current rotation speed over time w(t) may be something like this:
w(t) = w0*exp(-c*(t - t0))
with t0 being the time the friction starts, wt being the rotation speed at that time.
The coefficient c > 0 determines how fast the speed will decrease - the higher c, the faster the speed will go down. This formula is valid for all t >= t0.
Note that for t = t0 the exp function returns 1 and you get the initial speed, while for t -> ∞ the exp function (and so the resulting speed) returns -> 0 (the minus in front of the c guarantees this).
You've already accepted an answer, but I'll put in my piece.
I'll assume you already know how to make the sphere spin at a constant rate, and how to make it accelerate under the applied torque. Making it decellerate is just a matter of applying another torque, one that must be calculated.
When an object slides on a solid surface, the rate of deceleration is constant. The force is in the direction opposite to motion, and its magnitude depends on a couple of things but not speed. When the object comes to a full stop, the force vanishes. The same applies to a globe turning on a solid pivot.
When an object passes through a fluid, the force of decelleration increases with speed, so the faster the object the greater the drag. As the object slows down, decelleration grows weaker, and the object keeps slowing down but never stops. This describes a globe spinning in air or water. At reasonably high speeds, drag is proporional to v2, and at very low speeds it is proportional to v (I don't know about the transition between these domains).
So I suggest t = -a wb, where w is angular velocity. The parameter a is the strength of the friction, and b describes the kind of decelleration; b=0 is like friction on a solid pivot, b=2 is like spinning in air, and b=1 is like rotating in syrup. Other values of b may or may not look realistic or be realistic.
I'm implementing a 2D game with ships in space.
In order to do it, I'm using LÖVE, which wraps Box2D with Lua. But I believe that my question can be answered by anyone with a greater understanding of physics than myself - so pseudo code is accepted as a response.
My problem is that I don't know how to move my spaceships properly on a 2D physics-enabled world. More concretely:
A ship of mass m is located at an initial position {x, y}. It has an initial velocity vector of {vx, vy} (can be {0,0}).
The objective is a point in {xo,yo}. The ship has to reach the objective having a velocity of {vxo, vyo} (or near it), following the shortest trajectory.
There's a function called update(dt) that is called frequently (i.e. 30 times per second). On this function, the ship can modify its position and trajectory, by applying "impulses" to itself. The magnitude of the impulses is binary: you can either apply it in a given direction, or not to apply it at all). In code, it looks like this:
function Ship:update(dt)
m = self:getMass()
x,y = self:getPosition()
vx,vy = self:getLinearVelocity()
xo,yo = self:getTargetPosition()
vxo,vyo = self:getTargetVelocity()
thrust = self:getThrust()
if(???)
angle = ???
self:applyImpulse(math.sin(angle)*thrust, math.cos(angle)*thrust))
end
end
The first ??? is there to indicate that in some occasions (I guess) it would be better to "not to impulse" and leave the ship "drift". The second ??? part consists on how to calculate the impulse angle on a given dt.
We are in space, so we can ignore things like air friction.
Although it would be very nice, I'm not looking for someone to code this for me; I put the code there so my problem is clearly understood.
What I need is an strategy - a way of attacking this. I know some basic physics, but I'm no expert. For example, does this problem have a name? That sort of thing.
Thanks a lot.
EDIT: Beta provided a valid strategy for this and Judge kindly implemented it directly in LÖVE, in the comments.
EDIT2: After more googling I also found openSteer. It's on C++, but it does what I pretended. It will probably be helpful to anyone reaching this question.
It's called motion planning, and it's not trivial.
Here's a simple way to get a non-optimal trajectory:
Stop. Apply thrust opposite to the direction of velocity until velocity is zero.
Calculate the last leg, which will be the opposite of the first, a steady thrust from a standing start that gets the ship to x0 and v0. The starting point will be at a distance of |v0|^2/(2*thrust) from x0.
Get to that starting point (and then make the last leg). Getting from one standing point to another is easy: thrust toward it until you're halfway there, then thrust backward until you stop.
If you want a quick and dirty approach to an optimal trajectory, you could use an iterative approach: Start with the non-optimal approach, above; that's just a time sequence of thrust angles. Now try doing little variations of that sequence, keeping a population of sequences that get close to the goal. reject the worst, experiment with the best -- if you're feeling bold you could make this a genetic algorithm -- and with luck it will start to round the corners.
If you want the exact answer, use the calculus of variations. I'll take a crack at that, and if I succeed I'll post the answer here.
EDIT: Here's the exact solution to a simpler problem.
Suppose instead of a thrust that we can point in any direction, we have four fixed thrusters pointing in the {+X, +Y, -X, -Y} directions. At any given time we will firing at most one of the +/-X and at most one of the +/-Y (there's no point in firing +x and -X at the same time). So now the X and Y problems are independent (they aren't in the original problem because thrust must be shared between X and Y). We must now solve the 1-D problem -- and apply it twice.
It turns out the best trajectory involves thrusting in one direction, then the other, and not going back to the first one again. (Coasting is useful only if the other axis's solution will take longer than yours so you have time to kill.) Solve the velocity problem first: suppose (WLOG) that your target velocity is greater than your initial velocity. To reach the target velocity you will need a period of thrust (+) of duration
T = (Vf - Vi)/a
(I'm using Vf: final velocity, Vi: initial velocity, a: magnitude of thrust.)
We notice that if that's all we do, the location won't come out right. The actual final location will be
X = (Vi + Vf)T/2
So we have to add a correction of
D = Xf - X = Xf -(Vi+Vf)T/2
Now to make the location come out right, we add a period of thrust in one direction before that, and an equal period in the opposite direction after. This will leave the final velocity undisturbed, but give us some displacement. If the duration of this first period (and the third) is t, then the displacement we get from it is
d = +/-(at^2 + atT)
The +/- depends on whether we thrust + then -, or - then +. Suppose it's +.
We solve the quadratic:
t = (-aT + sqrt(a^2 T^2 + 4 a D))/2a
And we're done.
In the absence of additional info, we can assume there are 3 forces acting upon the spaceship and eventually dictating its trajectory:
"impulses" : [user/program-controlled] force.
The user (or program) appear to have full control over this, i.e. it controls the direction of the impulse and its thrust (probably within a 0-to-max range)
some external force: call it gravity, whatever...
Such force could be driven by several sources but we're just interested in the resulting combined force: at a given time and space this external force acts upon the ship with a given strengh and direction. The user/program has no control over these.
inertia: this is related to the ship's current velocity and direction. This force generally causes the ship to continue in its current direction at its current speed. There may be other [space-age] parameters controlling the inertia but generally, it is proportional to both velocity and to the ship's mass (Intuitively, it will be easier to bring a ship to a stop if its current velocity is smaller and/or if its mass is smaller)
Apparently the user/program only controls (within limits) the first force.
It is unclear, from the question, whether the problem at hand is:
[Problem A] to write a program which discovers the dynamics of the system (and/or adapts to changes these dynamics).
or..
[Problem B] to suggest a model -a formula- which can be used to compute the combined force eventually applied to the ship: the "weighed" sum of the user-controlled impulse and the other two system/physics-driven forces.
The latter question, Problem B, is more readily and succinctly explained, so let's suggest the following model:
Constant Parameters:
ExternalForceX = strength of the external force in the X direction
ExternalForceY = id. Y direction
MassOfShip = coeficient controlling
Variable Parameters:
ImpulseAngle = direction of impulse
ImpulseThrust = force of thrust
Formula:
Vx[new] = (cos(ImpulseAngle) * ImpulseThrust) + ExternalForceX + (MassOfShip * Vx[current])
Vy[new] = (sin(ImpulseAngle) * ImpulseThrust) + ExternalForceY + (MassOfShip * Vy[current])
Note that the above model assumes a constant External force (constant both in terms of its strength and direction); that is: akin to that of a gravitational field relatively distant from the area displayed (just like say the Earth gravity, considered within the span of a football field). If the scale of the displayed area is big relative to the source(s) of external forces, the middle term of the formulas above should then be modified to include: a trigonometric factor based on the angle between the center of the source and the current position and/or a [reversely] proportional factor based on the distance between the center of the source and the current position.
Similarly, the Ship's mass is assumed to remain constant, it could well be a variable, based say on the mass of the Ship when empty, to which the weight of fuel is removed/added as the game progresses.
Now... All the above assume that the dynamics of the system are controlled by the game designer: essentially choosing a set of values for the parameter mentioned and possibly adding a bit of complexity in the math of the formula (and also ensuring proper scaling to generally "keep" the ship within the display area).
What if instead, the system dynamics were readily programmed into the game (and assumed to be hidden/random), and the task at hand is to write a program which will progressively decide the direction and thrust value of the impulses to drive the ship to its targeted destination, in a way that its velocity at the target be as close as possible to getTargetVelocity()? This is the "Problem A".
This type of problem can be tackled with a PID Controller. In a nuthell, such a controller "decides" which amount of action (in this game's case = which impulse angle and amount of thrust to apply), based on three, weighed, factors, loosely defined below:
how far-off we are the current values from "set point": this is the P=Proportional part of PID
how fast are we approaching the "set point": this is the D=Derivative part of PID
how long and how much have we been away from the "set point": this is the I=Intergral part of PID
A less sophisticated controller could for example only use the proportional factor. This would result in oscillating, sometimes with much amplitude on either side of the set point ("I'm X units away from where I'm supposed to be: let me yank the steering wheel and press on gas"). Such overshooting of the set point are tempered by the Derivative factor ("Yeah, I'm still not where I'm supposed to be but the progress I made since the last time I check is very big: better slow down a bit"). Finally the Integral part takes into account the fact that all things being equal with regards to the combined Proportional and Derivative part, a smaller or bigger action would be appropriate depending on whether we've been "off-track" for a long time or not and of much off track we've been all this time (eg. "Lately we've been tracking rather close to where we're supposed to be, no point in making rash moves")
We can discuss the details implementing PID controllers for the specific needs of the space ship game, if that is effectively what is required. The idea was to provide a flavor of what can be done.
To just get from the current position to the destination with an initial velocity, then apply thrust along the normalized difference between the shortest path and the current velocity. You don't actually need the angle.
-- shortest path minus initial velocity
dx,dy = x0 - x - vx, y0 - y - vy
-- normalize the direction vector
magnitude = sqrt(dx*dx + dy*dy)
dx,dy = dx/magnitude, dy/mangitude
-- apply the thrust in the direction we just calculated
self:applyImpulse(thrust*dx, thrust*dy)
Note that this does not take the target velocity into account because that gets extremely complicated.
I have a very small Lua module for handling 2D vectors in this paste bin. You are welcome to use it. The code above would reduce to:
d = destination - position - velocity
d:normalize()
d = d * thrust
self:applyImpulse(d.x, d.y)
Are you expelling fuel? Your mass will change with time if you are.
Thrust is a reactive force. It's the rate of change of mass, times the speed of the exhaust relative to the spaceship.
Do you have external forces? If you do, these need to enter into your impulse calculation.
Let's assume a magical thrust with no mass being expelled, and no external forces.
Impulse has units of momentum. It's the integral of a force over time.
First off, you'll need to figure out exactly what the API calls "thrust" and impulse. If you're feeding it a thrust multiplied by a scalar (number), then applyImpulse has to do something else to your input to be able to use it as an impulse, because the units don't match up.
Assuming your "thrust" is a force, then you multiply that thrust by the time interval (1/30 second) to get the impulse, and break out the components.
Don't know if I'm answering your question, but hopefully that helps you to understand the physics a bit.
It's easier to think about if you separate the ship's velocity into components, parallel and perpendicular to the target velocity vector.
Considering along the perpendicular axis, the ship wants to come in line with the target position as soon as possible, and then stay there.
Along the parallel axis, it should be accelerating in whatever direction will bring it close to the target velocity. (Obviously if that acceleration takes it away from the target point, you'll need to decide what to do. Fly past the point and then double-back?)
I would deal with the two of them separately, and probably perpendicular first. Once it's working, and if that doesn't prove nice enough, you can start to think about if there are ways to get the ship to fire intelligent angles between perpendicular and parallel.
(EDIT: also, I forgot to mention, this will take some adjustment to deal with the scenario where you are offset a lot in the perpendicular direction but not much in the parallel direction. The important lesson here is to take the components, which gives you useful numbers on which to base a decision.)
Your angle is the Inverse Tangent the Opposite/Adjacent
So angle = InvTan(VY/VX)
No sure what your are talking about concerning wanting to drift??