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?
Do you have any kind of delay in that other task? Can you post your code?
Yes I did have a delay. It was about 15 ms.
Can you please post your code?
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);