Hello, this is my first post on here so I’m sorry that this might not have the right formatting.
Over the past few months I have been working on building and programming an x drive. I have not gone at a fast pace and I didn’t do thorough research into how to do it. I just saw it as a fun challenge and worked on it when I had time or felt motivated. How hard could it be?
Harder than expected which is why I am writing this.
I was inexperienced with coding and wanted to do fancy stuff. I wanted to use the inertial sensor to make the robot move based on the view of the driver. If I pushed up on the controller, I wanted the robot to move up from my perspective and not the robots and, by extension, I wanted to be able to spin and move in a given direction at the same time. When I originally started I was trying to work out mathematically how to make the robot go in a diagonal while spinning and from doing that, I unlocked driver-view controlling.
This explanation is mostly based on math and I used complex concepts such as a thorough understanding of sine funtions and a small amount of matrices. I didn’t try to make this simple. I don’t want to make this a math tutorial so if you don’t understand the math, google it. Not sure what to google? Leave a comment and I’ll drop some links to stuff that might help explain it.
The Fundamental Equations
With an x drive, it is pretty trivial to figure out how to go in 8 directions (up, down, left, right, and diagonals) so I started with a table like this:
Let me explain what some of those variables represent.
-
c is spin, everywhere you see a c in this explanation, it means spin.
-
ri stands for “initial angle” from when I was doing diagonal spinning, it just is the direction that I want the robot to go in. Later, the input from the controller will be put in as ri.
-
the 1, 2, 3, and 4 stand for the motors. 1 is top left, 2 is top right, 3 is bottom left, 4 is bottom right
-
the angles across the top are ra or “actual angle”. This is the angle that the robot is facing in with facing forward being 0. This will later be measured with the inertial sensor
The data shows what power values I have to give to which motors to move the robot in whatever direction ri is if the robot is facing in the top angle (ra). For example, the last column on the right shows that in order to move the robot forward (ri=0) when the robot is facing forward (ra=360), the 1st motor has to spin forward, the 2nd motor has to spin in reverse, the 3rd motor has to spin forward, and the 4th motor has to spin in reverse.
Now how do I calculate the values in between 0 (360) and 45 degrees?
At this point, I switched to desmos and I started to construct a sine function to fit my data.
These are my sine functions:
Of course, they aren’t perfect, I want to add more variables to them like c and ri and the scaling is weird, right now 360 tells each motor to spin at 70% power. So I made more tables (I included the 1st one for easy comparison):
As you can see, the difference between the 1st and 2nd table is just that each value is moved over 45. Like the column for 45 in the first table is the column for 90 in the 2nd table. This translates to a -ri in each of the equations.
The difference between the 2nd and 3rd table is more discrete, more interesting. On my table, 1 and -1 don’t mean literally 1, they just mean that the motor has to go forward or backward. A spin of 1 (c of 1) just makes the wheels spin forward more. Look at how it cycles in row 2 (for motor 1): 1,1,0,-1,0,1,1,1. C just moves the sine functions up and down. I also scaled the sine functions by sqrt2 in this step to make it so they are nice and give 1 or greater and not smaller values. It really doesn’t matter what I scale it by because I will rescale it later but just run with the sqrt2 for now.
So we have our basic, fundamental equations:
Inputs
In order to actually use these equations in our program, we have to change our inputs from our controller into ra, ri, and c.
What are the inputs?
-
a: up down from the controller (axis 2)
-
b: left right from the controller (axis 1)
-
c: left right from the second joystick on the controller (axis 4)
-
raw_gyro: just what I named the heading from the inertial sensor
And from those there is also another variable that we must add:
- f: the “force” with which the joystick is being pushed. This is just sqrt(a^2+b^2) using pythag.
We need f so we can give the motors more or less power depending on how much we are pushing the stick. To add this to the functions, we just multiply the sine parts by f.
Finding ri from a and b
We have to turn a and b into a direction to put into the functions. There are a few ways to do this and the first time I did it, I did it a super jank way that I am not going to discuss. Here is the nice, pretty way to do it using two arctan functions: arctan angle find
def find_ri(a, b):
global ri
if b<0:
ri=-math.atan(a/b)-math.pi/2
elif b>0:
ri=-math.atan(a/b)+math.pi/2
else:
if a<0:
ri=math.pi
else:
ri=0
Finding ra from raw_gyro
This one is really simple. I just converted it from going from 0 to 360 to going from -180 to 180 and then converted it to radians.
def find_ra(raw_gyro):
global ra
if raw_gyro>180:
ra= (raw_gyro-360)/180*math.pi
else:
ra= raw_gyro/180*math.pi
We can leave c alone for now but we’ll come back to it and f later.
And with that, it brings us to my jank blockly code that worked somehow even though I have been fixing stuff in it for months now.
https://www.robotmesh.com/studio/5fa6e70e3c6a8a1ceea79b80
Improvements
There are a few things I improved in the code. First off, I switched to python (that was fun) and figured out that I should use, without exception, radians throughout my code. That caused me pain. Then I fixed three main things:
-
Scaling the function outputs and making it neater with arrays
-
Fixing the jank ri finding code (it was so jank I’m not going to tell you about it)
-
Fixing the jank controller outputs
Scaling the Function Arrays
With the old code, it was made so that to spin, it gave the motors more than 100% power. A motor cannot go more than 100% so it automatically goes at 100% instead. With some motors scaling down and others not, the original ratio between the sine functions was disrupted. I had to find a way to scale the sine function array to keep the original ratios but still giving as much power as possible.
In order to figure out how to do this, I made another chart:
The orange spaces were easy to figure out. I just took the move array (which I am going to call a matrix because that is what I think of it as) and multiplied it by the f% and then added the c% * the spin matrix which is just 1,1,1,1. The spaces that add up to more than 100% were the tricky part. I realized that I wanted to scale c and f down so they added to 100 while also keeping the ratio between them constant. From there, I made this desmos and wrote this code:
https://www.robotmesh.com/studio/5fb7fd387ec24d0652fa775c
That code was simply a test to see if my equations for scaling c and f worked.
After getting the c and f scaling figured out, I needed to get the move matrix (the array from the sine functions) to always produce matrices with the max value in the matrix being 1. I wanted there to always be a 1. I wanted it to always give full power to one of the motors and never give more than 100% power ever. To do that, I just found the largest number in the matrix and divided it by that number. Any number divided by itself = 1 and the result is to just scale the other numbers up or down to preserve the ratios.
def scale_motor_sine_funtions(raw_m):
global c, f, new_m, actual_m
l = 0
new_m = []
actual_m = []
#finding largest value and dividing raw_m by it
for count in range(len(raw_m)):
if math.fabs(raw_m[count]) > math.fabs(l):
l = math.fabs(raw_m[count])
for motor_value in raw_m:
new_m.append(float(motor_value) / l)
#scale c and f if needed
if c+f > 100:
c = c/(f+c)*100.
f= 100-c
#update motor array
for motor_value in new_m:
actual_m.append(motor_value*f + c)
This code is what I use to add c and f into the equation in a careful and meticulous way.
Fixing the Jank Controller Outputs
So I had all that figured out, went to drive my bot, and it was jank.
The reason?
These are the values that my controller can produce:
IDK if it is obvious, but that is not a circle with radius 100 like I thought it was.
The angle finding software didn’t work right and my f was going all the way up to 130 at times.
I had to find a way to get that to give me a direction and a f.
After a few days of headbanging, anger, and bad solutions, I came up with a super slick way to just ignore that jank.
I made it so that if the f value was more than 100, I would just compress a and b so that f was 100. By doing this, I essentially made the controller give outputs in a 100 radius circle, which was what I wanted. I am really proud of how I solved this problem.
Here’s the desmos:
If you drag the black point around, the blue point always stays 100 away from the origin and is always at the correct angle. (unless you drag it to 0,0 but don’t worry about that)
Conclusion
I don’t know if you actually read all that but here is my final code:
https://www.robotmesh.com/studio/5fa9c9683c6a8a1ceea7ceda
rAN and rn are pretty self explanatory so I’m not going to go over them. Just know rN is “new angle” and rAN is “new actual angle”
I want to thank the vex discord, especially the people in #software. Over the course of this endeavour, they helped me a lot with all the questions I had.
I hope that this monologue was at least a little informative and entertaining and will help others program their x drives.
I plan to get the autonomous code figured out next so look out for that. I probably will take a while though because I want to, in the end, have a function where I just tell it to go to a point on the xy grid and have it automatically find where it is, figure out how to get to the point, and go there.
Thanks for reading! If you have questions, I’ll do my best to answer them and I will probably update this main post with more stuff. I probably made a stupid mistake someplace in here that I will have to fix. I’ve read and reread this post so many times. I have to let it go eventually.