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 //
// //
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
```