2
$\begingroup$

Currently my setup is:

  • 2x hobby TT motors connected to a Pi Zero with a motor HAT
  • Python script running on the Pi which runs a Blynk python client and controls motors via the gpiozero library
  • Blynk iOS app using the joystick widget
  • Motors are attached on either side of a 'homebrew' chassis at the rear, parallel to each other, with a free-rotating wheel at the front.

What I want to do is to smoothly control motor direction & speed using the joystick values. I have smooth control over speed forward / backwards on the y axis, but want to somehow interpolate x & y for fine control of direction also.

Something like this math.stackexchange question is what I'm looking for, but I have no idea how to convert the accepted answer into code.

$\endgroup$
1
$\begingroup$

Converting a joystick $(x,y)$ into differential-drive $(left, right)$ control is not difficult. But there are multiple approaches because of different robot and joystick characteristics. For example, compare the differences between a "playstation-style" controller joystick (which will usually be used in an all-or-nothing kind of way and might have a sloppy dead-band near the center) vs. a large PC gaming flight simulator joystick (which gives you much finer grained control). Or a narrow 2-wheeled robot (which will rotate in place very fast) vs. a wide, tracked, skid-steer robot (which has lots of rotational friction). So you can see how you will need to tweak the feel of the controller.

This simplest procedure falls out of the discovery that if you just rotated the joystick 45 degrees, then you can use the joystick's $x$ and $y$ channels directly as the $left$ and $right$ wheel controls. For example, with the joystick all the way to the upper right corner $(1, 1)$, this is both wheels full speed ahead. And with the joystick to the upper left corner $(-1, 1)$, this is a full-speed turn in place with left wheel going backwards and the right wheel going forwards. In other words:

$$ \begin{bmatrix} left \\ right \\ \end{bmatrix} = \begin{bmatrix} cos(-45^o) && -sin(-45^o)\\ sin(-45^o) && cos(-45^o) \\ \end{bmatrix} \begin{bmatrix} x_{joy} \\ y_{joy} \\ \end{bmatrix} $$

Or in Python code:

def joy_to_diff_drive(joy_x, joy_y):
    left = joy_x * math.sqrt(2.0)/2.0 + joy_y * math.sqrt(2.0)/2.0
    right = -joy_x * math.sqrt(2.0)/2.0 + joy_y * math.sqrt(2.0)/2.0
return [left, right]

But if you use this, you will notice something strange. The robot will go faster when turning than when going straight. For example if your joystick axes go between -127 and 127, when going straight they will only be commanded to be 90, and when you put the joystick in a corner to turn with only 1 wheel, you will get a motor command of 180! Of course you could scale and cap the output to achieve the desired behavior, but the root of the problem is what is described in the post you mention here: https://math.stackexchange.com/questions/553478/how-covert-joysitck-x-y-coordinates-to-robot-motor-speed. I also discuss it in this post: How to program a three wheel Omni?. The problem is that the joystick range is square, but you really want it to be circular.

The easiest way to think about the problem from here on is in polar coordinates. Some Python code again:

# convert to polar coordinates
theta = math.atan2(joy_y, joy_x)
r = math.sqrt(joy_x * joy_x + joy_y * joy_y)

Then, let's determine the maximum "r" for the given theta. (Actually, the math for this is quite simple using "similar triangles", no trig required.) This let's us determine the actual speed.

# this is the maximum r for a given angle
if abs(joy_x) > abs(joy_y):
    max_r = abs(r / joy_x)
else:
    max_r = abs(r / joy_y)

# this is the actual throttle  
magnitude = r / max_r

Next we turn this into left and right wheel velocities. Now we'll use theta instead of the raw joystick left and right. But we do a similar rotation matrix thing.

turn_damping = 3.0 # for example
left = magnitude * (math.sin(theta) + math.cos(theta) / turn_damping)
right = magnitude * (math.sin(theta) - math.cos(theta) / turn_damping)

As you can see, I threw in something I alluded to earlier. For many robots, turning in place with full speed is way too fast, so you can turn it down with a damping factor.

Actually, the astute reader will find that for low values of "turn_damping", the output can go above the input. So some capping may still be required. I haven't figured out how to get rid of this yet. I believe it is because we really have an oval space instead of a circle, and this must be taken into account when calculating the magnitude.

| improve this answer | |
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.