Odometry Move To Position Help

Programming Support
#1

Hello programmers,

I want to move my robot to a specific point using odometry but the robot will keep running even after the robot has reached the target X value though this does not happen with the target Y value.

void goToPosition(double targetX, double targetY, double targetOrientation, double motorPower, double distanceError)
{
  TrackPOS();
   double xError = targetX - X;
   double yError = targetY - Y;
   double d = hypot(xError, yError); //distance

   while(d > distanceError)
   {
    TrackPOS();
    xError = targetX - X;
    yError = targetY - Y;
    d = hypot(xError, yError);
    //double theta = atan2(xError, yError)*(57.2958);
    double movementX = (sin(Theta))*motorPower; //outside of this method there is a double value called Theta with a capital T
    double movementY = (cos(Theta))*motorPower;
    double pivotCorrection = (inertialSensor.orientation(yaw, degrees)-targetOrientation);

    LM1.spin(fwd, movementY+movementX+pivotCorrection, velocityUnits(rpm));
    LM2.spin(fwd, movementY-movementX+pivotCorrection, velocityUnits(rpm));
    RM1.spin(fwd, movementY-movementX+pivotCorrection, velocityUnits(rpm));
    RM2.spin(fwd, movementY+movementX, velocityUnits(rpm));
    vex::task::sleep(20);
  }
}

Any help with this issue would be greatly appreciated, thank you.

#2

Can you elaborate a little more on this? Do you mean it stops when they value is correct? It could be that you don’t have enough angular correction to reduce the distance error however it’s hard to know without further information.

1 Like
#3

It works as intended when I set the motors to move only to the target Y coordinate like so:

LM1.spin(fwd, movementY, velocityUnits(rpm));
LM2.spin(fwd, movementY, velocityUnits(rpm));
RM1.spin(fwd, movementY, velocityUnits(rpm));
RM2.spin(fwd, movementY, velocityUnits(rpm));
#4

That’s not really a motion algorithm that will give you much success. It works if you just put in movementY because then it’ll just drive forward until it reaches the desired point. The real math for a holonomic (and I’m assuming this question is about a holonomic drive) is more complicated, and involves multiplying each of the wheel outputs by the cosine of the angle between it and the target point.

Look in the JAR-Template to see how we implemented holonomic drive to point. Line 328 in src/JAR-Template/drive.cpp

Hope this helps.

1 Like
#5

Thank you for the resource. I was wondering what is_settled in line 331 in in src/JAR-Template/drive.cpp does

#6

Sure! is_settled() is a member of the PID class, and you can see its definition in line 44 of src/JAR-Template/PID.cpp.

Essentially all it does is take a settling error (perhaps you might pick 1") and a settling time (say, 200ms). If the robot remains within that settling error for the duration of the settling time, then the is_settled() method returns true, and the drive function exits.

2 Likes