I am currently stuck on the move to point function for an x drive. I have not fully tested this code, but from my understanding it will not work if the robot heading is not 0.
float XError;
float YError;
float KP = 0;
void moveTo(float X, float Y){
while (XPosition != X or YPosition != Y){
XError = X - XPosition;
YError = Y - YPosition;
float XMotorPower = XError * KP;
float YMotorPower = YError * KP;
FrontLeft.spin(forward,XMotorPower + YMotorPower,voltageUnits::volt);
FrontRight.spin(forward,YMotorPower - XMotorPower,voltageUnits::volt);
BackLeft.spin(forward,YMotorPower - XMotorPower,voltageUnits::volt);
BackRight.spin(forward,YMotorPower + XMotorPower,voltageUnits::volt);
wait(.02,seconds);
}
}
You have to make the robot move in a vector / code it to be in heading-less mode.
There’s quite a lot of dialogue in this thread going over how the math works for exactly this problem.
So I thought with the popularity of X-Drives this season. (And it being my favorite drive ) I’d release this fairly simple Desmos graph that calculates the exact percentage to spin each wheel at in order to move in a certain direction. (Measured in radians)
Each colored line shows which direction and how fast each wheel should spin to move in exactly the desired direction. The black dotted line shows the direction and how fast you would move. (Motor speeds are scaled for maximum speed) No…
1 Like
Thanks, I got the code finished. I should be able to test it relatively soon.