OkapiLib

Programming Support PROS Support
, ,
1

I cannot get the Odometry using OkapiLib to work when I have another task running after the objects are built. For example, if I run a task after the objects are built and I print out the values of the odometry continuously, it gives me 0 for x 0 for y and inf for angle. Is there a way to fix this?

2

Do you have any kind of delay in that other task? Can you post your code?

2 Likes
3

Yes I did have a delay. It was about 15 ms.

4

Can you please post your code?

5

typedef struct {
std::shared_ptr chassis;
} my_task_arg;

void updateAngle(void* param) {
while(true) {
auto odom = ((my_task_arg*) param) → chassis;
auto vals = odom → getState();
odom → setState({vals.x,vals.y,QAngle(degreeToRadian * gyroInertialSensor.get_heading())});
delay(50);
}

}

void opcontrol() {

using namespace okapi;


auto chassis =  ChassisControllerBuilder()

.withMotors(frontLeftMotor, frontRightMotor, backRightMotor, backLeftMotor)
.withMaxVelocity(150)
.withSensors(left, right, back)
.withGains(
{0.005, 0.0001, 0.0002}, // Distance controller gains
{0.006, 0, 0}, // Turn controller gains
{0.0025, 0.000, 0.000} // Angle controller gains (helps drive straight) (0.0025, 0, 0.0007)
)

 .withDimensions(AbstractMotor::gearset::green, {{3_in, 15.5_in}, imev5GreenTPR})

// / .withOdometryTimeUtilFactory(ConfigurableTimeUtilFactory())
.withOdometry({{2.75_in, 9.75_in, 6.3_in, 2.75_in}, quadEncoderTPR}, StateMode::CARTESIAN) // use the same scales as the chassis (above)
.buildOdometry(); // build an odometry chassis

auto myChassis = ChassisControllerBuilder()
	.withMotors(frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor)
	.withSensors(left, right, back)
	.withGains(
		 {0.005, 0.0001, 0.0002}, // Distance controller gains
		 {0.006, 0, 0}, // Turn controller gains
		 {0.0025, 0.000, 0.000}  // Angle controller gains (helps drive straight)
	)
	// Green gearset, 4 in wheel diam, 11.5 in wheel track
	.withDimensions(AbstractMotor::gearset::green, {{2.75_in, 13.5_in}, imev5GreenTPR})
	.build();

	auto profileControllerF = AsyncMotionProfileControllerBuilder()
		.withLimits({
			1.0,  // Maximum linear velocity of the Chassis in m/s
			2.0,  // Maximum linear acceleration of the Chassis in m/s^2
			10.0,  // Maximum linear jerk of the Chassis in m/s/s/s
		})

		.withOutput(myChassis)
		.buildMotionProfileController();
























	chassis -> setState({0_in, 0_in, 0_deg});

	my_task_arg* argument = new my_task_arg();
  argument->chassis = chassis;
  pros::Task my_task(updateAngle, argument);