Move To Point Function

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.

1 Like

Thanks, I got the code finished. I should be able to test it relatively soon.