Sparkfun OTOS with FTC inital pose always 0

Robotics
1

This may not be a post for Sparkfun directly, but I’m trying here since the sensor comes from here. I am a mentor for an FTC robotics team and we are working to configure the OTOS with our FTC robot. We had it on our robot last year and it worked great. I can’t seem to get an initial pose to get set though. We are setting the initial pose by doing the following:

MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(62,12,Math.toRadians(180)));

And then we pass RoadRunner the start of a new path by using the following:

Action initial_move = drive.actionBuilder(new Pose2d(62,12,Math.toRadians(180)))
    .lineToX(58)
    .turn(Math.toRadians(-20))
    .build();

When we start up, the robot always turns around 180 degrees and then runs the lineToX and Turn operations correctly but only after making a 180 turn to start. It doesn’t seem to matter what we set, the initial heading is always at 0. I can re-factor all my code and write it to always ‘think’ that we’re starting from 0 but that’s a lot of work to basically re-calculate everything (well, it’s really probably just putting negatives in from of everything and flipping the field).

Anyways, it’s annoying and it would be great if someone has thoughts of what might cause this.

2

It could be a few things….

1. OTOS might not be ready before the pose is being read…..try adding a delay after creating the drive object:

MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(62,12,Math.toRadians(180)));
sleep(100); // Give OTOS time to set the pose

Also check if there’s a localizer.update() call happening before the pose is set. You might need to set the pose after the localizer is created.

2. Ensure the pose is actually being set in the MecanumDrive Constructor:

public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {// … other initialization …

// Make sure this line exists and is being called:
setPosition(pose); // or setPose(pose) or similar

// Or if using OTOS directly:
otos.setPosition(new SparkFunOTOS.Pose2D(pose.position.x, pose.position.y, pose.heading.toDouble()));
}

3. Diag Test: Add this right after creating your drive object to see what’s happening:

MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(62,12,Math.toRadians(180)));
sleep(100);
telemetry.addData("Initial Heading", drive.pose.heading.toDouble());
telemetry.update();
sleep(2000); // Give yourself time to read it

If this shows 0 (or 3.14 radians if you’re starting at 180°), then the pose is being set. If it shows 0 when you expect 180°, the pose isn’t being set properly.

  1. Nuclear Option (Workaround)

    If nothing else works, you can set the pose explicitly after initialization:

    MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
drive.pose = new Pose2d(62,12,Math.toRadians(180));
// Or if you have a setPose method:
// drive.setPose(new Pose2d(62,12,Math.toRadians(180)));

    Most likely culprit: It’s usually issue #1 or #2. The OTOS needs that brief moment to initialize, or the pose-setting code isn’t being called in your constructor.

Let me know what you find, and I can help troubleshoot further!

3

Thanks, I did find out what was going on. The final solution was to pass the beginPose to the OTOS localizer. Last year, the method we used was structured differently so we didn’t have to do that, but now that we’re using the full RR directly from RR, we have to pass the localizer the begining pose with:

drive.localizer.setPose(beginPose);