vex/2024code
19
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial commit - test serial

Browse Source
This commit is contained in:
Cole A. Deck
2024-03-24 22:20:00 -05:00
commit a4b1c1b7ed
273 changed files with 43716 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
SerialTest/include/okapi/api/control/async/asyncController.hpp Normal file
View File

@ -0,0 +1,24 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/closedLoopController.hpp"
namespace okapi {
/**
* Closed-loop controller that steps on its own in another thread and automatically writes to the
* output.
*/
template <typename Input, typename Output>
class AsyncController : public ClosedLoopController<Input, Output> {
public:
/**
* Blocks the current task until the controller has settled. Determining what settling means is
* implementation-dependent.
*/
virtual void waitUntilSettled() = 0;
};
} // namespace okapi

291
SerialTest/include/okapi/api/control/async/asyncLinearMotionProfileController.hpp Normal file
View File

@ -0,0 +1,291 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/async/asyncPositionController.hpp"
#include "okapi/api/control/util/pathfinderUtil.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/units/QAngularSpeed.hpp"
#include "okapi/api/units/QSpeed.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <atomic>
#include <map>
#include "squiggles.hpp"
namespace okapi {
class AsyncLinearMotionProfileController : public AsyncPositionController<std::string, double> {
public:
/**
* An Async Controller which generates and follows 1D motion profiles.
*
* @param itimeUtil The TimeUtil.
* @param ilimits The default limits.
* @param ioutput The output to write velocity targets to.
* @param idiameter The effective diameter for whatever the motor spins.
* @param ipair The gearset.
* @param ilogger The logger this instance will log to.
*/
AsyncLinearMotionProfileController(
const TimeUtil &itimeUtil,
const PathfinderLimits &ilimits,
const std::shared_ptr<ControllerOutput<double>> &ioutput,
const QLength &idiameter,
const AbstractMotor::GearsetRatioPair &ipair,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
AsyncLinearMotionProfileController(AsyncLinearMotionProfileController &&other) = delete;
AsyncLinearMotionProfileController &
operator=(AsyncLinearMotionProfileController &&other) = delete;
~AsyncLinearMotionProfileController() override;
/**
* Generates a path which intersects the given waypoints and saves it internally with a key of
* pathId. Call `executePath()` with the same `pathId` to run it.
*
* If the waypoints form a path which is impossible to achieve, an instance of
* `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
* are no waypoints, no path is generated.
*
* @param iwaypoints The waypoints to hit on the path.
* @param ipathId A unique identifier to save the path with.
*/
void generatePath(std::initializer_list<QLength> iwaypoints, const std::string &ipathId);
/**
* Generates a path which intersects the given waypoints and saves it internally with a key of
* pathId. Call `executePath()` with the same pathId to run it.
*
* If the waypoints form a path which is impossible to achieve, an instance of
* `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
* are no waypoints, no path is generated.
*
* @param iwaypoints The waypoints to hit on the path.
* @param ipathId A unique identifier to save the path with.
* @param ilimits The limits to use for this path only.
*/
void generatePath(std::initializer_list<QLength> iwaypoints,
const std::string &ipathId,
const PathfinderLimits &ilimits);
/**
* Removes a path and frees the memory it used. This function returns `true` if the path was
* either deleted or didn't exist in the first place. It returns `false` if the path could not be
* removed because it is running.
*
* @param ipathId A unique identifier for the path, previously passed to generatePath()
* @return `true` if the path no longer exists
*/
bool removePath(const std::string &ipathId);
/**
* Gets the identifiers of all paths saved in this `AsyncMotionProfileController`.
*
* @return The identifiers of all paths
*/
std::vector<std::string> getPaths();
/**
* Executes a path with the given ID. If there is no path matching the ID, the method will
* return. Any targets set while a path is being followed will be ignored.
*
* @param ipathId A unique identifier for the path, previously passed to `generatePath()`.
*/
void setTarget(std::string ipathId) override;
/**
* Executes a path with the given ID. If there is no path matching the ID, the method will
* return. Any targets set while a path is being followed will be ignored.
*
* @param ipathId A unique identifier for the path, previously passed to `generatePath()`.
* @param ibackwards Whether to follow the profile backwards.
*/
void setTarget(std::string ipathId, bool ibackwards);
/**
* Writes the value of the controller output. This method might be automatically called in another
* thread by the controller.
*
* This just calls `setTarget()`.
*/
void controllerSet(std::string ivalue) override;
/**
* Gets the last set target, or the default target if none was set.
*
* @return the last target
*/
std::string getTarget() override;
/**
* Gets the last set target, or the default target if none was set.
*
* @return the last target
*/
virtual std::string getTarget() const;
/**
* This is overridden to return the current path.
*
* @return The most recent value of the process variable.
*/
std::string getProcessValue() const override;
/**
* Blocks the current task until the controller has settled. This controller is settled when
* it has finished following a path. If no path is being followed, it is settled.
*/
void waitUntilSettled() override;
/**
* Generates a new path from the position (typically the current position) to the target and
* blocks until the controller has settled. Does not save the path which was generated.
*
* @param iposition The starting position.
* @param itarget The target position.
* @param ibackwards Whether to follow the profile backwards.
*/
void moveTo(const QLength &iposition, const QLength &itarget, bool ibackwards = false);
/**
* Generates a new path from the position (typically the current position) to the target and
* blocks until the controller has settled. Does not save the path which was generated.
*
* @param iposition The starting position.
* @param itarget The target position.
* @param ilimits The limits to use for this path only.
* @param ibackwards Whether to follow the profile backwards.
*/
void moveTo(const QLength &iposition,
const QLength &itarget,
const PathfinderLimits &ilimits,
bool ibackwards = false);
/**
* Returns the last error of the controller. Does not update when disabled. Returns zero if there
* is no path currently being followed.
*
* @return the last error
*/
double getError() const override;
/**
* Returns whether the controller has settled at the target. Determining what settling means is
* implementation-dependent.
*
* If the controller is disabled, this method must return `true`.
*
* @return whether the controller is settled
*/
bool isSettled() override;
/**
* Resets the controller's internal state so it is similar to when it was first initialized, while
* keeping any user-configured information. This implementation also stops movement.
*/
void reset() override;
/**
* Changes whether the controller is off or on. Turning the controller on after it was off will
* NOT cause the controller to move to its last set target.
*/
void flipDisable() override;
/**
* Sets whether the controller is off or on. Turning the controller on after it was off will
* NOT cause the controller to move to its last set target, unless it was reset in that time.
*
* @param iisDisabled whether the controller is disabled
*/
void flipDisable(bool iisDisabled) override;
/**
* Returns whether the controller is currently disabled.
*
* @return whether the controller is currently disabled
*/
bool isDisabled() const override;
/**
* This implementation does nothing because the API always requires the starting position to be
* specified.
*/
void tarePosition() override;
/**
* This implementation does nothing because the maximum velocity is configured using
* PathfinderLimits elsewhere.
*
* @param imaxVelocity Ignored.
*/
void setMaxVelocity(std::int32_t imaxVelocity) override;
/**
* Starts the internal thread. This should not be called by normal users. This method is called
* by the AsyncControllerFactory when making a new instance of this class.
*/
void startThread();
/**
* Returns the underlying thread handle.
*
* @return The underlying thread handle.
*/
CrossplatformThread *getThread() const;
/**
* Attempts to remove a path without stopping execution, then if that fails, disables the
* controller and removes the path.
*
* @param ipathId The path ID that will be removed
*/
void forceRemovePath(const std::string &ipathId);
protected:
std::shared_ptr<Logger> logger;
std::map<std::string, std::vector<squiggles::ProfilePoint>> paths{};
PathfinderLimits limits;
std::shared_ptr<ControllerOutput<double>> output;
QLength diameter;
AbstractMotor::GearsetRatioPair pair;
double currentProfilePosition{0};
TimeUtil timeUtil;
// This must be locked when accessing the current path
CrossplatformMutex currentPathMutex;
std::string currentPath{""};
std::atomic_bool isRunning{false};
std::atomic_int direction{1};
std::atomic_bool disabled{false};
std::atomic_bool dtorCalled{false};
CrossplatformThread *task{nullptr};
static void trampoline(void *context);
void loop();
/**
* Follow the supplied path. Must follow the disabled lifecycle.
*/
virtual void executeSinglePath(const std::vector<squiggles::ProfilePoint> &path,
std::unique_ptr<AbstractRate> rate);
/**
* Converts linear "chassis" speed to rotational motor speed.
*
* @param linear "chassis" frame speed
* @return motor frame speed
*/
QAngularSpeed convertLinearToRotational(QSpeed linear) const;
std::string getPathErrorMessage(const std::vector<PathfinderPoint> &points,
const std::string &ipathId,
int length);
};
} // namespace okapi

326
SerialTest/include/okapi/api/control/async/asyncMotionProfileController.hpp Normal file
View File

@ -0,0 +1,326 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/chassis/controller/chassisScales.hpp"
#include "okapi/api/chassis/model/skidSteerModel.hpp"
#include "okapi/api/control/async/asyncPositionController.hpp"
#include "okapi/api/control/util/pathfinderUtil.hpp"
#include "okapi/api/units/QAngularSpeed.hpp"
#include "okapi/api/units/QSpeed.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <atomic>
#include <iostream>
#include <map>
#include "squiggles.hpp"
namespace okapi {
class AsyncMotionProfileController : public AsyncPositionController<std::string, PathfinderPoint> {
public:
/**
* An Async Controller which generates and follows 2D motion profiles. Throws a
* `std::invalid_argument` exception if the gear ratio is zero.
*
* @param itimeUtil The TimeUtil.
* @param ilimits The default limits.
* @param imodel The chassis model to control.
* @param iscales The chassis dimensions.
* @param ipair The gearset.
* @param ilogger The logger this instance will log to.
*/
AsyncMotionProfileController(const TimeUtil &itimeUtil,
const PathfinderLimits &ilimits,
const std::shared_ptr<ChassisModel> &imodel,
const ChassisScales &iscales,
const AbstractMotor::GearsetRatioPair &ipair,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
AsyncMotionProfileController(AsyncMotionProfileController &&other) = delete;
AsyncMotionProfileController &operator=(AsyncMotionProfileController &&other) = delete;
~AsyncMotionProfileController() override;
/**
* Generates a path which intersects the given waypoints and saves it internally with a key of
* pathId. Call `executePath()` with the same pathId to run it.
*
* If the waypoints form a path which is impossible to achieve, an instance of
* `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
* are no waypoints, no path is generated.
*
* @param iwaypoints The waypoints to hit on the path.
* @param ipathId A unique identifier to save the path with.
*/
void generatePath(std::initializer_list<PathfinderPoint> iwaypoints, const std::string &ipathId);
/**
* Generates a path which intersects the given waypoints and saves it internally with a key of
* pathId. Call `executePath()` with the same pathId to run it.
*
* If the waypoints form a path which is impossible to achieve, an instance of
* `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
* are no waypoints, no path is generated.
*
* NOTE: The waypoints are expected to be in the
* okapi::State::FRAME_TRANSFORMATION format where +x is forward, +y is right,
* and 0 theta is measured from the +x axis to the +y axis.
*
* @param iwaypoints The waypoints to hit on the path.
* @param ipathId A unique identifier to save the path with.
* @param ilimits The limits to use for this path only.
*/
void generatePath(std::initializer_list<PathfinderPoint> iwaypoints,
const std::string &ipathId,
const PathfinderLimits &ilimits);
/**
* Removes a path and frees the memory it used. This function returns true if the path was either
* deleted or didn't exist in the first place. It returns false if the path could not be removed
* because it is running.
*
* @param ipathId A unique identifier for the path, previously passed to `generatePath()`
* @return True if the path no longer exists
*/
bool removePath(const std::string &ipathId);
/**
* Gets the identifiers of all paths saved in this `AsyncMotionProfileController`.
*
* @return The identifiers of all paths
*/
std::vector<std::string> getPaths();
/**
* Executes a path with the given ID. If there is no path matching the ID, the method will
* return. Any targets set while a path is being followed will be ignored.
*
* @param ipathId A unique identifier for the path, previously passed to `generatePath()`.
*/
void setTarget(std::string ipathId) override;
/**
* Executes a path with the given ID. If there is no path matching the ID, the method will
* return. Any targets set while a path is being followed will be ignored.
*
* @param ipathId A unique identifier for the path, previously passed to `generatePath()`.
* @param ibackwards Whether to follow the profile backwards.
* @param imirrored Whether to follow the profile mirrored.
*/
void setTarget(std::string ipathId, bool ibackwards, bool imirrored = false);
/**
* Writes the value of the controller output. This method might be automatically called in another
* thread by the controller. This just calls `setTarget()`.
*/
void controllerSet(std::string ivalue) override;
/**
* Gets the last set target, or the default target if none was set.
*
* @return the last target
*/
std::string getTarget() override;
/**
* This is overridden to return the current path.
*
* @return The most recent value of the process variable.
*/
std::string getProcessValue() const override;
/**
* Blocks the current task until the controller has settled. This controller is settled when
* it has finished following a path. If no path is being followed, it is settled.
*/
void waitUntilSettled() override;
/**
* Generates a new path from the position (typically the current position) to the target and
* blocks until the controller has settled. Does not save the path which was generated.
*
* @param iwaypoints The waypoints to hit on the path.
* @param ibackwards Whether to follow the profile backwards.
* @param imirrored Whether to follow the profile mirrored.
*/
void moveTo(std::initializer_list<PathfinderPoint> iwaypoints,
bool ibackwards = false,
bool imirrored = false);
/**
* Generates a new path from the position (typically the current position) to the target and
* blocks until the controller has settled. Does not save the path which was generated.
*
* @param iwaypoints The waypoints to hit on the path.
* @param ilimits The limits to use for this path only.
* @param ibackwards Whether to follow the profile backwards.
* @param imirrored Whether to follow the profile mirrored.
*/
void moveTo(std::initializer_list<PathfinderPoint> iwaypoints,
const PathfinderLimits &ilimits,
bool ibackwards = false,
bool imirrored = false);
/**
* Returns the last error of the controller. Does not update when disabled. This implementation
* always returns zero since the robot is assumed to perfectly follow the path. Subclasses can
* override this to be more accurate using odometry information.
*
* @return the last error
*/
PathfinderPoint getError() const override;
/**
* Returns whether the controller has settled at the target. Determining what settling means is
* implementation-dependent.
*
* If the controller is disabled, this method must return true.
*
* @return whether the controller is settled
*/
bool isSettled() override;
/**
* Resets the controller so it can start from 0 again properly. Keeps configuration from
* before. This implementation also stops movement.
*/
void reset() override;
/**
* Changes whether the controller is off or on. Turning the controller on after it was off will
* NOT cause the controller to move to its last set target.
*/
void flipDisable() override;
/**
* Sets whether the controller is off or on. Turning the controller on after it was off will
* NOT cause the controller to move to its last set target, unless it was reset in that time.
*
* @param iisDisabled whether the controller is disabled
*/
void flipDisable(bool iisDisabled) override;
/**
* Returns whether the controller is currently disabled.
*
* @return whether the controller is currently disabled
*/
bool isDisabled() const override;
/**
* This implementation does nothing because the API always requires the starting position to be
* specified.
*/
void tarePosition() override;
/**
* This implementation does nothing because the maximum velocity is configured using
* PathfinderLimits elsewhere.
*
* @param imaxVelocity Ignored.
*/
void setMaxVelocity(std::int32_t imaxVelocity) override;
/**
* Starts the internal thread. This should not be called by normal users. This method is called
* by the `AsyncMotionProfileControllerBuilder` when making a new instance of this class.
*/
void startThread();
/**
* @return The underlying thread handle.
*/
CrossplatformThread *getThread() const;
/**
* Saves a generated path to a file. Paths are stored as `<ipathId>.csv`. An SD card
* must be inserted into the brain and the directory must exist. `idirectory` can be prefixed with
* `/usd/`, but it this is not required.
*
* @param idirectory The directory to store the path file in
* @param ipathId The path ID of the generated path
*/
void storePath(const std::string &idirectory, const std::string &ipathId);
/**
* Loads a path from a directory on the SD card containing a path CSV file. `/usd/` is
* automatically prepended to `idirectory` if it is not specified.
*
* @param idirectory The directory that the path files are stored in
* @param ipathId The path ID that the paths are stored under (and will be loaded into)
*/
void loadPath(const std::string &idirectory, const std::string &ipathId);
/**
* Attempts to remove a path without stopping execution. If that fails, disables the controller
* and removes the path.
*
* @param ipathId The path ID that will be removed
*/
void forceRemovePath(const std::string &ipathId);
protected:
std::shared_ptr<Logger> logger;
std::map<std::string, std::vector<squiggles::ProfilePoint>> paths{};
PathfinderLimits limits;
std::shared_ptr<ChassisModel> model;
ChassisScales scales;
AbstractMotor::GearsetRatioPair pair;
TimeUtil timeUtil;
// This must be locked when accessing the current path
CrossplatformMutex currentPathMutex;
std::string currentPath{""};
std::atomic_bool isRunning{false};
std::atomic_int direction{1};
std::atomic_bool mirrored{false};
std::atomic_bool disabled{false};
std::atomic_bool dtorCalled{false};
CrossplatformThread *task{nullptr};
static void trampoline(void *context);
void loop();
/**
* Follow the supplied path. Must follow the disabled lifecycle.
*/
virtual void executeSinglePath(const std::vector<squiggles::ProfilePoint> &path,
std::unique_ptr<AbstractRate> rate);
/**
* Converts linear chassis speed to rotational motor speed.
*
* @param linear chassis frame speed
* @return motor frame speed
*/
QAngularSpeed convertLinearToRotational(QSpeed linear) const;
std::string getPathErrorMessage(const std::vector<PathfinderPoint> &points,
const std::string &ipathId,
int length);
/**
* Joins and escapes a directory and file name
*
* @param directory The directory path, separated by forward slashes (/) and with or without a
* trailing slash
* @param filename The file name in the directory
* @return the fully qualified and legal path name
*/
static std::string makeFilePath(const std::string &directory, const std::string &filename);
void internalStorePath(std::ostream &file, const std::string &ipathId);
void internalLoadPath(std::istream &file, const std::string &ipathId);
void internalLoadPathfinderPath(std::istream &leftFile,
std::istream &rightFile,
const std::string &ipathId);
static constexpr double DT = 0.01;
};
} // namespace okapi

145
SerialTest/include/okapi/api/control/async/asyncPosIntegratedController.hpp Normal file
View File

@ -0,0 +1,145 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/async/asyncPositionController.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
namespace okapi {
/**
* Closed-loop controller that uses the V5 motor's onboard control to move. Input units are whatever
* units the motor is in.
*/
class AsyncPosIntegratedController : public AsyncPositionController<double, double> {
public:
/**
* Closed-loop controller that uses the V5 motor's onboard control to move. Input units are
* whatever units the motor is in. Throws a std::invalid_argument exception if the gear ratio is
* zero.
*
* @param imotor The motor to control.
* @param ipair The gearset.
* @param imaxVelocity The maximum velocity after gearing.
* @param itimeUtil The TimeUtil.
* @param ilogger The logger this instance will log to.
*/
AsyncPosIntegratedController(const std::shared_ptr<AbstractMotor> &imotor,
const AbstractMotor::GearsetRatioPair &ipair,
std::int32_t imaxVelocity,
const TimeUtil &itimeUtil,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* Sets the target for the controller.
*/
void setTarget(double itarget) override;
/**
* Gets the last set target, or the default target if none was set.
*
* @return the last target
*/
double getTarget() override;
/**
* @return The most recent value of the process variable.
*/
double getProcessValue() const override;
/**
* Returns the last error of the controller. Does not update when disabled.
*/
double getError() const override;
/**
* Returns whether the controller has settled at the target. Determining what settling means is
* implementation-dependent.
*
* If the controller is disabled, this method must return true.
*
* @return whether the controller is settled
*/
bool isSettled() override;
/**
* Resets the controller's internal state so it is similar to when it was first initialized, while
* keeping any user-configured information.
*/
void reset() override;
/**
* Changes whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*/
void flipDisable() override;
/**
* Sets whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*
* @param iisDisabled whether the controller is disabled
*/
void flipDisable(bool iisDisabled) override;
/**
* Returns whether the controller is currently disabled.
*
* @return whether the controller is currently disabled
*/
bool isDisabled() const override;
/**
* Blocks the current task until the controller has settled. Determining what settling means is
* implementation-dependent.
*/
void waitUntilSettled() override;
/**
* Writes the value of the controller output. This method might be automatically called in another
* thread by the controller. The range of input values is expected to be [-1, 1].
*
* @param ivalue the controller's output in the range [-1, 1]
*/
void controllerSet(double ivalue) override;
/**
* Sets the "absolute" zero position of the controller to its current position.
*/
void tarePosition() override;
/**
* Sets a new maximum velocity in motor RPM [0-600].
*
* @param imaxVelocity The new maximum velocity in motor RPM [0-600].
*/
void setMaxVelocity(std::int32_t imaxVelocity) override;
/**
* Stops the motor mid-movement. Does not change the last set target.
*/
virtual void stop();
protected:
std::shared_ptr<Logger> logger;
TimeUtil timeUtil;
std::shared_ptr<AbstractMotor> motor;
AbstractMotor::GearsetRatioPair pair;
std::int32_t maxVelocity;
double lastTarget{0};
double offset{0};
bool controllerIsDisabled{false};
bool hasFirstTarget{false};
std::unique_ptr<SettledUtil> settledUtil;
/**
* Resumes moving after the controller is reset. Should not cause movement if the controller is
* turned off, reset, and turned back on.
*/
virtual void resumeMovement();
};
} // namespace okapi

100
SerialTest/include/okapi/api/control/async/asyncPosPidController.hpp Normal file
View File

@ -0,0 +1,100 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/async/asyncPositionController.hpp"
#include "okapi/api/control/async/asyncWrapper.hpp"
#include "okapi/api/control/controllerOutput.hpp"
#include "okapi/api/control/iterative/iterativePosPidController.hpp"
#include "okapi/api/control/offsettableControllerInput.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <memory>
namespace okapi {
class AsyncPosPIDController : public AsyncWrapper<double, double>,
public AsyncPositionController<double, double> {
public:
/**
* An async position PID controller.
*
* @param iinput The controller input. Will be turned into an OffsettableControllerInput.
* @param ioutput The controller output.
* @param itimeUtil The TimeUtil.
* @param ikP The proportional gain.
* @param ikI The integral gain.
* @param ikD The derivative gain.
* @param ikBias The controller bias.
* @param iratio Any external gear ratio.
* @param iderivativeFilter The derivative filter.
*/
AsyncPosPIDController(
const std::shared_ptr<ControllerInput<double>> &iinput,
const std::shared_ptr<ControllerOutput<double>> &ioutput,
const TimeUtil &itimeUtil,
double ikP,
double ikI,
double ikD,
double ikBias = 0,
double iratio = 1,
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* An async position PID controller.
*
* @param iinput The controller input.
* @param ioutput The controller output.
* @param itimeUtil The TimeUtil.
* @param ikP The proportional gain.
* @param ikI The integral gain.
* @param ikD The derivative gain.
* @param ikBias The controller bias.
* @param iratio Any external gear ratio.
* @param iderivativeFilter The derivative filter.
*/
AsyncPosPIDController(
const std::shared_ptr<OffsetableControllerInput> &iinput,
const std::shared_ptr<ControllerOutput<double>> &ioutput,
const TimeUtil &itimeUtil,
double ikP,
double ikI,
double ikD,
double ikBias = 0,
double iratio = 1,
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* Sets the "absolute" zero position of the controller to its current position.
*/
void tarePosition() override;
/**
* This implementation does not respect the maximum velocity.
*
* @param imaxVelocity Ignored.
*/
void setMaxVelocity(std::int32_t imaxVelocity) override;
/**
* Set controller gains.
*
* @param igains The new gains.
*/
void setGains(const IterativePosPIDController::Gains &igains);
/**
* Gets the current gains.
*
* @return The current gains.
*/
IterativePosPIDController::Gains getGains() const;
protected:
std::shared_ptr<OffsetableControllerInput> offsettableInput;
std::shared_ptr<IterativePosPIDController> internalController;
};
} // namespace okapi

28
SerialTest/include/okapi/api/control/async/asyncPositionController.hpp Normal file
View File

@ -0,0 +1,28 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/async/asyncController.hpp"
#include <memory>
namespace okapi {
template <typename Input, typename Output>
class AsyncPositionController : virtual public AsyncController<Input, Output> {
public:
/**
* Sets the "absolute" zero position of the controller to its current position.
*/
virtual void tarePosition() = 0;
/**
* Sets a new maximum velocity (typically motor RPM [0-600]). The interpretation of the units
* of this velocity and whether it will be respected is implementation-dependent.
*
* @param imaxVelocity The new maximum velocity.
*/
virtual void setMaxVelocity(std::int32_t imaxVelocity) = 0;
};
} // namespace okapi

124
SerialTest/include/okapi/api/control/async/asyncVelIntegratedController.hpp Normal file
View File

@ -0,0 +1,124 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/async/asyncVelocityController.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <memory>
namespace okapi {
/**
* Closed-loop controller that uses the V5 motor's onboard control to move. Input units are whatever
* units the motor is in.
*/
class AsyncVelIntegratedController : public AsyncVelocityController<double, double> {
public:
/**
* Closed-loop controller that uses the V5 motor's onboard control to move. Input units are
* whatever units the motor is in. Throws a std::invalid_argument exception if the gear ratio is
* zero.
*
* @param imotor The motor to control.
* @param ipair The gearset.
* @param imaxVelocity The maximum velocity after gearing.
* @param itimeUtil The TimeUtil.
* @param ilogger The logger this instance will log to.
*/
AsyncVelIntegratedController(const std::shared_ptr<AbstractMotor> &imotor,
const AbstractMotor::GearsetRatioPair &ipair,
std::int32_t imaxVelocity,
const TimeUtil &itimeUtil,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* Sets the target for the controller.
*/
void setTarget(double itarget) override;
/**
* Gets the last set target, or the default target if none was set.
*
* @return the last target
*/
double getTarget() override;
/**
* @return The most recent value of the process variable.
*/
double getProcessValue() const override;
/**
* Returns the last error of the controller. Does not update when disabled.
*/
double getError() const override;
/**
* Returns whether the controller has settled at the target. Determining what settling means is
* implementation-dependent.
*
* If the controller is disabled, this method must return true.
*
* @return whether the controller is settled
*/
bool isSettled() override;
/**
* Resets the controller's internal state so it is similar to when it was first initialized, while
* keeping any user-configured information.
*/
void reset() override;
/**
* Changes whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*/
void flipDisable() override;
/**
* Sets whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*
* @param iisDisabled whether the controller is disabled
*/
void flipDisable(bool iisDisabled) override;
/**
* Returns whether the controller is currently disabled.
*
* @return whether the controller is currently disabled
*/
bool isDisabled() const override;
/**
* Blocks the current task until the controller has settled. Determining what settling means is
* implementation-dependent.
*/
void waitUntilSettled() override;
/**
* Writes the value of the controller output. This method might be automatically called in another
* thread by the controller. The range of input values is expected to be [-1, 1].
*
* @param ivalue the controller's output in the range [-1, 1]
*/
void controllerSet(double ivalue) override;
protected:
std::shared_ptr<Logger> logger;
TimeUtil timeUtil;
std::shared_ptr<AbstractMotor> motor;
AbstractMotor::GearsetRatioPair pair;
std::int32_t maxVelocity;
double lastTarget = 0;
bool controllerIsDisabled = false;
bool hasFirstTarget = false;
std::unique_ptr<SettledUtil> settledUtil;
virtual void resumeMovement();
};
} // namespace okapi

64
SerialTest/include/okapi/api/control/async/asyncVelPidController.hpp Normal file
View File

@ -0,0 +1,64 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/async/asyncVelocityController.hpp"
#include "okapi/api/control/async/asyncWrapper.hpp"
#include "okapi/api/control/controllerInput.hpp"
#include "okapi/api/control/controllerOutput.hpp"
#include "okapi/api/control/iterative/iterativeVelPidController.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <memory>
namespace okapi {
class AsyncVelPIDController : public AsyncWrapper<double, double>,
public AsyncVelocityController<double, double> {
public:
/**
* An async velocity PID controller.
*
* @param iinput The controller input.
* @param ioutput The controller output.
* @param itimeUtil The TimeUtil.
* @param ikP The proportional gain.
* @param ikD The derivative gain.
* @param ikF The feed-forward gain.
* @param ikSF A feed-forward gain to counteract static friction.
* @param ivelMath The VelMath used for calculating velocity.
* @param iratio Any external gear ratio.
* @param iderivativeFilter The derivative filter.
*/
AsyncVelPIDController(
const std::shared_ptr<ControllerInput<double>> &iinput,
const std::shared_ptr<ControllerOutput<double>> &ioutput,
const TimeUtil &itimeUtil,
double ikP,
double ikD,
double ikF,
double ikSF,
std::unique_ptr<VelMath> ivelMath,
double iratio = 1,
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* Set controller gains.
*
* @param igains The new gains.
*/
void setGains(const IterativeVelPIDController::Gains &igains);
/**
* Gets the current gains.
*
* @return The current gains.
*/
IterativeVelPIDController::Gains getGains() const;
protected:
std::shared_ptr<IterativeVelPIDController> internalController;
};
} // namespace okapi

14
SerialTest/include/okapi/api/control/async/asyncVelocityController.hpp Normal file
View File

@ -0,0 +1,14 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/async/asyncController.hpp"
#include <memory>
namespace okapi {
template <typename Input, typename Output>
class AsyncVelocityController : virtual public AsyncController<Input, Output> {};
} // namespace okapi

287
SerialTest/include/okapi/api/control/async/asyncWrapper.hpp Normal file
View File

@ -0,0 +1,287 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/async/asyncController.hpp"
#include "okapi/api/control/controllerInput.hpp"
#include "okapi/api/control/iterative/iterativeController.hpp"
#include "okapi/api/control/util/settledUtil.hpp"
#include "okapi/api/coreProsAPI.hpp"
#include "okapi/api/util/abstractRate.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/mathUtil.hpp"
#include "okapi/api/util/supplier.hpp"
#include <atomic>
#include <memory>
namespace okapi {
template <typename Input, typename Output>
class AsyncWrapper : virtual public AsyncController<Input, Output> {
public:
/**
* A wrapper class that transforms an `IterativeController` into an `AsyncController` by running
* it in another task. The input controller will act like an `AsyncController`.
*
* @param iinput controller input, passed to the `IterativeController`
* @param ioutput controller output, written to from the `IterativeController`
* @param icontroller the controller to use
* @param irateSupplier used for rates used in the main loop and in `waitUntilSettled`
* @param iratio Any external gear ratio.
* @param ilogger The logger this instance will log to.
*/
AsyncWrapper(const std::shared_ptr<ControllerInput<Input>> &iinput,
const std::shared_ptr<ControllerOutput<Output>> &ioutput,
const std::shared_ptr<IterativeController<Input, Output>> &icontroller,
const Supplier<std::unique_ptr<AbstractRate>> &irateSupplier,
const double iratio = 1,
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger())
: logger(std::move(ilogger)),
rateSupplier(irateSupplier),
input(iinput),
output(ioutput),
controller(icontroller),
ratio(iratio) {
}
AsyncWrapper(AsyncWrapper<Input, Output> &&other) = delete;
AsyncWrapper<Input, Output> &operator=(AsyncWrapper<Input, Output> &&other) = delete;
~AsyncWrapper() override {
dtorCalled.store(true, std::memory_order_release);
delete task;
}
/**
* Sets the target for the controller.
*/
void setTarget(const Input itarget) override {
LOG_INFO("AsyncWrapper: Set target to " + std::to_string(itarget));
hasFirstTarget = true;
controller->setTarget(itarget * ratio);
lastTarget = itarget;
}
/**
* Writes the value of the controller output. This method might be automatically called in another
* thread by the controller.
*
* @param ivalue the controller's output
*/
void controllerSet(const Input ivalue) override {
controller->controllerSet(ivalue);
}
/**
* Gets the last set target, or the default target if none was set.
*
* @return the last target
*/
Input getTarget() override {
return controller->getTarget();
}
/**
* @return The most recent value of the process variable.
*/
Input getProcessValue() const override {
return controller->getProcessValue();
}
/**
* Returns the last calculated output of the controller.
*/
Output getOutput() const {
return controller->getOutput();
}
/**
* Returns the last error of the controller. Does not update when disabled.
*/
Output getError() const override {
return controller->getError();
}
/**
* Returns whether the controller has settled at the target. Determining what settling means is
* implementation-dependent.
*
* If the controller is disabled, this method must return true.
*
* @return whether the controller is settled
*/
bool isSettled() override {
return isDisabled() || controller->isSettled();
}
/**
* Set time between loops.
*
* @param isampleTime time between loops
*/
void setSampleTime(const QTime &isampleTime) {
controller->setSampleTime(isampleTime);
}
/**
* Set controller output bounds.
*
* @param imax max output
* @param imin min output
*/
void setOutputLimits(const Output imax, const Output imin) {
controller->setOutputLimits(imax, imin);
}
/**
* Sets the (soft) limits for the target range that controllerSet() scales into. The target
* computed by controllerSet() is scaled into the range [-itargetMin, itargetMax].
*
* @param itargetMax The new max target for controllerSet().
* @param itargetMin The new min target for controllerSet().
*/
void setControllerSetTargetLimits(double itargetMax, double itargetMin) {
controller->setControllerSetTargetLimits(itargetMax, itargetMin);
}
/**
* Get the upper output bound.
*
* @return the upper output bound
*/
Output getMaxOutput() {
return controller->getMaxOutput();
}
/**
* Get the lower output bound.
*
* @return the lower output bound
*/
Output getMinOutput() {
return controller->getMinOutput();
}
/**
* Resets the controller's internal state so it is similar to when it was first initialized, while
* keeping any user-configured information.
*/
void reset() override {
LOG_INFO_S("AsyncWrapper: Reset");
controller->reset();
hasFirstTarget = false;
}
/**
* Changes whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*/
void flipDisable() override {
LOG_INFO("AsyncWrapper: flipDisable " + std::to_string(!controller->isDisabled()));
controller->flipDisable();
resumeMovement();
}
/**
* Sets whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*
* @param iisDisabled whether the controller is disabled
*/
void flipDisable(const bool iisDisabled) override {
LOG_INFO("AsyncWrapper: flipDisable " + std::to_string(iisDisabled));
controller->flipDisable(iisDisabled);
resumeMovement();
}
/**
* Returns whether the controller is currently disabled.
*
* @return whether the controller is currently disabled
*/
bool isDisabled() const override {
return controller->isDisabled();
}
/**
* Blocks the current task until the controller has settled. Determining what settling means is
* implementation-dependent.
*/
void waitUntilSettled() override {
LOG_INFO_S("AsyncWrapper: Waiting to settle");
auto rate = rateSupplier.get();
while (!isSettled()) {
rate->delayUntil(motorUpdateRate);
}
LOG_INFO_S("AsyncWrapper: Done waiting to settle");
}
/**
* Starts the internal thread. This should not be called by normal users. This method is called
* by the AsyncControllerFactory when making a new instance of this class.
*/
void startThread() {
if (!task) {
task = new CrossplatformThread(trampoline, this, "AsyncWrapper");
}
}
/**
* Returns the underlying thread handle.
*
* @return The underlying thread handle.
*/
CrossplatformThread *getThread() const {
return task;
}
protected:
std::shared_ptr<Logger> logger;
Supplier<std::unique_ptr<AbstractRate>> rateSupplier;
std::shared_ptr<ControllerInput<Input>> input;
std::shared_ptr<ControllerOutput<Output>> output;
std::shared_ptr<IterativeController<Input, Output>> controller;
bool hasFirstTarget{false};
Input lastTarget;
double ratio;
std::atomic_bool dtorCalled{false};
CrossplatformThread *task{nullptr};
static void trampoline(void *context) {
if (context) {
static_cast<AsyncWrapper *>(context)->loop();
}
}
void loop() {
auto rate = rateSupplier.get();
while (!dtorCalled.load(std::memory_order_acquire) && !task->notifyTake(0)) {
if (!isDisabled()) {
output->controllerSet(controller->step(input->controllerGet()));
}
rate->delayUntil(controller->getSampleTime());
}
}
/**
* Resumes moving after the controller is reset. Should not cause movement if the controller is
* turned off, reset, and turned back on.
*/
virtual void resumeMovement() {
if (isDisabled()) {
// This will grab the output *when disabled*
output->controllerSet(controller->getOutput());
} else {
if (hasFirstTarget) {
setTarget(lastTarget);
}
}
}
};
} // namespace okapi

86
SerialTest/include/okapi/api/control/closedLoopController.hpp Normal file
View File

@ -0,0 +1,86 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/controllerOutput.hpp"
#include "okapi/api/units/QTime.hpp"
namespace okapi {
/**
* An abstract closed-loop controller.
*
* @tparam Input The target/input type.
* @tparam Output The error/output type.
*/
template <typename Input, typename Output>
class ClosedLoopController : public ControllerOutput<Input> {
public:
virtual ~ClosedLoopController() = default;
/**
* Sets the target for the controller.
*
* @param itarget the new target
*/
virtual void setTarget(Input itarget) = 0;
/**
* Gets the last set target, or the default target if none was set.
*
* @return the last target
*/
virtual Input getTarget() = 0;
/**
* @return The most recent value of the process variable.
*/
virtual Input getProcessValue() const = 0;
/**
* Returns the last error of the controller. Does not update when disabled.
*
* @return the last error
*/
virtual Output getError() const = 0;
/**
* Returns whether the controller has settled at the target. Determining what settling means is
* implementation-dependent.
*
* If the controller is disabled, this method must return `true`.
*
* @return whether the controller is settled
*/
virtual bool isSettled() = 0;
/**
* Resets the controller's internal state so it is similar to when it was first initialized, while
* keeping any user-configured information.
*/
virtual void reset() = 0;
/**
* Changes whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*/
virtual void flipDisable() = 0;
/**
* Sets whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*
* @param iisDisabled whether the controller is disabled
*/
virtual void flipDisable(bool iisDisabled) = 0;
/**
* Returns whether the controller is currently disabled.
*
* @return whether the controller is currently disabled
*/
virtual bool isDisabled() const = 0;
};
} // namespace okapi

19
SerialTest/include/okapi/api/control/controllerInput.hpp Normal file
View File

@ -0,0 +1,19 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
namespace okapi {
template <typename T> class ControllerInput {
public:
/**
* Get the sensor value for use in a control loop. This method might be automatically called in
* another thread by the controller.
*
* @return the current sensor value, or ``PROS_ERR`` on a failure.
*/
virtual T controllerGet() = 0;
};
} // namespace okapi

19
SerialTest/include/okapi/api/control/controllerOutput.hpp Normal file
View File

@ -0,0 +1,19 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
namespace okapi {
template <typename T> class ControllerOutput {
public:
/**
* Writes the value of the controller output. This method might be automatically called in another
* thread by the controller. The range of input values is expected to be `[-1, 1]`.
*
* @param ivalue the controller's output in the range `[-1, 1]`
*/
virtual void controllerSet(T ivalue) = 0;
};
} // namespace okapi

79
SerialTest/include/okapi/api/control/iterative/iterativeController.hpp Normal file
View File

@ -0,0 +1,79 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/closedLoopController.hpp"
#include "okapi/api/units/QTime.hpp"
namespace okapi {
/**
* Closed-loop controller that steps iteratively using the step method below.
*
* `ControllerOutput::controllerSet()` should set the controller's target to the input scaled by
* the output bounds.
*/
template <typename Input, typename Output>
class IterativeController : public ClosedLoopController<Input, Output> {
public:
/**
* Do one iteration of the controller.
*
* @param ireading A new measurement.
* @return The controller output.
*/
virtual Output step(Input ireading) = 0;
/**
* Returns the last calculated output of the controller.
*/
virtual Output getOutput() const = 0;
/**
* Set controller output bounds.
*
* @param imax max output
* @param imin min output
*/
virtual void setOutputLimits(Output imax, Output imin) = 0;
/**
* Sets the (soft) limits for the target range that controllerSet() scales into. The target
* computed by `controllerSet()` is scaled into the range `[-itargetMin, itargetMax]`.
*
* @param itargetMax The new max target for `controllerSet()`.
* @param itargetMin The new min target for `controllerSet()`.
*/
virtual void setControllerSetTargetLimits(Output itargetMax, Output itargetMin) = 0;
/**
* Get the upper output bound.
*
* @return the upper output bound
*/
virtual Output getMaxOutput() = 0;
/**
* Get the lower output bound.
*
* @return the lower output bound
*/
virtual Output getMinOutput() = 0;
/**
* Set time between loops.
*
* @param isampleTime time between loops
*/
virtual void setSampleTime(QTime isampleTime) = 0;
/**
* Get the last set sample time.
*
* @return sample time
*/
virtual QTime getSampleTime() const = 0;
};
} // namespace okapi

150
SerialTest/include/okapi/api/control/iterative/iterativeMotorVelocityController.hpp Normal file
View File

@ -0,0 +1,150 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/iterative/iterativeVelocityController.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include <array>
#include <memory>
namespace okapi {
class IterativeMotorVelocityController : public IterativeVelocityController<double, double> {
public:
/**
* Velocity controller that automatically writes to the motor.
*/
IterativeMotorVelocityController(
const std::shared_ptr<AbstractMotor> &imotor,
const std::shared_ptr<IterativeVelocityController<double, double>> &icontroller);
/**
* Do one iteration of the controller.
*
* @param inewReading new measurement
* @return controller output
*/
double step(double ireading) override;
/**
* Sets the target for the controller.
*/
void setTarget(double itarget) override;
/**
* Writes the value of the controller output. This method might be automatically called in another
* thread by the controller. The range of input values is expected to be `[-1, 1]`.
*
* @param ivalue the controller's output in the range `[-1, 1]`
*/
void controllerSet(double ivalue) override;
/**
* Gets the last set target, or the default target if none was set.
*
* @return the last target
*/
double getTarget() override;
/**
* @return The most recent value of the process variable.
*/
double getProcessValue() const override;
/**
* Returns the last calculated output of the controller.
*/
double getOutput() const override;
/**
* Get the upper output bound.
*
* @return the upper output bound
*/
double getMaxOutput() override;
/**
* Get the lower output bound.
*
* @return the lower output bound
*/
double getMinOutput() override;
/**
* Returns the last error of the controller. Does not update when disabled.
*/
double getError() const override;
/**
* Returns whether the controller has settled at the target. Determining what settling means is
* implementation-dependent.
*
* @return whether the controller is settled
*/
bool isSettled() override;
/**
* Set time between loops in ms.
*
* @param isampleTime time between loops in ms
*/
void setSampleTime(QTime isampleTime) override;
/**
* Set controller output bounds.
*
* @param imax max output
* @param imin min output
*/
void setOutputLimits(double imax, double imin) override;
/**
* Sets the (soft) limits for the target range that controllerSet() scales into. The target
* computed by controllerSet() is scaled into the range [-itargetMin, itargetMax].
*
* @param itargetMax The new max target for controllerSet().
* @param itargetMin The new min target for controllerSet().
*/
void setControllerSetTargetLimits(double itargetMax, double itargetMin) override;
/**
* Resets the controller's internal state so it is similar to when it was first initialized, while
* keeping any user-configured information.
*/
void reset() override;
/**
* Changes whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*/
void flipDisable() override;
/**
* Sets whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*
* @param iisDisabled whether the controller is disabled
*/
void flipDisable(bool iisDisabled) override;
/**
* Returns whether the controller is currently disabled.
*
* @return whether the controller is currently disabled
*/
bool isDisabled() const override;
/**
* Get the last set sample time.
*
* @return sample time
*/
QTime getSampleTime() const override;
protected:
std::shared_ptr<AbstractMotor> motor;
std::shared_ptr<IterativeVelocityController<double, double>> controller;
};
} // namespace okapi

276
SerialTest/include/okapi/api/control/iterative/iterativePosPidController.hpp Normal file
View File

@ -0,0 +1,276 @@
/*
* Based on the Arduino PID controller: https://github.com/br3ttb/Arduino-PID-Library
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/iterative/iterativePositionController.hpp"
#include "okapi/api/control/util/settledUtil.hpp"
#include "okapi/api/filter/filter.hpp"
#include "okapi/api/filter/passthroughFilter.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <limits>
#include <memory>
namespace okapi {
class IterativePosPIDController : public IterativePositionController<double, double> {
public:
struct Gains {
double kP{0};
double kI{0};
double kD{0};
double kBias{0};
bool operator==(const Gains &rhs) const;
bool operator!=(const Gains &rhs) const;
};
/**
* Position PID controller.
*
* @param ikP the proportional gain
* @param ikI the integration gain
* @param ikD the derivative gain
* @param ikBias the controller bias
* @param itimeUtil see TimeUtil docs
* @param iderivativeFilter a filter for filtering the derivative term
* @param ilogger The logger this instance will log to.
*/
IterativePosPIDController(
double ikP,
double ikI,
double ikD,
double ikBias,
const TimeUtil &itimeUtil,
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
/**
* Position PID controller.
*
* @param igains the controller gains
* @param itimeUtil see TimeUtil docs
* @param iderivativeFilter a filter for filtering the derivative term
*/
IterativePosPIDController(
const Gains &igains,
const TimeUtil &itimeUtil,
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
/**
* Do one iteration of the controller. Returns the reading in the range [-1, 1] unless the
* bounds have been changed with setOutputLimits().
*
* @param inewReading new measurement
* @return controller output
*/
double step(double inewReading) override;
/**
* Sets the target for the controller.
*
* @param itarget new target position
*/
void setTarget(double itarget) override;
/**
* Writes the value of the controller output. This method might be automatically called in another
* thread by the controller. The range of input values is expected to be [-1, 1].
*
* @param ivalue the controller's output in the range [-1, 1]
*/
void controllerSet(double ivalue) override;
/**
* Gets the last set target, or the default target if none was set.
*
* @return the last target
*/
double getTarget() override;
/**
* Gets the last set target, or the default target if none was set.
*
* @return the last target
*/
double getTarget() const;
/**
* @return The most recent value of the process variable.
*/
double getProcessValue() const override;
/**
* Returns the last calculated output of the controller. Output is in the range [-1, 1]
* unless the bounds have been changed with setOutputLimits().
*/
double getOutput() const override;
/**
* Get the upper output bound.
*
* @return the upper output bound
*/
double getMaxOutput() override;
/**
* Get the lower output bound.
*
* @return the lower output bound
*/
double getMinOutput() override;
/**
* Returns the last error of the controller. Does not update when disabled.
*/
double getError() const override;
/**
* Returns whether the controller has settled at the target. Determining what settling means is
* implementation-dependent.
*
* If the controller is disabled, this method must return true.
*
* @return whether the controller is settled
*/
bool isSettled() override;
/**
* Set time between loops in ms.
*
* @param isampleTime time between loops
*/
void setSampleTime(QTime isampleTime) override;
/**
* Set controller output bounds. Default bounds are [-1, 1].
*
* @param imax max output
* @param imin min output
*/
void setOutputLimits(double imax, double imin) override;
/**
* Sets the (soft) limits for the target range that controllerSet() scales into. The target
* computed by controllerSet() is scaled into the range [-itargetMin, itargetMax].
*
* @param itargetMax The new max target for controllerSet().
* @param itargetMin The new min target for controllerSet().
*/
void setControllerSetTargetLimits(double itargetMax, double itargetMin) override;
/**
* Resets the controller's internal state so it is similar to when it was first initialized, while
* keeping any user-configured information.
*/
void reset() override;
/**
* Changes whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*/
void flipDisable() override;
/**
* Sets whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*
* @param iisDisabled whether the controller is disabled
*/
void flipDisable(bool iisDisabled) override;
/**
* Returns whether the controller is currently disabled.
*
* @return whether the controller is currently disabled
*/
bool isDisabled() const override;
/**
* Get the last set sample time.
*
* @return sample time
*/
QTime getSampleTime() const override;
/**
* Set integrator bounds. Default bounds are [-1, 1].
*
* @param imax max integrator value
* @param imin min integrator value
*/
virtual void setIntegralLimits(double imax, double imin);
/**
* Set the error sum bounds. Default bounds are [0, std::numeric_limits<double>::max()]. Error
* will only be added to the integral term when its absolute value is between these bounds of
* either side of the target.
*
* @param imax max error value that will be summed
* @param imin min error value that will be summed
*/
virtual void setErrorSumLimits(double imax, double imin);
/**
* Set whether the integrator should be reset when error is 0 or changes sign.
*
* @param iresetOnZero true to reset
*/
virtual void setIntegratorReset(bool iresetOnZero);
/**
* Set controller gains.
*
* @param igains The new gains.
*/
virtual void setGains(const Gains &igains);
/**
* Gets the current gains.
*
* @return The current gains.
*/
Gains getGains() const;
protected:
std::shared_ptr<Logger> logger;
double kP, kI, kD, kBias;
QTime sampleTime{10_ms};
double target{0};
double lastReading{0};
double error{0};
double lastError{0};
std::unique_ptr<Filter> derivativeFilter;
// Integral bounds
double integral{0};
double integralMax{1};
double integralMin{-1};
// Error will only be added to the integral term within these bounds on either side of the target
double errorSumMin{0};
double errorSumMax{std::numeric_limits<double>::max()};
double derivative{0};
// Output bounds
double output{0};
double outputMax{1};
double outputMin{-1};
double controllerSetTargetMax{1};
double controllerSetTargetMin{-1};
// Reset the integrated when the controller crosses 0 or not
bool shouldResetOnCross{true};
bool controllerIsDisabled{false};
std::unique_ptr<AbstractTimer> loopDtTimer;
std::unique_ptr<SettledUtil> settledUtil;
};
} // namespace okapi

13
SerialTest/include/okapi/api/control/iterative/iterativePositionController.hpp Normal file
View File

@ -0,0 +1,13 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/iterative/iterativeController.hpp"
namespace okapi {
template <typename Input, typename Output>
class IterativePositionController : public IterativeController<Input, Output> {};
} // namespace okapi

255
SerialTest/include/okapi/api/control/iterative/iterativeVelPidController.hpp Normal file
View File

@ -0,0 +1,255 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/iterative/iterativeVelocityController.hpp"
#include "okapi/api/control/util/settledUtil.hpp"
#include "okapi/api/filter/passthroughFilter.hpp"
#include "okapi/api/filter/velMath.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
namespace okapi {
class IterativeVelPIDController : public IterativeVelocityController<double, double> {
public:
struct Gains {
double kP{0};
double kD{0};
double kF{0};
double kSF{0};
bool operator==(const Gains &rhs) const;
bool operator!=(const Gains &rhs) const;
};
/**
* Velocity PD controller.
*
* @param ikP the proportional gain
* @param ikD the derivative gain
* @param ikF the feed-forward gain
* @param ikSF a feed-forward gain to counteract static friction
* @param ivelMath The VelMath used for calculating velocity.
* @param itimeUtil see TimeUtil docs
* @param iderivativeFilter a filter for filtering the derivative term
* @param ilogger The logger this instance will log to.
*/
IterativeVelPIDController(
double ikP,
double ikD,
double ikF,
double ikSF,
std::unique_ptr<VelMath> ivelMath,
const TimeUtil &itimeUtil,
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
/**
* Velocity PD controller.
*
* @param igains The controller gains.
* @param ivelMath The VelMath used for calculating velocity.
* @param itimeUtil see TimeUtil docs
* @param iderivativeFilter a filter for filtering the derivative term
* @param ilogger The logger this instance will log to.
*/
IterativeVelPIDController(
const Gains &igains,
std::unique_ptr<VelMath> ivelMath,
const TimeUtil &itimeUtil,
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
/**
* Do one iteration of the controller. Returns the reading in the range [-1, 1] unless the
* bounds have been changed with setOutputLimits().
*
* @param inewReading new measurement
* @return controller output
*/
double step(double inewReading) override;
/**
* Sets the target for the controller.
*
* @param itarget new target velocity
*/
void setTarget(double itarget) override;
/**
* Writes the value of the controller output. This method might be automatically called in another
* thread by the controller. The range of input values is expected to be [-1, 1].
*
* @param ivalue the controller's output in the range [-1, 1]
*/
void controllerSet(double ivalue) override;
/**
* Gets the last set target, or the default target if none was set.
*
* @return the last target
*/
double getTarget() override;
/**
* Gets the last set target, or the default target if none was set.
*
* @return the last target
*/
double getTarget() const;
/**
* @return The most recent value of the process variable.
*/
double getProcessValue() const override;
/**
* Returns the last calculated output of the controller.
*/
double getOutput() const override;
/**
* Get the upper output bound.
*
* @return the upper output bound
*/
double getMaxOutput() override;
/**
* Get the lower output bound.
*
* @return the lower output bound
*/
double getMinOutput() override;
/**
* Returns the last error of the controller. Does not update when disabled.
*/
double getError() const override;
/**
* Returns whether the controller has settled at the target. Determining what settling means is
* implementation-dependent.
*
* If the controller is disabled, this method must return true.
*
* @return whether the controller is settled
*/
bool isSettled() override;
/**
* Set time between loops in ms.
*
* @param isampleTime time between loops
*/
void setSampleTime(QTime isampleTime) override;
/**
* Set controller output bounds. Default bounds are [-1, 1].
*
* @param imax max output
* @param imin min output
*/
void setOutputLimits(double imax, double imin) override;
/**
* Sets the (soft) limits for the target range that controllerSet() scales into. The target
* computed by controllerSet() is scaled into the range [-itargetMin, itargetMax].
*
* @param itargetMax The new max target for controllerSet().
* @param itargetMin The new min target for controllerSet().
*/
void setControllerSetTargetLimits(double itargetMax, double itargetMin) override;
/**
* Resets the controller's internal state so it is similar to when it was first initialized, while
* keeping any user-configured information.
*/
void reset() override;
/**
* Changes whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*/
void flipDisable() override;
/**
* Sets whether the controller is off or on. Turning the controller on after it was off will
* cause the controller to move to its last set target, unless it was reset in that time.
*
* @param iisDisabled whether the controller is disabled
*/
void flipDisable(bool iisDisabled) override;
/**
* Returns whether the controller is currently disabled.
*
* @return whether the controller is currently disabled
*/
bool isDisabled() const override;
/**
* Get the last set sample time.
*
* @return sample time
*/
QTime getSampleTime() const override;
/**
* Do one iteration of velocity calculation.
*
* @param inewReading new measurement
* @return filtered velocity
*/
virtual QAngularSpeed stepVel(double inewReading);
/**
* Set controller gains.
*
* @param igains The new gains.
*/
virtual void setGains(const Gains &igains);
/**
* Gets the current gains.
*
* @return The current gains.
*/
Gains getGains() const;
/**
* Sets the number of encoder ticks per revolution. Default is 1800.
*
* @param tpr number of measured units per revolution
*/
virtual void setTicksPerRev(double tpr);
/**
* Returns the current velocity.
*/
virtual QAngularSpeed getVel() const;
protected:
std::shared_ptr<Logger> logger;
double kP, kD, kF, kSF;
QTime sampleTime{10_ms};
double error{0};
double derivative{0};
double target{0};
double outputSum{0};
double output{0};
double outputMax{1};
double outputMin{-1};
double controllerSetTargetMax{1};
double controllerSetTargetMin{-1};
bool controllerIsDisabled{false};
std::unique_ptr<VelMath> velMath;
std::unique_ptr<Filter> derivativeFilter;
std::unique_ptr<AbstractTimer> loopDtTimer;
std::unique_ptr<SettledUtil> settledUtil;
};
} // namespace okapi

13
SerialTest/include/okapi/api/control/iterative/iterativeVelocityController.hpp Normal file
View File

@ -0,0 +1,13 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/iterative/iterativeController.hpp"
namespace okapi {
template <typename Input, typename Output>
class IterativeVelocityController : public IterativeController<Input, Output> {};
} // namespace okapi

41
SerialTest/include/okapi/api/control/offsettableControllerInput.hpp Normal file
View File

@ -0,0 +1,41 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/controllerInput.hpp"
#include <memory>
namespace okapi {
class OffsetableControllerInput : public ControllerInput<double> {
public:
/**
* A ControllerInput which can be tared to change the zero position.
*
* @param iinput The ControllerInput to reference.
*/
explicit OffsetableControllerInput(const std::shared_ptr<ControllerInput<double>> &iinput);
virtual ~OffsetableControllerInput();
/**
* Get the sensor value for use in a control loop. This method might be automatically called in
* another thread by the controller.
*
* @return the current sensor value, or PROS_ERR on a failure.
*/
double controllerGet() override;
/**
* Sets the "absolute" zero position of this controller input to its current position. This does
* nothing if the underlying controller input returns PROS_ERR.
*/
virtual void tarePosition();
protected:
std::shared_ptr<ControllerInput<double>> input;
double offset{0};
};
} // namespace okapi

131
SerialTest/include/okapi/api/control/util/controllerRunner.hpp Normal file
View File

@ -0,0 +1,131 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/async/asyncController.hpp"
#include "okapi/api/control/controllerOutput.hpp"
#include "okapi/api/control/iterative/iterativeController.hpp"
#include "okapi/api/util/abstractRate.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <memory>
namespace okapi {
template <typename Input, typename Output> class ControllerRunner {
public:
/**
* A utility class that runs a closed-loop controller.
*
* @param itimeUtil The TimeUtil.
* @param ilogger The logger this instance will log to.
*/
explicit ControllerRunner(const TimeUtil &itimeUtil,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger())
: logger(ilogger), rate(itimeUtil.getRate()) {
}
/**
* Runs the controller until it has settled.
*
* @param itarget the new target
* @param icontroller the controller to run
* @return the error when settled
*/
virtual Output runUntilSettled(const Input itarget, AsyncController<Input, Output> &icontroller) {
LOG_INFO("ControllerRunner: runUntilSettled(AsyncController): Set target to " +
std::to_string(itarget));
icontroller.setTarget(itarget);
while (!icontroller.isSettled()) {
rate->delayUntil(10_ms);
}
LOG_INFO("ControllerRunner: runUntilSettled(AsyncController): Done waiting to settle");
return icontroller.getError();
}
/**
* Runs the controller until it has settled.
*
* @param itarget the new target
* @param icontroller the controller to run
* @param ioutput the output to write to
* @return the error when settled
*/
virtual Output runUntilSettled(const Input itarget,
IterativeController<Input, Output> &icontroller,
ControllerOutput<Output> &ioutput) {
LOG_INFO("ControllerRunner: runUntilSettled(IterativeController): Set target to " +
std::to_string(itarget));
icontroller.setTarget(itarget);
while (!icontroller.isSettled()) {
ioutput.controllerSet(icontroller.getOutput());
rate->delayUntil(10_ms);
}
LOG_INFO("ControllerRunner: runUntilSettled(IterativeController): Done waiting to settle");
return icontroller.getError();
}
/**
* Runs the controller until it has reached its target, but not necessarily settled.
*
* @param itarget the new target
* @param icontroller the controller to run
* @return the error when settled
*/
virtual Output runUntilAtTarget(const Input itarget,
AsyncController<Input, Output> &icontroller) {
LOG_INFO("ControllerRunner: runUntilAtTarget(AsyncController): Set target to " +
std::to_string(itarget));
icontroller.setTarget(itarget);
double error = icontroller.getError();
double lastError = error;
while (error != 0 && std::copysign(1.0, error) == std::copysign(1.0, lastError)) {
lastError = error;
rate->delayUntil(10_ms);
error = icontroller.getError();
}
LOG_INFO("ControllerRunner: runUntilAtTarget(AsyncController): Done waiting to settle");
return icontroller.getError();
}
/**
* Runs the controller until it has reached its target, but not necessarily settled.
*
* @param itarget the new target
* @param icontroller the controller to run
* @param ioutput the output to write to
* @return the error when settled
*/
virtual Output runUntilAtTarget(const Input itarget,
IterativeController<Input, Output> &icontroller,
ControllerOutput<Output> &ioutput) {
LOG_INFO("ControllerRunner: runUntilAtTarget(IterativeController): Set target to " +
std::to_string(itarget));
icontroller.setTarget(itarget);
double error = icontroller.getError();
double lastError = error;
while (error != 0 && std::copysign(1.0, error) == std::copysign(1.0, lastError)) {
ioutput.controllerSet(icontroller.getOutput());
lastError = error;
rate->delayUntil(10_ms);
error = icontroller.getError();
}
LOG_INFO("ControllerRunner: runUntilAtTarget(IterativeController): Done waiting to settle");
return icontroller.getError();
}
protected:
std::shared_ptr<Logger> logger;
std::unique_ptr<AbstractRate> rate;
};
} // namespace okapi

156
SerialTest/include/okapi/api/control/util/flywheelSimulator.hpp Normal file
View File

@ -0,0 +1,156 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include <functional>
namespace okapi {
class FlywheelSimulator {
public:
/**
* A simulator for an inverted pendulum. The center of mass of the system changes as the link
* rotates (by default, you can set a new torque function with setExternalTorqueFunction()).
*/
explicit FlywheelSimulator(double imass = 0.01,
double ilinkLen = 1,
double imuStatic = 0.1,
double imuDynamic = 0.9,
double itimestep = 0.01);
virtual ~FlywheelSimulator();
/**
* Step the simulation by the timestep.
*
* @return the current angle
*/
double step();
/**
* Step the simulation by the timestep.
*
* @param itorque new input torque
* @return the current angle
*/
double step(double itorque);
/**
* Sets the torque function used to calculate the torque due to external forces. This torque gets
* summed with the input torque.
*
* For example, the default torque function has the torque due to gravity vary as the link swings:
* [](double angle, double mass, double linkLength) {
* return (linkLength * std::cos(angle)) * (mass * -1 * gravity);
* }
*
* @param itorqueFunc the torque function. The return value is the torque due to external forces
*/
void setExternalTorqueFunction(
std::function<double(double angle, double mass, double linkLength)> itorqueFunc);
/**
* Sets the input torque. The input will be bounded by the max torque.
*
* @param itorque new input torque
*/
void setTorque(double itorque);
/**
* Sets the max torque. The input torque cannot exceed this maximum torque.
*
* @param imaxTorque new maximum torque
*/
void setMaxTorque(double imaxTorque);
/**
* Sets the current angle.
*
* @param iangle new angle
**/
void setAngle(double iangle);
/**
* Sets the mass (kg).
*
* @param imass new mass
*/
void setMass(double imass);
/**
* Sets the link length (m).
*
* @param ilinkLen new link length
*/
void setLinkLength(double ilinkLen);
/**
* Sets the static friction (N*m).
*
* @param imuStatic new static friction
*/
void setStaticFriction(double imuStatic);
/**
* Sets the dynamic friction (N*m).
*
* @param imuDynamic new dynamic friction
*/
void setDynamicFriction(double imuDynamic);
/**
* Sets the timestep (sec).
*
* @param itimestep new timestep
*/
void setTimestep(double itimestep);
/**
* Returns the current angle (angle in rad).
*
* @return the current angle
*/
double getAngle() const;
/**
* Returns the current omgea (angular velocity in rad / sec).
*
* @return the current omega
*/
double getOmega() const;
/**
* Returns the current acceleration (angular acceleration in rad / sec^2).
*
* @return the current acceleration
*/
double getAcceleration() const;
/**
* Returns the maximum torque input.
*
* @return the max torque input
*/
double getMaxTorque() const;
protected:
double inputTorque = 0; // N*m
double maxTorque = 0.5649; // N*m
double angle = 0; // rad
double omega = 0; // rad / sec
double accel = 0; // rad / sec^2
double mass; // kg
double linkLen; // m
double muStatic; // N*m
double muDynamic; // N*m
double timestep; // sec
double I = 0; // moment of inertia
std::function<double(double, double, double)> torqueFunc;
const double minTimestep = 0.000001; // 1 us
virtual double stepImpl();
};
} // namespace okapi

23
SerialTest/include/okapi/api/control/util/pathfinderUtil.hpp Normal file
View File

@ -0,0 +1,23 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/units/QAngle.hpp"
#include "okapi/api/units/QLength.hpp"
namespace okapi {
struct PathfinderPoint {
QLength x; // X coordinate relative to the start of the movement
QLength y; // Y coordinate relative to the start of the movement
QAngle theta; // Exit angle relative to the start of the movement
};
struct PathfinderLimits {
double maxVel; // Maximum robot velocity in m/s
double maxAccel; // Maximum robot acceleration in m/s/s
double maxJerk; // Maximum robot jerk in m/s/s/s
};
} // namespace okapi

80
SerialTest/include/okapi/api/control/util/pidTuner.hpp Normal file
View File

@ -0,0 +1,80 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/control/controllerInput.hpp"
#include "okapi/api/control/controllerOutput.hpp"
#include "okapi/api/control/iterative/iterativePosPidController.hpp"
#include "okapi/api/units/QTime.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <memory>
#include <vector>
namespace okapi {
class PIDTuner {
public:
struct Output {
double kP, kI, kD;
};
PIDTuner(const std::shared_ptr<ControllerInput<double>> &iinput,
const std::shared_ptr<ControllerOutput<double>> &ioutput,
const TimeUtil &itimeUtil,
QTime itimeout,
std::int32_t igoal,
double ikPMin,
double ikPMax,
double ikIMin,
double ikIMax,
double ikDMin,
double ikDMax,
std::size_t inumIterations = 5,
std::size_t inumParticles = 16,
double ikSettle = 1,
double ikITAE = 2,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
virtual ~PIDTuner();
virtual Output autotune();
protected:
static constexpr double inertia = 0.5; // Particle inertia
static constexpr double confSelf = 1.1; // Self confidence
static constexpr double confSwarm = 1.2; // Particle swarm confidence
static constexpr int increment = 5;
static constexpr int divisor = 5;
static constexpr QTime loopDelta = 10_ms; // NOLINT
struct Particle {
double pos, vel, best;
};
struct ParticleSet {
Particle kP, kI, kD;
double bestError;
};
std::shared_ptr<Logger> logger;
TimeUtil timeUtil;
std::shared_ptr<ControllerInput<double>> input;
std::shared_ptr<ControllerOutput<double>> output;
const QTime timeout;
const std::int32_t goal;
const double kPMin;
const double kPMax;
const double kIMin;
const double kIMax;
const double kDMin;
const double kDMax;
const std::size_t numIterations;
const std::size_t numParticles;
const double kSettle;
const double kITAE;
};
} // namespace okapi

51
SerialTest/include/okapi/api/control/util/settledUtil.hpp Normal file
View File

@ -0,0 +1,51 @@
/*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#pragma once
#include "okapi/api/units/QTime.hpp"
#include "okapi/api/util/abstractTimer.hpp"
#include <memory>
namespace okapi {
class SettledUtil {
public:
/**
* A utility class to determine if a control loop has settled based on error. A control loop is
* settled if the error is within `iatTargetError` and `iatTargetDerivative` for `iatTargetTime`.
*
* @param iatTargetTimer A timer used to track `iatTargetTime`.
* @param iatTargetError The minimum error to be considered settled.
* @param iatTargetDerivative The minimum error derivative to be considered settled.
* @param iatTargetTime The minimum time within atTargetError to be considered settled.
*/
explicit SettledUtil(std::unique_ptr<AbstractTimer> iatTargetTimer,
double iatTargetError = 50,
double iatTargetDerivative = 5,
QTime iatTargetTime = 250_ms);
virtual ~SettledUtil();
/**
* Returns whether the controller is settled.
*
* @param ierror The current error.
* @return Whether the controller is settled.
*/
virtual bool isSettled(double ierror);
/**
* Resets the "at target" timer and clears the previous error.
*/
virtual void reset();
protected:
double atTargetError = 50;
double atTargetDerivative = 5;
QTime atTargetTime = 250_ms;
std::unique_ptr<AbstractTimer> atTargetTimer;
double lastError = 0;
};
} // namespace okapi