Issues with OKAPI odom

Programming Support
#1

Hey everyone, the code below doesn’t work as I want it to. Essentially, the last setState has 0 effect on the robot and the robot doesn’t move backward 1 foot; instead, it turns 90 degrees and then moves backward 1 foot. When I log the state, I get a ThreeEncoderOdometry Error (a tick diff was greater than the maximum allowable diff). Presumably, this would mean the encoders are bad, but if they were bad, none of the previous movements would work either. Thanks for any help.

std::shared_ptr<OdomChassisController> odomchas =
		ChassisControllerBuilder()
				.withMotors(DRIVE_FRONT_LEFT, DRIVE_FRONT_RIGHT, DRIVE_BACK_RIGHT, DRIVE_BACK_LEFT)
				//.withSensors(leftencoder, rightencoder, middleencoder)
				.withGains(
					{0.002, 0.00001, 0}, // Distance controller gains
					{0.007, 0, 0}, // Turn controller gains
					{0.002, 0, 0.00006}  // Angle controller gains (helps drive straight)
				 	)
				.withSensors(
					ADIEncoder{'G', 'H'},
					ADIEncoder{'C', 'D', true},
					ADIEncoder{'E', 'F'}
				)
				.withDimensions(AbstractMotor::gearset::green, {{2.75_in, 7_in, 1_in, 2.75_in}, quadEncoderTPR})
				.withOdometry()
				.buildOdometry();
void autonomous() {
	odomchas->setState({0_in,0_in,0_deg});
	// //initalize
	xModel->strafe(-50);
	pros::delay(600);
	xModel->strafe(0);
	//intake deploy
	left_intake.move_velocity(-100);
	right_intake.move_velocity(-100);
	indexer.move_velocity(-200);
	main_intake.move_velocity(200);
	odomchas->turnToAngle(-90_deg);

	//
	// //bottom right corner
	left_intake.move_velocity(200);
	right_intake.move_velocity(200);
	odomchas->turnToAngle(-90_deg);
	main_intake.move_velocity(200);
	indexer.move_velocity(-200);
	odomchas->setState({0_in,0_in,0_deg});
	odomchas->driveToPoint({2.25_ft, 0_ft});

	pros::delay(500);
	left_intake.move_velocity(0);
	right_intake.move_velocity(0);
	indexer.move_velocity(0);
	main_intake.move_velocity(0);
	odomchas->setState({0_in,0_in,0_deg});
	odomchas->driveToPoint({1_ft, 0_ft}, true);

}
1 Like
#2

What happens if you reset the encoders (either via the odomchs or odomchs->getModel(), I forget which) either before or after the call to odomchas->setState?

2 Likes
#3

Just FYI, Okapi is a word, not an acronym.

1 Like
#4

I used xModel->resetSensors() and nothing changed. Would that be the correct reset function?

#6

Yes, it sounds like you successfully tried what I suggested. Other suggestion would be to add a small pros::delay(50) (maybe as little as 20 maybe as much as 200) after the setState that is causing problems

4 Likes
#7

Just tried that… did nothing whatsoever–even made the delay 500. I even tried commenting out the whole block of code above it and it still performed the 90 degree turn. I commented out even the setState part and it still performed the turn. Its like its inbuilt or something lol.

#8

The true parameter here means to drive in reverse, so the second part of your statement:

Make sense.

And

Should result in the robot making forward movements of different lengths.

What happens if you print out the results of odomchas->getState() before and after each of the driveToPoint calls?

3 Likes
#9

encodervalues2

This is what I’m getting (sorry for the bad formatting lol). It seems like the 2nd setState isn’t working perfectly (not setting the states to exactly 0).

I put the getState() statement right before and after each driveToPoint statement

#10

The odometry thread keeps track of the sensor values from the previous iteration. I wonder if you:

odomchas->setState({0_in,0_in,0_deg});  // This state may drift slightly during the next few statements
xModel->resetSensors(); // Zero out the odometry sensors
pros::delay(30);  // This should wait for at least 2 loops of the 10ms odom thread (depending on thread schedules)
xModel->resetSensors(); // This may not be necessary
odomchas->setState({0_in,0_in,0_deg}); // This state shouldn't drift

I did have a hard time following the output you posted. Could you add new-lines between each output and maybe a descriptor?

1 Like
#11 
odomchas->setState({0_in,0_in,0_deg});
	
    printf( odomchas->getState().str().c_str());
	odomchas->driveToPoint({1_ft, 0_ft});
	printf(odomchas->getState().str().c_str());

	left_intake.move_velocity(0);
	right_intake.move_velocity(0);
	indexer.move_velocity(0);
	main_intake.move_velocity(0);

	odomchas->setState({0_in,0_in,0_deg});
	pros::delay(500);
	
    printf(odomchas->getState().str().c_str());
	odomchas->driveToPoint({1_ft, 0_ft}, true);
	printf(odomchas->getState().str().c_str());

Here is where I have placed each of the output statements. I placed one before/after the first driveToPoint, and one before/after the second driveToPoint. The outputs in the screenshot above are in order according to the code. The first getState() gave us all 0’s, which is expected since I have a setState(0,0,0) right above that. The second get state gives us the position of the robot after I instructed it to move forward 1 foot, giving us 0.29 m which is roughly a foot. The third get state gives us values very close to 0, but not exactly 0 , which maybe a problem since I setState to all 0’s right above it. The last getState is after the driveToPoint (where the bot goes haywire and turns 90 degrees and backs up), giving us erroneous values.

The odomstates would be read like this:

Odomstate(x=0, y=0, theta=0), 
//drive to point () #1
Odomstate(x-0.29, y=-0.023, theta = -0.19)

Odomstate(x=0.000305, y=-0.0000088m, theta = -0.19642 deg)
//drive to point() #2
Odomstate(x=0.404060, y=-0.055934m, theta = -177.767857 deg)
#12

I also just tried your suggestion of resetting the sensors setting states twice, and it didn’t work. Let me try checking whether xModel ->resetSensors() is actually working

update: resetsensors works, but I noticed that when I’m running the program Im getting an error telling me the encoder value was more the maximum allowable difference (1000), and skips the odometry step. it may signal a faulty encoder, but I think that if the encoders were bad then the previous steps wouldn’t work