Created
April 21, 2020 19:00
-
-
Save acetousk/3912bd4ba60f614fe989c5c6e4c1f6d0 to your computer and use it in GitHub Desktop.
Example of OkapiLib AsyncLinearMotionProfileController for Forward and Straight Movements
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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; | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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