Skip to content

Instantly share code, notes, and snippets.

@acetousk
Created April 21, 2020 19:00
Show Gist options
  • Save acetousk/3912bd4ba60f614fe989c5c6e4c1f6d0 to your computer and use it in GitHub Desktop.
Save acetousk/3912bd4ba60f614fe989c5c6e4c1f6d0 to your computer and use it in GitHub Desktop.
Example of OkapiLib AsyncLinearMotionProfileController for Forward and Straight Movements
#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;
}
#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};
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment