Example of OkapiLib AsyncLinearMotionProfileController for Forward and Straight Movements

chassis.cpp

#include "chassis.hpp"

template <typename T> double sgn(T val) {
    return (T(0) < val) - (val < T(0));
}

void Chassis::linearProfileStraight(QLength idistance, QLength icurrentPos){
    leftProfileController->generatePath ( {icurrentPos.abs(),idistance.abs()}, "straight", straightLimits );
    rightProfileController->generatePath( {icurrentPos.abs(),idistance.abs()}, "straight", straightLimits );
    bool backward = false;
    if(sgn(idistance.convert(meter))==-1){
        backward = true;
    }
    leftProfileController->setTarget( "straight", backward);
    rightProfileController->setTarget("straight", backward);
    while(!linearProfileWaitTilSettled());
    leftProfileController ->removePath("straight");
    rightProfileController->removePath("straight");
}

void Chassis::linearProfileTurn(QAngle iangle, QLength icurrentPos){
    QLength turnLength = iangle.convert(radian) * (chassisScales.wheelTrack/2);
    leftProfileController->generatePath ( {icurrentPos.abs(),turnLength.abs()}, "turn", turnLimits );
    rightProfileController->generatePath( {icurrentPos.abs(),turnLength.abs()}, "turn", turnLimits );
    bool left = false;
    if(sgn(iangle.convert(radian))==-1){
        left = true;
    }
    leftProfileController->setTarget( "turn", left);
    rightProfileController->setTarget("turn", !left);
    pros::delay(10);
    while(!linearProfileWaitTilSettled());
    leftProfileController ->removePath("turn");
    rightProfileController->removePath("turn");
}

bool Chassis::linearProfileWaitTilSettled(){
    while(!leftProfileController->isSettled() && !rightProfileController->isSettled()){
        pros::delay(10);
    }
    return true;
}

chassis.hpp

#pragma once

#include "okapi/api.hpp"

#include <math.h>

using namespace okapi;
using namespace okapi::literals;

class Chassis{
    public:
    Chassis() = default;

    //odom
    std::shared_ptr<TwoEncoderOdometry> odom{nullptr};

    //chassis models
    std::shared_ptr<SkidSteerModel> skidSteerModel{nullptr};

    //profile controllers
    void linearProfileStraight(QLength idistance, QLength icurrentPos = 0_in);
    void linearProfileTurn(QAngle iangle, QLength icurrentPos = 0_in);
    bool linearProfileWaitTilSettled();
  
     //make sure to create these profile controllers before using them or else you will get a segfault when running the methods above
    std::shared_ptr<AsyncLinearMotionProfileController> leftProfileController{nullptr};
    std::shared_ptr<AsyncLinearMotionProfileController> rightProfileController{nullptr};

};