# Using trigonometry to set motor speeds for a two-wheeled bot

Hey All~
Iceskier expressed an interest in this code, so I am posting it here and hoping that the zip file extracts okay. Sorry for the delay; I tried a couple of different ways to post it, so I’m really hoping that this one works!! Ideally, I had wanted to add some more graphics and author notes, but instead, I am simply posting a zip file, an image, and the following attempt at a lucid explanation:

My basic idea was simple: If the position of a joystick is forward and left of zero, we would expect that both wheels would be turning forward. However, because we are also to the left of center, the bot should be turning left. Therefore, the right wheel must be turning faster to make the bot actually turn left. I hypothesized that the relative velocities of the two wheels could be calculated using the Pythagorean Theorem.

Example 1
So… if we use the position of the joystick relative to the center point of a coordinate plane, then the altitude of a right triangle drawn to that point (the Y-value) can be used to set the velocity of the wheel on THAT side of the Y-axis. In the image below, note that the length of the solid line in triangle A corresponds to the velocity for the solid-outlined wheel on the left. The length of the dotted hypotenuse is the velocity for the dotted wheel, which is on the OTHER side of the Y-axis. Since the solid line is shorter than the dotted hypotenuse, the left wheel is actually turning slower than the right wheel, and the bot is turning left.

Example 2
Likewise, if the joystick were at position B, the length of the dotted altitude in triangle B would correspond to the velocity of the dotted wheel on THAT side of the Y-axis, while the solid-line, the hypotenuse, would be used to set the velocity of the wheel on the OTHER side of the Y-axis. In this example, the bot would be turning more sharply to the right because the joystick is farther to the right of center than point A was to the left of center in example 1.

Additionally, because velocities can be either positive or negative (such as when we are in quadrants 3 and 4) we can consistently instruct both drive motors to turn FORWARD at the assigned velocities and we will still get the expected behavior when the joystick is moved to all four quadrants. The fine print:
If you look at the attached code, you’ll see that I coded a variable for an independent brake for each motor that sets the velocity instantly to zero for the given motor. I also coded a NULL zone from -5 to 5 (basically a “near zero” on the X-axis) to make driving straight forward or backward a little more forgiving. The bot will simply go straight forward or backward at the Y-value velocity, even if you wiggle the joystick slightly to the left or right. I also added a NULL zone for the Y-axis. This sends the bot into a left or right spin if you are attempting to only move the stick side-to-side. This NULL zone on the Y-axis works pretty well to prevent accidental forward / backward movement of the joystick from causing the bot to unexpectedly “jitter.”

Whew! Not sure that I nailed this explanation, so please feel free to post questions, try the attached code, and, as always, to experiment on your own Thanks to Iceskier for asking and best o’ luck! 