Initial commit - test serial
This commit is contained in:
Cole A. Deck
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
287
SerialTest/include/okapi/api/control/async/asyncWrapper.hpp
Normal 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
|
||||
Reference in New Issue
Block a user