As I’m working on a Raspberry Pi Zero W based robot, doing my first tests on Python programming, I needed to port the function created previously in this article for Arduino (c++).
This time though, I added the capability to provide the limits for the input and the output on any range you need it. I’ll explain why in the next steps.
Step 1: Joystick Input
Let’s suppose you have an input that is like a joystick, so the x, and y values provided by the joystick will vary between -1 and 1, as can be seen in the diagram.
As can be seen, the Joystick send us a 1,1 on the top right corner, a -1, -1 on the bottom, left, 0, 0 in the center, and so on.
This code will convert those x, y values to the corresponding speeds you need to apply to each wheel of your differential driven robot.
Step 2: Differential Drive Output
As said, we need to convert the pair of input values, to something we can use to drive a Differential drive (aka tank drive) robot. You can check in the image the main idea of what we need to achieve.
Step 3: Source Code
def joystickToDiff(x, y, minJoystick, maxJoystick, minSpeed, maxSpeed):<br> # If x and y are 0, then there is not much to calculate... if x == 0 and y == 0: return (0, 0) # First Compute the angle in deg # First hypotenuse z = math.sqrt(x * x + y * y) # angle in radians rad = math.acos(math.fabs(x) / z) # and in degrees angle = rad * 180 / math.pi # Now angle indicates the measure of turn # Along a straight line, with an angle o, the turn co-efficient is same # this applies for angles between 0-90, with angle 0 the coeff is -1 # with angle 45, the co-efficient is 0 and with angle 90, it is 1 tcoeff = -1 + (angle / 90) * 2 turn = tcoeff * math.fabs(math.fabs(y) - math.fabs(x)) turn = round(turn * 100, 0) / 100 # And max of y or x is the movement mov = max(math.fabs(y), math.fabs(x)) # First and third quadrant if (x >= 0 and y >= 0) or (x < 0 and y < 0): rawLeft = mov rawRight = turn else: rawRight = mov rawLeft = turn # Reverse polarity if y < 0: rawLeft = 0 - rawLeft rawRight = 0 - rawRight # minJoystick, maxJoystick, minSpeed, maxSpeed # Map the values onto the defined rang rightOut = map(rawRight, minJoystick, maxJoystick, minSpeed, maxSpeed) leftOut = map(rawLeft, minJoystick, maxJoystick, minSpeed, maxSpeed) return (rightOut, leftOut)
def map(v, in_min, in_max, out_min, out_max): # Check that the value is at least in_min if v < in_min: v = in_min # Check that the value is at most in_max if v > in_max: v = in_max return (v - in_min) * (out_max - out_min) // (in_max - in_min) + out_min
I added as many comments as I could, trying to make everything clear.
So, what are the differences with the Arduino version?
Just because I think this is more accurate to the purpose of the function
It returns a tuple
So you just get the right and left speeds on it
The joystick and wheel speed limits:
This change is the most important. In reality, I´m not using a joystick to control the robot, but my cellphone´s accelerometer, so, thanks to this parameters, I can reconfigure the limits of the input. In case of a joystick might be -1 to 1, but in case of the accelerometer, it goes from -9.8 to 9.8 (the gravity)
Also, I wanted the output to be configurable with the minSpeed, maxSpeed parameters. This allows me to just get the range of values I need for my servo controller (in my case -4095 to 4095, but you can set whatever speed range you want there, and the output will be scaled accordingly)
What about that map function?
If you are an Arduino programmer, you might know it already, since I port it from there. It is used to map an input value from the input range limits, to the output range limits. As I´m not aware of it existing in python, I just added to my utils functions. The only difference with the Arduino version, is that I added a constraint to the value, so it cannot be smaller or bigger than the input range specified.
I hope this makes sense, and if it doesn’t, please let me know. I promise I’ll do my best to make it more clear. You can find more about this article and the rest of the robot in my blog at savagemakers.com