Omnidirectional Robot Code

VEX V5 General Discussion Technical Discussion
1

I wrote some code for a three-wheeled robot using the omnidirectional wheels, and thought that I’d help out anyone who wants to code something like that. I know that I struggled for a while trying to figure it all out, especially when I couldn’t get the atan() function to work, and so I found another solution that I thought I should share.

#pragma config(Motor, port2, lMotor, tmotorVex393_MC29, openLoop)                  // motor at the top left of the triangle
#pragma config(Motor, port3, rMotor, tmotorVex393_MC29, openLoop)                  // motor at the top right of the triangle
#pragma config(Motor, port4, bMotor, tmotorVex393_MC29, openLoop)                  // motor at the bottom of the triangle

///////////////////////////////////////////////////////////////////////////////////
//                                                                               //
//  this is some relatively simple code to operate a three-wheeled kiwibot with  //
//  omnidirectional wheels at 120 degree angles to each other. it uses the left  //
//  joystick for movement in two dimensions and the right joystick for rotation  //
//                                                                               //
///////////////////////////////////////////////////////////////////////////////////

task main() {
  while(true) {
    int threshold = 15;                                                            // a threshold and some variable logic to keep the motors stopped near zero. //
    int rValue = fabs(vexRT[Ch1]) <= threshold ? 0 : vexRT[Ch1];                   // it is always a good idea to do this, as the vex controllers rarely output //
    int xValueRaw = fabs(vexRT[Ch4]) <= threshold ? 0 : vexRT[Ch4];                // a perfect zero when you let go of the sticks. this sort of variable logic //
    int yValueRaw = fabs(vexRT[Ch3]) <= threshold ? 0 : vexRT[Ch3];                // is essentially an if statement inside of where each variable is declared. //

    int maxSpeed = 127 - fabs(rValue);                                             // because the motors max out at 127, if you are going at full throttle and then //
    int xValue = xValueRaw * maxSpeed / 127;                                       // try to turn, you won't turn. this bit of code ensures that the speed variable //
    int yValue = yValueRaw * maxSpeed / 127;                                       // plus the rotation value will always be less than 127, so you can always turn. //

    motor[lMotor] = (1 / 2) * xValue + (sqrt(3) / 2) * yValue + rValue;            // this is a bit more complicated, but essentially, we're calculating the speeds of //
    motor[rMotor] = (1 / 2) * xValue - (sqrt(3) / 2) * yValue + rValue;            // each of the motors based on their respective angles to the body. you can look up //
    motor[bMotor] = (-1) * xValue + rValue;                                        // 'rotation of axes' on wikipedia if you want to understand what's happening here. //
  }
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                                           //
//  i wrote this code after trying to use the method that you always find online to code an omnidirectional  //
//  robot: decomposing the two input vectors into their magnitude and direction, and then recomposing those  //
//  parameters into the vectors of each of the wheels. but i just could not get the atan() function to work  //
//  right. i also realized that the problem was needlessly complicated and much harder on the computer than  //
//  it needed to be. this solution requires a bit more critical thinking, but is much more elegant, i think  //
//                                                                                                           //
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
2

I’m guessing you are referring to a kiwi drive.

We also did some work on that and can confirm that the other way is indeed quite complicated. From testing in desmos, the equations we have should work, but when applied to the robot it gets a bit wonky. (link to the desmos test) By wonky, I mean that it tends to swerve when driving and it rotates when strafing diagonally even when not feeding it a rotational input. (The swerving could almost certainly be fixed with PID, but we chose to take a break from the headache of kiwi drive code and have temporarily switched to an x-drive until our V5 stuff arrives.) The rotating while strafing diagonally does not seem to be from inaccuracy of the motors though. Does this solution fix this? Did you even have this problem in the first place? Are hotdogs sandwiches?

3

So, I didn’t have wonkiness as bad as what you’re describing, but i did still have a bit of the jank. A major issue with building a kiwibot with vex parts is that… well… vex’s omnidirectional wheels aren’t the best it’s hard for omnidirectional wheels, which have rollers at 90 degrees to the rotation axis, to work in a perfectly frictionless way when placed at 120 degrees to each other. When strafing at 90 degrees to the foreward direction, I had an issue where the robot would make these large arcs, rather than moving directly to the left or right. This is simply a problem with the wheels though, not the code. If you build a kiwibot with four wheels at 90 degrees to each other, and just have two listen to the y axis input, and two the x-axis, it drives perfectly, because the wheels are made for that.

So, as for your code. I’m not sure which types of motors you have on hand, but I know that the motors I had maxed out when given a value at or higher than 127. When you want your robot to rotate, you have to take that into account. For example, if I want my robot to go forward, but arc to the left while doing so, I’m going to have to make sure that the “going forward” value in my code and the “rotating to the right” value add together to always be less than 127, so as to make sure that everything runs correctly. That’s what the code is doing here:

    int maxSpeed = 127 - fabs(rValue);
    int xValue = xValueRaw * maxSpeed / 127;
    int yValue = yValueRaw * maxSpeed / 127;

Now that we understand that, we can tackle the issue of getting those darn motor vectors working the right way. Your Desmos code seems to be working correctly, but I think we both can agree that it’s a bit complicated (I have very little experience with Desmos though, so maybe I’m a bit biased there. A lot of that file is going over my head xP).

The “normal” way of doing a kiwi drive is to compose your input vectors into a single vector with a magnitude and an angle, and then to decompose that vector into the three smaller ones that add up to make it. That is hard, and also needlessly difficult for the computer to do. Think, why turn a number into an angle, only to turn it back again? What not just go straight to the final vector? Well, that’s what my code is doing. It’s a bit hard to describe right, but, for a given motor, you give it power equal to the sine of it’s angle to the body, times the x-input value, plus the negative cosine of its angle to the body, times the y-input value, all plus the rotational-input value (which I had mapped to the x-axis on the right stick).

If that’s confusing, try thinking about how you would program a “kiwibot” with four wheels, all perpendicular to each other, when “forwards” is 45 degrees off from two of the wheels (as in, the ‘forwards’ direction is in between two of the wheels, and all of the wheels are parallel to the one across from them, and perpendicular to the ones on either side). If that were the case, your code would have (1/sqrt(2)) and - (1/sqrt2)) being multiplied by the various x and y inputs for each of the motors. If you really want to get a good intuition for how this works, I invite you to actually build that robot, and just try to mess around with the last bits of my code, trying to make it work for the four wheels.

I can say that this code does work, as I built and tested it, and then extrapolated it out into code for robots with four, six, and eight wheels as well. The six and eight-wheeled robots didn’t move perfectly, because of the wheel-based issue i explained earlier, but the four-wheeled robot drove like a charm.

TLDR; No, I didn’t have the same jankiness that you’ve described, which is likely because you didn’t take the maximum values of the motors into account when rotating. Feel free to dissect my code, or, if you want the full mathematical intuition, just look it up on wikipedia. I wish you the best, and happy hacking!

4 
 To start, I want to clear some things up. In Vex, what you are describing as an omnidirectional chassis is normally called a holonomic drive. It's not that you are wrong, it's just that more people will understand what you mean if you use the common terminology. (Especially for this case, since we have wheels called omnidirectional wheels that are most commonly used on a simple tank drive) A 3 wheeled holonomic drive is called a kiwi drive and a 4 wheeled holonomic drive is called an x-drive. (I'm ignoring mechanum drive)

Now that we are using the same vocabulary, I'll start by saying that we are a senior team and that all of us can program well, so we know that the motors cap at ±127. I'm 99.9% certain motor capping is not our issue. When we first sat down to program our kiwi drive, we did some research and found the method you are using. From our testing, we had problems with the robot rotating while strafing diagonally. This is what prompted us to create the devilishly complicated mess that you saw on desmos. We knew that while strafing the code was already adjusting each wheel to the correct speed using the magnitude of our desired vector and the direction of our desired vector. We decided to add in our desired change in the rotation into the mix as well, and after throwing all the equations into a blender (the blender being wolfram-alpha) we ended up with a daunting set of solutions. (At least I'm pretty sure that's how this works. My teammate took lead on this project, so I'm a bit hazy on the details.)

 During this time we switched from our first prototype chassis to a newer design which looked somewhat like this:


Instead of a single front wheel, we went with 2 wheels chained together This was to add stability without taking up space with unpowered support wheels. This could very well be what caused our problem since the older prototype did not seem to have much of a problem with inconsistent driving. (It’s worth noting that due to an error present in the code at the time the turning was WAY too sensitive. It may be that the old chassis had the same problem but it went unnoticed because of the wacky steering.) Another possibility is that without any sort of control loop the 393 motors were simply too inconsistent to keep such an unconventional wheelbase traveling in a perfectly straight line. (It’s not a friction problem since it swerved each direction in nearly equal measures.) However, the aforementioned problem with rotating while strafing diagonally was clearly present in both versions of the chassis and across both versions of the code, and it was far too consistent to be caused by the motors. (The robot always turned left while strafing forward and left and always turned to the right while strafing forward and to the right. Oddly enough, while strafing directly left/right the robot maintained orientation without a hitch.) I’m really not sure what caused it since our efforts to fix it programmatically did not work out too well. We’ll try to fix it again once our V5 arrives, but until then we will put it on hold.

Side note: you mentioned that your robot moved in a large arc while strafing. From our testing, this came from our center of gravity being off-center. We managed to fix this problem by weighing each wheel so that our code could correct for this. This wasn't a huge problem in the first place, but hopefully that helps you somewhat.
5

Ah, I see that you know more than I do. I’m terribly sorry if I sounded condescending; I promise it wasn’t my intent. But I can see now that, whatever the problem is, it’s definitely beyond my knowledge base! All ideas of mine from here on out are complete speculation, and, as I no longer have access to my robot, I can’t experimentally confirm or deny anything.

My best guess:
The problem is in the omnidirectional wheels themselves. Because of the width of the rollers, a certain degree of friction appears when they’re going in any direction that isn’t along the path of the wheel or the path of the roller. As in, it looks like friction on a specific wheel will increase sinusoidally as the wheel rolls along 45 degrees out of phase with either perfectly forwards or perfectly sideways. This is why, when going perfectly sideways or forwards, our robots drive well, as the friction forces cancel out. However, when going along some diagonal, one wheel will be going “against the grain” a fair bit more than the other. Which explains why an x drive doesn’t have as many problems as any of the holonomic drives i built or the drive system you have: the friction always cancels out when all the wheels have a parallel opposite. I don’t know exactly how you would compensate for that in your code, but you guys seem pretty dang competent, so I’ll leave that to you to figure out. The only way I can think of to avoid this sort of problem mechanically is to basically invent a new type of omnidirectional wheel with a much smaller outer surface area, and much smaller rollers. For your code, you’ll probably have to experimentally figure out the frictional coefficient, and the exact nature of the sine wave that it’s being multiplied by at different angles. I wish you the best, and good luck.

6

I certainly didn’t want to come off as sounding like I know all there is to know about Vex. We all have room to improve. I just wanted to make sure you knew that we weren’t a new team without much experience. You seem to have done a great job in completing the task at hand, so don’t sell yourself short!

What you are saying about friction makes a lot of sense. We will have to look into that after our competition this weekend. And you are definitely right about the path we are taking being resource-intensive for the processor. Even if we can figure out how to account for all the forces at play, our code might just be too complex to be effective. Either way, it is a fun challenge to take on, so we certainly won’t give up just yet.

I’d like to thank you for taking the time to lend us your perspective. Having someone else to offer their own perspective is hugely helpful. We’ve been trying to “fix” our code for quite some time now, but we might not even be trying to solve the correct problem. If we can figure it out, we’ll definitely make a post to share our solution with the community. :slight_smile:

7

Last year, we had a similar code, except with an octogon shaped bot with four omnidirectional wheels.

8

Sounds like the robot needs ABS/traction control to monitor the current relative speeds of wheels as well as the standard omni drive code that sets the desired speeds of the wheels. Watching something like the weighted average difference-from-target-velocity would be useful. So say wheel A is going 70% of its target velocity, wheel B is going 72%, but wheel C is lagging at 55% of its target velocity. The “traction control” program would encourage C to accelerate harder and have A and B slow their acceleration (or even decelerate) to match C’s proportional velocity. The result would be an algorithm that prioritizes the direction of the aggregate velocity vector over the magnitude of the aggregate velocity vector.

As for the wonky steering, most omnidrive code assumes a center of mass that’s equidistant from each of the wheels, as well as that the axes of the wheels pass through the center of mass. That the center of mass lies along the wheels’ axes seems the most important assumption; if it’s violated then forces from those wheels will never be entirely rotational in nature, making the math much more complicated than it normally would be.

9

What do the “? 0” mean in lines 4-6 in the code? Is that some sort of boolean logic?

10

It is ternary operator. If you write x=(a?b:c); then “x” will be assigned value of “b” if “a” is true, or if “a” is false then “x” will be assigned value of “c”.

Also, please, don’t revive old topics.

2 Likes