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

134
SerialTest/include/okapi/api.hpp Normal file
View File

@ -0,0 +1,134 @@
/*
* 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
/** @mainpage OkapiLib Index Page
*
* @section intro_sec Introduction
*
* **OkapiLib** is a PROS library for programming VEX V5 robots. This library is intended to raise
* the floor for teams with all levels of experience. New teams should have an easier time getting
* their robot up and running, and veteran teams should find that OkapiLib doesn't get in the way or
* place any limits on functionality.
*
* For tutorials on how to get the most out of OkapiLib, see the
* [Tutorials](docs/tutorials/index.md) section. For documentation on using the OkapiLib API, see
* the [API](docs/api/index.md) section.
*
* @section getting_started Getting Started
* Not sure where to start? Take a look at the
* [Getting Started](docs/tutorials/walkthrough/gettingStarted.md) tutorial.
* Once you have OkapiLib set up, check out the
* [Clawbot](docs/tutorials/walkthrough/clawbot.md) tutorial.
*
* @section using_docs Using The Documentation
*
* Start with reading the [Tutorials](docs/tutorials/index.md). Use the [API](docs/api/index.md)
* section to explore the class hierarchy. To see a list of all available classes, use the
* [Classes](annotated.html) section.
*
* This documentation has a powerful search feature, which can be brought up with the keyboard
* shortcuts `Tab` or `T`. All exports to the `okapi` namespace such as enums, constants, units, or
* functions can be found [here](@ref okapi).
*/
#include "okapi/api/chassis/controller/chassisControllerIntegrated.hpp"
#include "okapi/api/chassis/controller/chassisControllerPid.hpp"
#include "okapi/api/chassis/controller/chassisScales.hpp"
#include "okapi/api/chassis/controller/defaultOdomChassisController.hpp"
#include "okapi/api/chassis/controller/odomChassisController.hpp"
#include "okapi/api/chassis/model/hDriveModel.hpp"
#include "okapi/api/chassis/model/readOnlyChassisModel.hpp"
#include "okapi/api/chassis/model/skidSteerModel.hpp"
#include "okapi/api/chassis/model/threeEncoderSkidSteerModel.hpp"
#include "okapi/api/chassis/model/threeEncoderXDriveModel.hpp"
#include "okapi/api/chassis/model/xDriveModel.hpp"
#include "okapi/impl/chassis/controller/chassisControllerBuilder.hpp"
#include "okapi/api/control/async/asyncLinearMotionProfileController.hpp"
#include "okapi/api/control/async/asyncMotionProfileController.hpp"
#include "okapi/api/control/async/asyncPosIntegratedController.hpp"
#include "okapi/api/control/async/asyncPosPidController.hpp"
#include "okapi/api/control/async/asyncVelIntegratedController.hpp"
#include "okapi/api/control/async/asyncVelPidController.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/iterativeMotorVelocityController.hpp"
#include "okapi/api/control/iterative/iterativePosPidController.hpp"
#include "okapi/api/control/iterative/iterativeVelPidController.hpp"
#include "okapi/api/control/util/controllerRunner.hpp"
#include "okapi/api/control/util/flywheelSimulator.hpp"
#include "okapi/api/control/util/pidTuner.hpp"
#include "okapi/api/control/util/settledUtil.hpp"
#include "okapi/impl/control/async/asyncMotionProfileControllerBuilder.hpp"
#include "okapi/impl/control/async/asyncPosControllerBuilder.hpp"
#include "okapi/impl/control/async/asyncVelControllerBuilder.hpp"
#include "okapi/impl/control/iterative/iterativeControllerFactory.hpp"
#include "okapi/impl/control/util/controllerRunnerFactory.hpp"
#include "okapi/impl/control/util/pidTunerFactory.hpp"
#include "okapi/api/odometry/odomMath.hpp"
#include "okapi/api/odometry/odometry.hpp"
#include "okapi/api/odometry/threeEncoderOdometry.hpp"
#include "okapi/api/device/rotarysensor/continuousRotarySensor.hpp"
#include "okapi/api/device/rotarysensor/rotarySensor.hpp"
#include "okapi/impl/device/adiUltrasonic.hpp"
#include "okapi/impl/device/button/adiButton.hpp"
#include "okapi/impl/device/button/controllerButton.hpp"
#include "okapi/impl/device/controller.hpp"
#include "okapi/impl/device/distanceSensor.hpp"
#include "okapi/impl/device/motor/adiMotor.hpp"
#include "okapi/impl/device/motor/motor.hpp"
#include "okapi/impl/device/motor/motorGroup.hpp"
#include "okapi/impl/device/opticalSensor.hpp"
#include "okapi/impl/device/rotarysensor/IMU.hpp"
#include "okapi/impl/device/rotarysensor/adiEncoder.hpp"
#include "okapi/impl/device/rotarysensor/adiGyro.hpp"
#include "okapi/impl/device/rotarysensor/integratedEncoder.hpp"
#include "okapi/impl/device/rotarysensor/potentiometer.hpp"
#include "okapi/impl/device/rotarysensor/rotationSensor.hpp"
#include "okapi/api/filter/averageFilter.hpp"
#include "okapi/api/filter/composableFilter.hpp"
#include "okapi/api/filter/demaFilter.hpp"
#include "okapi/api/filter/ekfFilter.hpp"
#include "okapi/api/filter/emaFilter.hpp"
#include "okapi/api/filter/filter.hpp"
#include "okapi/api/filter/filteredControllerInput.hpp"
#include "okapi/api/filter/medianFilter.hpp"
#include "okapi/api/filter/passthroughFilter.hpp"
#include "okapi/api/filter/velMath.hpp"
#include "okapi/impl/filter/velMathFactory.hpp"
#include "okapi/api/units/QAcceleration.hpp"
#include "okapi/api/units/QAngle.hpp"
#include "okapi/api/units/QAngularAcceleration.hpp"
#include "okapi/api/units/QAngularJerk.hpp"
#include "okapi/api/units/QAngularSpeed.hpp"
#include "okapi/api/units/QArea.hpp"
#include "okapi/api/units/QForce.hpp"
#include "okapi/api/units/QFrequency.hpp"
#include "okapi/api/units/QJerk.hpp"
#include "okapi/api/units/QLength.hpp"
#include "okapi/api/units/QMass.hpp"
#include "okapi/api/units/QPressure.hpp"
#include "okapi/api/units/QSpeed.hpp"
#include "okapi/api/units/QTime.hpp"
#include "okapi/api/units/QTorque.hpp"
#include "okapi/api/units/QVolume.hpp"
#include "okapi/api/units/RQuantityName.hpp"
#include "okapi/api/util/abstractRate.hpp"
#include "okapi/api/util/abstractTimer.hpp"
#include "okapi/api/util/mathUtil.hpp"
#include "okapi/api/util/supplier.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include "okapi/impl/util/configurableTimeUtilFactory.hpp"
#include "okapi/impl/util/rate.hpp"
#include "okapi/impl/util/timeUtilFactory.hpp"
#include "okapi/impl/util/timer.hpp"

142
SerialTest/include/okapi/api/chassis/controller/chassisController.hpp Normal file
View File

@ -0,0 +1,142 @@
/*
* 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/chassisModel.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/units/QAngle.hpp"
#include "okapi/api/units/QLength.hpp"
#include <memory>
#include <valarray>
namespace okapi {
class ChassisController {
public:
/**
* A ChassisController adds a closed-loop layer on top of a ChassisModel. moveDistance and
* turnAngle both use closed-loop control to move the robot. There are passthrough functions for
* everything defined in ChassisModel.
*
* @param imodel underlying ChassisModel
*/
explicit ChassisController() = default;
virtual ~ChassisController() = default;
/**
* Drives the robot straight for a distance (using closed-loop control).
*
* @param itarget distance to travel
*/
virtual void moveDistance(QLength itarget) = 0;
/**
* Drives the robot straight for a distance (using closed-loop control).
*
* @param itarget distance to travel in motor degrees
*/
virtual void moveRaw(double itarget) = 0;
/**
* Sets the target distance for the robot to drive straight (using closed-loop control).
*
* @param itarget distance to travel
*/
virtual void moveDistanceAsync(QLength itarget) = 0;
/**
* Sets the target distance for the robot to drive straight (using closed-loop control).
*
* @param itarget distance to travel in motor degrees
*/
virtual void moveRawAsync(double itarget) = 0;
/**
* Turns the robot clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for
*/
virtual void turnAngle(QAngle idegTarget) = 0;
/**
* Turns the robot clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for in motor degrees
*/
virtual void turnRaw(double idegTarget) = 0;
/**
* Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for
*/
virtual void turnAngleAsync(QAngle idegTarget) = 0;
/**
* Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for in motor degrees
*/
virtual void turnRawAsync(double idegTarget) = 0;
/**
* Sets whether turns should be mirrored.
*
* @param ishouldMirror whether turns should be mirrored
*/
virtual void setTurnsMirrored(bool ishouldMirror) = 0;
/**
* Checks whether the internal controllers are currently settled.
*
* @return Whether this ChassisController is settled.
*/
virtual bool isSettled() = 0;
/**
* Delays until the currently executing movement completes.
*/
virtual void waitUntilSettled() = 0;
/**
* Interrupts the current movement to stop the robot.
*/
virtual void stop() = 0;
/**
* Sets a new maximum velocity in RPM [0-600].
*
* @param imaxVelocity The new maximum velocity.
*/
virtual void setMaxVelocity(double imaxVelocity) = 0;
/**
* @return The maximum velocity in RPM [0-600].
*/
virtual double getMaxVelocity() const = 0;
/**
* Get the ChassisScales.
*/
virtual ChassisScales getChassisScales() const = 0;
/**
* Get the GearsetRatioPair.
*/
virtual AbstractMotor::GearsetRatioPair getGearsetRatioPair() const = 0;
/**
* @return The internal ChassisModel.
*/
virtual std::shared_ptr<ChassisModel> getModel() = 0;
/**
* @return The internal ChassisModel.
*/
virtual ChassisModel &model() = 0;
};
} // namespace okapi

184
SerialTest/include/okapi/api/chassis/controller/chassisControllerIntegrated.hpp Normal file
View File

@ -0,0 +1,184 @@
/*
* 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/chassisController.hpp"
#include "okapi/api/control/async/asyncPosIntegratedController.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
namespace okapi {
class ChassisControllerIntegrated : public ChassisController {
public:
/**
* ChassisController using the V5 motor's integrated control. Puts the motors into encoder count
* units. Throws a `std::invalid_argument` exception if the gear ratio is zero. The initial
* model's max velocity will be propagated to the controllers.
*
* @param itimeUtil The TimeUtil.
* @param imodel The ChassisModel used to read from sensors/write to motors.
* @param ileftController The controller used for the left side motors.
* @param irightController The controller used for the right side motors.
* @param igearset The internal gearset and external ratio used on the drive motors.
* @param iscales The ChassisScales.
* @param ilogger The logger this instance will log to.
*/
ChassisControllerIntegrated(
const TimeUtil &itimeUtil,
std::shared_ptr<ChassisModel> imodel,
std::unique_ptr<AsyncPosIntegratedController> ileftController,
std::unique_ptr<AsyncPosIntegratedController> irightController,
const AbstractMotor::GearsetRatioPair &igearset = AbstractMotor::gearset::green,
const ChassisScales &iscales = ChassisScales({1, 1}, imev5GreenTPR),
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
/**
* Drives the robot straight for a distance (using closed-loop control).
*
* ```cpp
* // Drive forward 6 inches
* chassis->moveDistance(6_in);
*
* // Drive backward 0.2 meters
* chassis->moveDistance(-0.2_m);
* ```
*
* @param itarget distance to travel
*/
void moveDistance(QLength itarget) override;
/**
* Drives the robot straight for a distance (using closed-loop control).
*
* ```cpp
* // Drive forward by spinning the motors 400 degrees
* chassis->moveRaw(400);
* ```
*
* @param itarget distance to travel in motor degrees
*/
void moveRaw(double itarget) override;
/**
* Sets the target distance for the robot to drive straight (using closed-loop control).
*
* @param itarget distance to travel
*/
void moveDistanceAsync(QLength itarget) override;
/**
* Sets the target distance for the robot to drive straight (using closed-loop control).
*
* @param itarget distance to travel in motor degrees
*/
void moveRawAsync(double itarget) override;
/**
* Turns the robot clockwise in place (using closed-loop control).
*
* ```cpp
* // Turn 90 degrees clockwise
* chassis->turnAngle(90_deg);
* ```
*
* @param idegTarget angle to turn for
*/
void turnAngle(QAngle idegTarget) override;
/**
* Turns the robot clockwise in place (using closed-loop control).
*
* ```cpp
* // Turn clockwise by spinning the motors 200 degrees
* chassis->turnRaw(200);
* ```
*
* @param idegTarget angle to turn for in motor degrees
*/
void turnRaw(double idegTarget) override;
/**
* Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for
*/
void turnAngleAsync(QAngle idegTarget) override;
/**
* Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for in motor degrees
*/
void turnRawAsync(double idegTarget) override;
/**
* Sets whether turns should be mirrored.
*
* @param ishouldMirror whether turns should be mirrored
*/
void setTurnsMirrored(bool ishouldMirror) override;
/**
* Checks whether the internal controllers are currently settled.
*
* @return Whether this ChassisController is settled.
*/
bool isSettled() override;
/**
* Delays until the currently executing movement completes.
*/
void waitUntilSettled() override;
/**
* Interrupts the current movement to stop the robot.
*/
void stop() override;
/**
* Get the ChassisScales.
*/
ChassisScales getChassisScales() const override;
/**
* Get the GearsetRatioPair.
*/
AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;
/**
* @return The internal ChassisModel.
*/
std::shared_ptr<ChassisModel> getModel() override;
/**
* @return The internal ChassisModel.
*/
ChassisModel &model() override;
/**
* Sets a new maximum velocity in RPM [0-600].
*
* @param imaxVelocity The new maximum velocity.
*/
void setMaxVelocity(double imaxVelocity) override;
/**
* @return The maximum velocity in RPM [0-600].
*/
double getMaxVelocity() const override;
protected:
std::shared_ptr<Logger> logger;
bool normalTurns{true};
std::shared_ptr<ChassisModel> chassisModel;
TimeUtil timeUtil;
std::unique_ptr<AsyncPosIntegratedController> leftController;
std::unique_ptr<AsyncPosIntegratedController> rightController;
int lastTarget;
ChassisScales scales;
AbstractMotor::GearsetRatioPair gearsetRatioPair;
};
} // namespace okapi

275
SerialTest/include/okapi/api/chassis/controller/chassisControllerPid.hpp Normal file
View File

@ -0,0 +1,275 @@
/*
* 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/chassisController.hpp"
#include "okapi/api/control/iterative/iterativePosPidController.hpp"
#include "okapi/api/util/abstractRate.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <atomic>
#include <memory>
#include <tuple>
namespace okapi {
class ChassisControllerPID : public ChassisController {
public:
/**
* ChassisController using PID control. Puts the motors into encoder count units. Throws a
* `std::invalid_argument` exception if the gear ratio is zero.
*
* @param itimeUtil The TimeUtil.
* @param imodel The ChassisModel used to read from sensors/write to motors.
* @param idistanceController The PID controller that controls chassis distance for driving
* straight.
* @param iturnController The PID controller that controls chassis angle for turning.
* @param iangleController The PID controller that controls chassis angle for driving straight.
* @param igearset The internal gearset and external ratio used on the drive motors.
* @param iscales The ChassisScales.
* @param ilogger The logger this instance will log to.
*/
ChassisControllerPID(
TimeUtil itimeUtil,
std::shared_ptr<ChassisModel> imodel,
std::unique_ptr<IterativePosPIDController> idistanceController,
std::unique_ptr<IterativePosPIDController> iturnController,
std::unique_ptr<IterativePosPIDController> iangleController,
const AbstractMotor::GearsetRatioPair &igearset = AbstractMotor::gearset::green,
const ChassisScales &iscales = ChassisScales({1, 1}, imev5GreenTPR),
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
ChassisControllerPID(const ChassisControllerPID &) = delete;
ChassisControllerPID(ChassisControllerPID &&other) = delete;
ChassisControllerPID &operator=(const ChassisControllerPID &other) = delete;
ChassisControllerPID &operator=(ChassisControllerPID &&other) = delete;
~ChassisControllerPID() override;
/**
* Drives the robot straight for a distance (using closed-loop control).
*
* ```cpp
* // Drive forward 6 inches
* chassis->moveDistance(6_in);
*
* // Drive backward 0.2 meters
* chassis->moveDistance(-0.2_m);
* ```
*
* @param itarget distance to travel
*/
void moveDistance(QLength itarget) override;
/**
* Drives the robot straight for a distance (using closed-loop control).
*
* ```cpp
* // Drive forward by spinning the motors 400 degrees
* chassis->moveRaw(400);
* ```
*
* @param itarget distance to travel in motor degrees
*/
void moveRaw(double itarget) override;
/**
* Sets the target distance for the robot to drive straight (using closed-loop control).
*
* @param itarget distance to travel
*/
void moveDistanceAsync(QLength itarget) override;
/**
* Sets the target distance for the robot to drive straight (using closed-loop control).
*
* @param itarget distance to travel in motor degrees
*/
void moveRawAsync(double itarget) override;
/**
* Turns the robot clockwise in place (using closed-loop control).
*
* ```cpp
* // Turn 90 degrees clockwise
* chassis->turnAngle(90_deg);
* ```
*
* @param idegTarget angle to turn for
*/
void turnAngle(QAngle idegTarget) override;
/**
* Turns the robot clockwise in place (using closed-loop control).
*
* ```cpp
* // Turn clockwise by spinning the motors 200 degrees
* chassis->turnRaw(200);
* ```
*
* @param idegTarget angle to turn for in motor degrees
*/
void turnRaw(double idegTarget) override;
/**
* Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for
*/
void turnAngleAsync(QAngle idegTarget) override;
/**
* Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
*
* @param idegTarget angle to turn for in motor degrees
*/
void turnRawAsync(double idegTarget) override;
/**
* Sets whether turns should be mirrored.
*
* @param ishouldMirror whether turns should be mirrored
*/
void setTurnsMirrored(bool ishouldMirror) override;
/**
* Checks whether the internal controllers are currently settled.
*
* @return Whether this ChassisController is settled.
*/
bool isSettled() override;
/**
* Delays until the currently executing movement completes.
*/
void waitUntilSettled() override;
/**
* Gets the ChassisScales.
*/
ChassisScales getChassisScales() const override;
/**
* Gets the GearsetRatioPair.
*/
AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;
/**
* Sets the velocity mode flag. When the controller is in velocity mode, the control loop will
* set motor velocities. When the controller is in voltage mode (`ivelocityMode = false`), the
* control loop will set motor voltages. Additionally, when the controller is in voltage mode,
* it will not obey maximum velocity limits.
*
* @param ivelocityMode Whether the controller should be in velocity or voltage mode.
*/
void setVelocityMode(bool ivelocityMode);
/**
* Sets the gains for all controllers.
*
* @param idistanceGains The distance controller gains.
* @param iturnGains The turn controller gains.
* @param iangleGains The angle controller gains.
*/
void setGains(const IterativePosPIDController::Gains &idistanceGains,
const IterativePosPIDController::Gains &iturnGains,
const IterativePosPIDController::Gains &iangleGains);
/**
* Gets the current controller gains.
*
* @return The current controller gains in the order: distance, turn, angle.
*/
std::tuple<IterativePosPIDController::Gains,
IterativePosPIDController::Gains,
IterativePosPIDController::Gains>
getGains() const;
/**
* Starts the internal thread. This method is called by the ChassisControllerBuilder when making a
* new instance of this class.
*/
void startThread();
/**
* Returns the underlying thread handle.
*
* @return The underlying thread handle.
*/
CrossplatformThread *getThread() const;
/**
* Interrupts the current movement to stop the robot.
*/
void stop() override;
/**
* Sets a new maximum velocity in RPM [0-600]. In voltage mode, the max velocity is ignored and a
* max voltage should be set on the underlying ChassisModel instead.
*
* @param imaxVelocity The new maximum velocity.
*/
void setMaxVelocity(double imaxVelocity) override;
/**
* @return The maximum velocity in RPM [0-600].
*/
double getMaxVelocity() const override;
/**
* @return The internal ChassisModel.
*/
std::shared_ptr<ChassisModel> getModel() override;
/**
* @return The internal ChassisModel.
*/
ChassisModel &model() override;
protected:
std::shared_ptr<Logger> logger;
bool normalTurns{true};
std::shared_ptr<ChassisModel> chassisModel;
TimeUtil timeUtil;
std::unique_ptr<IterativePosPIDController> distancePid;
std::unique_ptr<IterativePosPIDController> turnPid;
std::unique_ptr<IterativePosPIDController> anglePid;
ChassisScales scales;
AbstractMotor::GearsetRatioPair gearsetRatioPair;
bool velocityMode{true};
std::atomic_bool doneLooping{true};
std::atomic_bool doneLoopingSeen{true};
std::atomic_bool newMovement{false};
std::atomic_bool dtorCalled{false};
QTime threadSleepTime{10_ms};
static void trampoline(void *context);
void loop();
/**
* Wait for the distance setup (distancePid and anglePid) to settle.
*
* @return true if done settling; false if settling should be tried again
*/
bool waitForDistanceSettled();
/**
* Wait for the angle setup (anglePid) to settle.
*
* @return true if done settling; false if settling should be tried again
*/
bool waitForAngleSettled();
/**
* Stops all the controllers and the ChassisModel.
*/
void stopAfterSettled();
typedef enum { distance, angle, none } modeType;
modeType mode{none};
CrossplatformThread *task{nullptr};
};
} // namespace okapi

88
SerialTest/include/okapi/api/chassis/controller/chassisScales.hpp Normal file
View File

@ -0,0 +1,88 @@
/*
* 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"
#include "okapi/api/units/RQuantity.hpp"
#include "okapi/api/util/logging.hpp"
#include <initializer_list>
#include <stdexcept>
#include <vector>
namespace okapi {
class ChassisScales {
public:
/**
* The scales a ChassisController needs to do all of its closed-loop control. The first element is
* the wheel diameter, the second element is the wheel track. For three-encoder configurations,
* the length from the center of rotation to the middle wheel and the middle wheel diameter are
* passed as the third and fourth elements.
*
* The wheel track is the center-to-center distance between the wheels (center-to-center
* meaning the width between the centers of both wheels). For example, if you are using four inch
* omni wheels and there are 11.5 inches between the centers of each wheel, you would call the
* constructor like so:
* `ChassisScales scales({4_in, 11.5_in}, imev5GreenTPR); // imev5GreenTPR for a green gearset`
*
* Wheel diameter
*
* +-+ Center of rotation
* | | |
* v v +----------+ Length to middle wheel
* | | from center of rotation
* +---> === | === |
* | + v + |
* | ++---------------++ |
* | | | v
* Wheel track | | |
* | | x |+| <-- Middle wheel
* | | |
* | | |
* | ++---------------++
* | + +
* +---> === ===
*
*
* @param idimensions {wheel diameter, wheel track} or {wheel diameter, wheel track, length to
* middle wheel, middle wheel diameter}.
* @param itpr The ticks per revolution of the encoders.
* @param ilogger The logger this instance will log to.
*/
ChassisScales(const std::initializer_list<QLength> &idimensions,
double itpr,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* The scales a ChassisController needs to do all of its closed-loop control. The first element is
* the straight scale, the second element is the turn scale. Optionally, the length from the
* center of rotation to the middle wheel and the middle scale can be passed as the third and
* fourth elements. The straight scale converts motor degrees to meters, the turn scale converts
* motor degrees to robot turn degrees, and the middle scale converts middle wheel degrees to
* meters.
*
* @param iscales {straight scale, turn scale} or {straight scale, turn scale, length to middle
* wheel in meters, middle scale}.
* @param itpr The ticks per revolution of the encoders.
* @param ilogger The logger this instance will log to.
*/
ChassisScales(const std::initializer_list<double> &iscales,
double itpr,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
QLength wheelDiameter;
QLength wheelTrack;
QLength middleWheelDistance;
QLength middleWheelDiameter;
double straight;
double turn;
double middle;
double tpr;
protected:
static void validateInputSize(std::size_t inputSize, const std::shared_ptr<Logger> &logger);
};
} // namespace okapi

183
SerialTest/include/okapi/api/chassis/controller/defaultOdomChassisController.hpp Normal file
View File

@ -0,0 +1,183 @@
/*
* 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/chassisControllerIntegrated.hpp"
#include "okapi/api/chassis/controller/odomChassisController.hpp"
#include "okapi/api/chassis/model/skidSteerModel.hpp"
#include "okapi/api/odometry/odometry.hpp"
#include <memory>
namespace okapi {
class DefaultOdomChassisController : public OdomChassisController {
public:
/**
* Odometry based chassis controller that moves using a separately constructed chassis controller.
* Spins up a task at the default priority plus 1 for odometry when constructed.
*
* Moves the robot around in the odom frame. Instead of telling the robot to drive forward or
* turn some amount, you instead tell it to drive to a specific point on the field or turn to
* a specific angle, relative to its starting position.
*
* @param itimeUtil The TimeUtil.
* @param iodometry The odometry to read state estimates from.
* @param icontroller The chassis controller to delegate to.
* @param imode The new default StateMode used to interpret target points and query the Odometry
* state.
* @param imoveThreshold minimum length movement (smaller movements will be skipped)
* @param iturnThreshold minimum angle turn (smaller turns will be skipped)
* @param ilogger The logger this instance will log to.
*/
DefaultOdomChassisController(const TimeUtil &itimeUtil,
std::shared_ptr<Odometry> iodometry,
std::shared_ptr<ChassisController> icontroller,
const StateMode &imode = StateMode::FRAME_TRANSFORMATION,
QLength imoveThreshold = 0_mm,
QAngle iturnThreshold = 0_deg,
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
DefaultOdomChassisController(const DefaultOdomChassisController &) = delete;
DefaultOdomChassisController(DefaultOdomChassisController &&other) = delete;
DefaultOdomChassisController &operator=(const DefaultOdomChassisController &other) = delete;
DefaultOdomChassisController &operator=(DefaultOdomChassisController &&other) = delete;
/**
* Drives the robot straight to a point in the odom frame.
*
* @param ipoint The target point to navigate to.
* @param ibackwards Whether to drive to the target point backwards.
* @param ioffset An offset from the target point in the direction pointing towards the robot. The
* robot will stop this far away from the target point.
*/
void driveToPoint(const Point &ipoint,
bool ibackwards = false,
const QLength &ioffset = 0_mm) override;
/**
* Turns the robot to face a point in the odom frame.
*
* @param ipoint The target point to turn to face.
*/
void turnToPoint(const Point &ipoint) override;
/**
* @return The internal ChassisController.
*/
std::shared_ptr<ChassisController> getChassisController();
/**
* @return The internal ChassisController.
*/
ChassisController &chassisController();
/**
* This delegates to the input ChassisController.
*/
void turnToAngle(const QAngle &iangle) override;
/**
* This delegates to the input ChassisController.
*/
void moveDistance(QLength itarget) override;
/**
* This delegates to the input ChassisController.
*/
void moveRaw(double itarget) override;
/**
* This delegates to the input ChassisController.
*/
void moveDistanceAsync(QLength itarget) override;
/**
* This delegates to the input ChassisController.
*/
void moveRawAsync(double itarget) override;
/**
* Turns chassis to desired angle (turns in the direction of smallest angle)
* (ex. If current angle is 0 and target is 270, the chassis will turn -90 degrees)
*
* @param idegTarget target angle
*/
void turnAngle(QAngle idegTarget) override;
/**
* This delegates to the input ChassisController.
*/
void turnRaw(double idegTarget) override;
/**
* Turns chassis to desired angle (turns in the direction of smallest angle)
* (ex. If current angle is 0 and target is 270, the chassis will turn -90 degrees)
*
* @param idegTarget target angle
*/
void turnAngleAsync(QAngle idegTarget) override;
/**
* This delegates to the input ChassisController.
*/
void turnRawAsync(double idegTarget) override;
/**
* This delegates to the input ChassisController.
*/
void setTurnsMirrored(bool ishouldMirror) override;
/**
* This delegates to the input ChassisController.
*/
bool isSettled() override;
/**
* This delegates to the input ChassisController.
*/
void waitUntilSettled() override;
/**
* This delegates to the input ChassisController.
*/
void stop() override;
/**
* This delegates to the input ChassisController.
*/
void setMaxVelocity(double imaxVelocity) override;
/**
* This delegates to the input ChassisController.
*/
double getMaxVelocity() const override;
/**
* This delegates to the input ChassisController.
*/
ChassisScales getChassisScales() const override;
/**
* This delegates to the input ChassisController.
*/
AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;
/**
* This delegates to the input ChassisController.
*/
std::shared_ptr<ChassisModel> getModel() override;
/**
* This delegates to the input ChassisController.
*/
ChassisModel &model() override;
protected:
std::shared_ptr<Logger> logger;
std::shared_ptr<ChassisController> controller;
void waitForOdomTask();
};
} // namespace okapi

154
SerialTest/include/okapi/api/chassis/controller/odomChassisController.hpp Normal file
View File

@ -0,0 +1,154 @@
/*
* 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/chassisController.hpp"
#include "okapi/api/chassis/model/skidSteerModel.hpp"
#include "okapi/api/coreProsAPI.hpp"
#include "okapi/api/odometry/odometry.hpp"
#include "okapi/api/odometry/point.hpp"
#include "okapi/api/units/QSpeed.hpp"
#include "okapi/api/util/abstractRate.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <atomic>
#include <memory>
#include <valarray>
namespace okapi {
class OdomChassisController : public ChassisController {
public:
/**
* Odometry based chassis controller. Starts task at the default for odometry when constructed,
* which calls `Odometry::step` every `10ms`. The default StateMode is
* `StateMode::FRAME_TRANSFORMATION`.
*
* Moves the robot around in the odom frame. Instead of telling the robot to drive forward or
* turn some amount, you instead tell it to drive to a specific point on the field or turn to
* a specific angle relative to its starting position.
*
* @param itimeUtil The TimeUtil.
* @param iodometry The Odometry instance to run in a new task.
* @param imode The new default StateMode used to interpret target points and query the Odometry
* state.
* @param imoveThreshold minimum length movement (smaller movements will be skipped)
* @param iturnThreshold minimum angle turn (smaller turns will be skipped)
*/
OdomChassisController(TimeUtil itimeUtil,
std::shared_ptr<Odometry> iodometry,
const StateMode &imode = StateMode::FRAME_TRANSFORMATION,
const QLength &imoveThreshold = 0_mm,
const QAngle &iturnThreshold = 0_deg,
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
~OdomChassisController() override;
OdomChassisController(const OdomChassisController &) = delete;
OdomChassisController(OdomChassisController &&other) = delete;
OdomChassisController &operator=(const OdomChassisController &other) = delete;
OdomChassisController &operator=(OdomChassisController &&other) = delete;
/**
* Drives the robot straight to a point in the odom frame.
*
* @param ipoint The target point to navigate to.
* @param ibackwards Whether to drive to the target point backwards.
* @param ioffset An offset from the target point in the direction pointing towards the robot. The
* robot will stop this far away from the target point.
*/
virtual void
driveToPoint(const Point &ipoint, bool ibackwards = false, const QLength &ioffset = 0_mm) = 0;
/**
* Turns the robot to face a point in the odom frame.
*
* @param ipoint The target point to turn to face.
*/
virtual void turnToPoint(const Point &ipoint) = 0;
/**
* Turns the robot to face an angle in the odom frame.
*
* @param iangle The angle to turn to.
*/
virtual void turnToAngle(const QAngle &iangle) = 0;
/**
* @return The current state.
*/
virtual OdomState getState() const;
/**
* Set a new state to be the current state. The default StateMode is
* `StateMode::FRAME_TRANSFORMATION`.
*
* @param istate The new state in the given format.
* @param imode The mode to treat the input state as.
*/
virtual void setState(const OdomState &istate);
/**
* Sets a default StateMode that will be used to interpret target points and query the Odometry
* state.
*
* @param imode The new default StateMode.
*/
void setDefaultStateMode(const StateMode &imode);
/**
* Set a new move threshold. Any requested movements smaller than this threshold will be skipped.
*
* @param imoveThreshold new move threshold
*/
virtual void setMoveThreshold(const QLength &imoveThreshold);
/**
* Set a new turn threshold. Any requested turns smaller than this threshold will be skipped.
*
* @param iturnTreshold new turn threshold
*/
virtual void setTurnThreshold(const QAngle &iturnTreshold);
/**
* @return The current move threshold.
*/
virtual QLength getMoveThreshold() const;
/**
* @return The current turn threshold.
*/
virtual QAngle getTurnThreshold() const;
/**
* Starts the internal odometry thread. This should not be called by normal users.
*/
void startOdomThread();
/**
* @return The underlying thread handle.
*/
CrossplatformThread *getOdomThread() const;
/**
* @return The internal odometry.
*/
std::shared_ptr<Odometry> getOdometry();
protected:
std::shared_ptr<Logger> logger;
TimeUtil timeUtil;
QLength moveThreshold;
QAngle turnThreshold;
std::shared_ptr<Odometry> odom;
CrossplatformThread *odomTask{nullptr};
std::atomic_bool dtorCalled{false};
StateMode defaultStateMode{StateMode::FRAME_TRANSFORMATION};
std::atomic_bool odomTaskRunning{false};
static void trampoline(void *context);
void loop();
};
} // namespace okapi

165
SerialTest/include/okapi/api/chassis/model/chassisModel.hpp Normal file
View File

@ -0,0 +1,165 @@
/*
* 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/model/readOnlyChassisModel.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include <array>
#include <initializer_list>
#include <memory>
#include <vector>
namespace okapi {
/**
* A version of the ReadOnlyChassisModel that also supports write methods, such as setting motor
* speed. Because this class can write to motors, there can only be one owner and as such copying
* is disabled.
*/
class ChassisModel : public ReadOnlyChassisModel {
public:
explicit ChassisModel() = default;
ChassisModel(const ChassisModel &) = delete;
ChassisModel &operator=(const ChassisModel &) = delete;
/**
* Drive the robot forwards (using open-loop control). Uses velocity mode.
*
* @param ipower motor power
*/
virtual void forward(double ispeed) = 0;
/**
* Drive the robot in an arc (using open-loop control). Uses velocity mode.
* The algorithm is (approximately):
* leftPower = forwardSpeed + yaw
* rightPower = forwardSpeed - yaw
*
* @param iforwadSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
*/
virtual void driveVector(double iforwardSpeed, double iyaw) = 0;
/**
* Drive the robot in an arc. Uses voltage mode.
* The algorithm is (approximately):
* leftPower = forwardSpeed + yaw
* rightPower = forwardSpeed - yaw
*
* @param iforwadSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
*/
virtual void driveVectorVoltage(double iforwardSpeed, double iyaw) = 0;
/**
* Turn the robot clockwise (using open-loop control). Uses velocity mode.
*
* @param ispeed motor power
*/
virtual void rotate(double ispeed) = 0;
/**
* Stop the robot (set all the motors to 0). Uses velocity mode.
*/
virtual void stop() = 0;
/**
* Drive the robot with a tank drive layout. Uses voltage mode.
*
* @param ileftSpeed left side speed
* @param irightSpeed right side speed
* @param ithreshold deadband on joystick values
*/
virtual void tank(double ileftSpeed, double irightSpeed, double ithreshold = 0) = 0;
/**
* Drive the robot with an arcade drive layout. Uses voltage mode.
*
* @param iforwardSpeed speed forward direction
* @param iyaw speed around the vertical axis
* @param ithreshold deadband on joystick values
*/
virtual void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) = 0;
/**
* Drive the robot with a curvature drive layout. The robot drives in constant radius turns
* where you control the curvature (inverse of radius) you drive in. This is advantageous
* because the forward speed will not affect the rate of turning. The algorithm switches to
* arcade if the forward speed is 0. Uses voltage mode.
*
* @param iforwardSpeed speed in the forward direction
* @param icurvature curvature (inverse of radius) to drive in
* @param ithreshold deadband on joystick values
*/
virtual void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) = 0;
/**
* Power the left side motors. Uses velocity mode.
*
* @param ipower motor power
*/
virtual void left(double ispeed) = 0;
/**
* Power the right side motors. Uses velocity mode.
*
* @param ipower motor power
*/
virtual void right(double ispeed) = 0;
/**
* Reset the sensors to their zero point.
*/
virtual void resetSensors() = 0;
/**
* Set the brake mode for each motor.
*
* @param mode new brake mode
*/
virtual void setBrakeMode(AbstractMotor::brakeMode mode) = 0;
/**
* Set the encoder units for each motor.
*
* @param units new motor encoder units
*/
virtual void setEncoderUnits(AbstractMotor::encoderUnits units) = 0;
/**
* Set the gearset for each motor.
*
* @param gearset new motor gearset
*/
virtual void setGearing(AbstractMotor::gearset gearset) = 0;
/**
* Sets a new maximum velocity in RPM. The usable maximum depends on the maximum velocity of the
* currently installed gearset. If the configured maximum velocity is greater than the attainable
* maximum velocity from the currently installed gearset, the ChassisModel will still scale to
* that velocity.
*
* @param imaxVelocity The new maximum velocity.
*/
virtual void setMaxVelocity(double imaxVelocity) = 0;
/**
* @return The current maximum velocity.
*/
virtual double getMaxVelocity() const = 0;
/**
* Sets a new maximum voltage in mV in the range `[0-12000]`.
*
* @param imaxVoltage The new maximum voltage.
*/
virtual void setMaxVoltage(double imaxVoltage) = 0;
/**
* @return The maximum voltage in mV `[0-12000]`.
*/
virtual double getMaxVoltage() const = 0;
};
} // namespace okapi

247
SerialTest/include/okapi/api/chassis/model/hDriveModel.hpp Normal file
View File

@ -0,0 +1,247 @@
/*
* 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/model/chassisModel.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/device/rotarysensor/continuousRotarySensor.hpp"
namespace okapi {
class HDriveModel : public ChassisModel {
public:
/**
* Model for an h-drive (wheels parallel with robot's direction of motion, with an additional
* wheel perpendicular to those). When the left and right side motors are powered +100%, the robot
* should move forward in a straight line. When the middle motor is powered +100%, the robot
* should strafe right in a straight line.
*
* @param ileftSideMotor The left side motor.
* @param irightSideMotor The right side motor.
* @param imiddleMotor The middle (perpendicular) motor.
* @param ileftEnc The left side encoder.
* @param irightEnc The right side encoder.
* @param imiddleEnc The middle encoder.
*/
HDriveModel(std::shared_ptr<AbstractMotor> ileftSideMotor,
std::shared_ptr<AbstractMotor> irightSideMotor,
std::shared_ptr<AbstractMotor> imiddleMotor,
std::shared_ptr<ContinuousRotarySensor> ileftEnc,
std::shared_ptr<ContinuousRotarySensor> irightEnc,
std::shared_ptr<ContinuousRotarySensor> imiddleEnc,
double imaxVelocity,
double imaxVoltage);
/**
* Drive the robot forwards (using open-loop control). Uses velocity mode. Sets the middle motor
* to zero velocity.
*
* @param ispeed motor power
*/
void forward(double ispeed) override;
/**
* Drive the robot in an arc (using open-loop control). Uses velocity mode. Sets the middle motor
* to zero velocity.
*
* The algorithm is (approximately):
* leftPower = ySpeed + zRotation
* rightPower = ySpeed - zRotation
*
* @param iySpeed speed on y axis (forward)
* @param izRotation speed around z axis (up)
*/
void driveVector(double iySpeed, double izRotation) override;
/**
* Drive the robot in an arc. Uses voltage mode. Sets the middle motor to zero velocity.
*
* The algorithm is (approximately):
* leftPower = forwardSpeed + yaw
* rightPower = forwardSpeed - yaw
*
* @param iforwadSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
*/
void driveVectorVoltage(double iforwardSpeed, double iyaw) override;
/**
* Turn the robot clockwise (using open-loop control). Uses velocity mode. Sets the middle motor
* to zero velocity.
*
* @param ispeed motor power
*/
void rotate(double ispeed) override;
/**
* Stop the robot (set all the motors to 0). Uses velocity mode.
*/
void stop() override;
/**
* Drive the robot with a tank drive layout. Uses voltage mode. Sets the middle motor to zero
* velocity.
*
* @param ileftSpeed left side speed
* @param irightSpeed right side speed
* @param ithreshold deadband on joystick values
*/
void tank(double ileftSpeed, double irightSpeed, double ithreshold = 0) override;
/**
* Drive the robot with an arcade drive layout. Uses voltage mode. Sets the middle motor to zero
* velocity.
*
* @param iforwardSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
* @param ithreshold deadband on joystick values
*/
void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) override;
/**
* Drive the robot with a curvature drive layout. The robot drives in constant radius turns
* where you control the curvature (inverse of radius) you drive in. This is advantageous
* because the forward speed will not affect the rate of turning. The algorithm switches to
* arcade if the forward speed is 0. Uses voltage mode. Sets the middle motor to zero velocity.
*
* @param iforwardSpeed speed in the forward direction
* @param icurvature curvature (inverse of radius) to drive in
* @param ithreshold deadband on joystick values
*/
void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) override;
/**
* Drive the robot with an arcade drive layout. Uses voltage mode.
*
* @param irightSpeed speed to the right
* @param iforwardSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
* @param ithreshold deadband on joystick values
*/
virtual void
hArcade(double irightSpeed, double iforwardSpeed, double iyaw, double ithreshold = 0);
/**
* Drive the robot with an curvature drive layout. Uses voltage mode.
*
* @param irightSpeed speed to the right
* @param iforwardSpeed speed in the forward direction
* @param icurvature curvature (inverse of radius) to drive in
* @param ithreshold deadband on joystick values
*/
virtual void
hCurvature(double irightSpeed, double iforwardSpeed, double icurvature, double ithreshold = 0);
/**
* Power the left side motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
void left(double ispeed) override;
/**
* Power the right side motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
void right(double ispeed) override;
/**
* Power the middle motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
virtual void middle(double ispeed);
/**
* Read the sensors.
*
* @return sensor readings in the format {left, right, middle}
*/
std::valarray<std::int32_t> getSensorVals() const override;
/**
* Reset the sensors to their zero point.
*/
void resetSensors() override;
/**
* Set the brake mode for each motor.
*
* @param mode new brake mode
*/
void setBrakeMode(AbstractMotor::brakeMode mode) override;
/**
* Set the encoder units for each motor.
*
* @param units new motor encoder units
*/
void setEncoderUnits(AbstractMotor::encoderUnits units) override;
/**
* Set the gearset for each motor.
*
* @param gearset new motor gearset
*/
void setGearing(AbstractMotor::gearset gearset) override;
/**
* Sets a new maximum velocity in RPM. The usable maximum depends on the maximum velocity of the
* currently installed gearset. If the configured maximum velocity is greater than the attainable
* maximum velocity from the currently installed gearset, the ChassisModel will still scale to
* that velocity.
*
* @param imaxVelocity The new maximum velocity.
*/
void setMaxVelocity(double imaxVelocity) override;
/**
* @return The current maximum velocity.
*/
double getMaxVelocity() const override;
/**
* Sets a new maximum voltage in mV in the range `[0-12000]`.
*
* @param imaxVoltage The new maximum voltage.
*/
void setMaxVoltage(double imaxVoltage) override;
/**
* @return The maximum voltage in mV in the range `[0-12000]`.
*/
double getMaxVoltage() const override;
/**
* Returns the left side motor.
*
* @return the left side motor
*/
std::shared_ptr<AbstractMotor> getLeftSideMotor() const;
/**
* Returns the left side motor.
*
* @return the left side motor
*/
std::shared_ptr<AbstractMotor> getRightSideMotor() const;
/**
* @return The middle motor.
*/
std::shared_ptr<AbstractMotor> getMiddleMotor() const;
protected:
double maxVelocity;
double maxVoltage;
std::shared_ptr<AbstractMotor> leftSideMotor;
std::shared_ptr<AbstractMotor> rightSideMotor;
std::shared_ptr<AbstractMotor> middleMotor;
std::shared_ptr<ContinuousRotarySensor> leftSensor;
std::shared_ptr<ContinuousRotarySensor> rightSensor;
std::shared_ptr<ContinuousRotarySensor> middleSensor;
};
} // namespace okapi

28
SerialTest/include/okapi/api/chassis/model/readOnlyChassisModel.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/coreProsAPI.hpp"
#include <valarray>
namespace okapi {
/**
* A version of the ChassisModel that only supports read methods, such as querying sensor values.
* This class does not let you write to motors, so it supports having multiple owners and as a
* result copying is enabled.
*/
class ReadOnlyChassisModel {
public:
virtual ~ReadOnlyChassisModel() = default;
/**
* Read the sensors.
*
* @return sensor readings (format is implementation dependent)
*/
virtual std::valarray<std::int32_t> getSensorVals() const = 0;
};
} // namespace okapi

198
SerialTest/include/okapi/api/chassis/model/skidSteerModel.hpp Normal file
View File

@ -0,0 +1,198 @@
/*
* 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/model/chassisModel.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/device/rotarysensor/continuousRotarySensor.hpp"
namespace okapi {
class SkidSteerModel : public ChassisModel {
public:
/**
* Model for a skid steer drive (wheels parallel with robot's direction of motion). When all
* motors are powered +100%, the robot should move forward in a straight line.
*
* @param ileftSideMotor The left side motor.
* @param irightSideMotor The right side motor.
* @param ileftEnc The left side encoder.
* @param irightEnc The right side encoder.
*/
SkidSteerModel(std::shared_ptr<AbstractMotor> ileftSideMotor,
std::shared_ptr<AbstractMotor> irightSideMotor,
std::shared_ptr<ContinuousRotarySensor> ileftEnc,
std::shared_ptr<ContinuousRotarySensor> irightEnc,
double imaxVelocity,
double imaxVoltage);
/**
* Drive the robot forwards (using open-loop control). Uses velocity mode.
*
* @param ispeed motor power
*/
void forward(double ispeed) override;
/**
* Drive the robot in an arc (using open-loop control). Uses velocity mode.
* The algorithm is (approximately):
* leftPower = ySpeed + zRotation
* rightPower = ySpeed - zRotation
*
* @param iySpeed speed on y axis (forward)
* @param izRotation speed around z axis (up)
*/
void driveVector(double iySpeed, double izRotation) override;
/**
* Drive the robot in an arc. Uses voltage mode.
* The algorithm is (approximately):
* leftPower = forwardSpeed + yaw
* rightPower = forwardSpeed - yaw
*
* @param iforwadSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
*/
void driveVectorVoltage(double iforwardSpeed, double iyaw) override;
/**
* Turn the robot clockwise (using open-loop control). Uses velocity mode.
*
* @param ispeed motor power
*/
void rotate(double ispeed) override;
/**
* Stop the robot (set all the motors to 0). Uses velocity mode.
*/
void stop() override;
/**
* Drive the robot with a tank drive layout. Uses voltage mode.
*
* @param ileftSpeed left side speed
* @param irightSpeed right side speed
* @param ithreshold deadband on joystick values
*/
void tank(double ileftSpeed, double irightSpeed, double ithreshold = 0) override;
/**
* Drive the robot with an arcade drive layout. Uses voltage mode.
*
* @param iforwardSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
* @param ithreshold deadband on joystick values
*/
void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) override;
/**
* Drive the robot with a curvature drive layout. The robot drives in constant radius turns
* where you control the curvature (inverse of radius) you drive in. This is advantageous
* because the forward speed will not affect the rate of turning. The algorithm switches to
* arcade if the forward speed is 0. Uses voltage mode.
*
* @param iforwardSpeed speed in the forward direction
* @param icurvature curvature (inverse of radius) to drive in
* @param ithreshold deadband on joystick values
*/
void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) override;
/**
* Power the left side motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
void left(double ispeed) override;
/**
* Power the right side motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
void right(double ispeed) override;
/**
* Read the sensors.
*
* @return sensor readings in the format {left, right}
*/
std::valarray<std::int32_t> getSensorVals() const override;
/**
* Reset the sensors to their zero point.
*/
void resetSensors() override;
/**
* Set the brake mode for each motor.
*
* @param mode new brake mode
*/
void setBrakeMode(AbstractMotor::brakeMode mode) override;
/**
* Set the encoder units for each motor.
*
* @param units new motor encoder units
*/
void setEncoderUnits(AbstractMotor::encoderUnits units) override;
/**
* Set the gearset for each motor.
*
* @param gearset new motor gearset
*/
void setGearing(AbstractMotor::gearset gearset) override;
/**
* Sets a new maximum velocity in RPM. The usable maximum depends on the maximum velocity of the
* currently installed gearset. If the configured maximum velocity is greater than the attainable
* maximum velocity from the currently installed gearset, the ChassisModel will still scale to
* that velocity.
*
* @param imaxVelocity The new maximum velocity.
*/
void setMaxVelocity(double imaxVelocity) override;
/**
* @return The current maximum velocity.
*/
double getMaxVelocity() const override;
/**
* Sets a new maximum voltage in mV in the range `[0-12000]`.
*
* @param imaxVoltage The new maximum voltage.
*/
void setMaxVoltage(double imaxVoltage) override;
/**
* @return The maximum voltage in mV in the range `[0-12000]`.
*/
double getMaxVoltage() const override;
/**
* Returns the left side motor.
*
* @return the left side motor
*/
std::shared_ptr<AbstractMotor> getLeftSideMotor() const;
/**
* Returns the left side motor.
*
* @return the left side motor
*/
std::shared_ptr<AbstractMotor> getRightSideMotor() const;
protected:
double maxVelocity;
double maxVoltage;
std::shared_ptr<AbstractMotor> leftSideMotor;
std::shared_ptr<AbstractMotor> rightSideMotor;
std::shared_ptr<ContinuousRotarySensor> leftSensor;
std::shared_ptr<ContinuousRotarySensor> rightSensor;
};
} // namespace okapi

46
SerialTest/include/okapi/api/chassis/model/threeEncoderSkidSteerModel.hpp Normal file
View File

@ -0,0 +1,46 @@
/*
* 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/model/skidSteerModel.hpp"
namespace okapi {
class ThreeEncoderSkidSteerModel : public SkidSteerModel {
public:
/**
* Model for a skid steer drive (wheels parallel with robot's direction of motion). When all
* motors are powered +127, the robot should move forward in a straight line.
*
* @param ileftSideMotor left side motor
* @param irightSideMotor right side motor
* @param ileftEnc left side encoder
* @param imiddleEnc middle encoder (mounted perpendicular to the left and right side encoders)
* @param irightEnc right side encoder
*/
ThreeEncoderSkidSteerModel(std::shared_ptr<AbstractMotor> ileftSideMotor,
std::shared_ptr<AbstractMotor> irightSideMotor,
std::shared_ptr<ContinuousRotarySensor> ileftEnc,
std::shared_ptr<ContinuousRotarySensor> irightEnc,
std::shared_ptr<ContinuousRotarySensor> imiddleEnc,
double imaxVelocity,
double imaxVoltage);
/**
* Read the sensors.
*
* @return sensor readings in the format {left, right, middle}
*/
std::valarray<std::int32_t> getSensorVals() const override;
/**
* Reset the sensors to their zero point.
*/
void resetSensors() override;
protected:
std::shared_ptr<ContinuousRotarySensor> middleSensor;
};
} // namespace okapi

50
SerialTest/include/okapi/api/chassis/model/threeEncoderXDriveModel.hpp Normal file
View File

@ -0,0 +1,50 @@
/*
* 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/model/xDriveModel.hpp"
namespace okapi {
class ThreeEncoderXDriveModel : public XDriveModel {
public:
/**
* Model for an x drive (wheels at 45 deg from a skid steer drive). When all motors are powered
* +100%, the robot should move forward in a straight line.
*
* @param itopLeftMotor The top left motor.
* @param itopRightMotor The top right motor.
* @param ibottomRightMotor The bottom right motor.
* @param ibottomLeftMotor The bottom left motor.
* @param ileftEnc The left side encoder.
* @param irightEnc The right side encoder.
* @param imiddleEnc The middle encoder.
*/
ThreeEncoderXDriveModel(std::shared_ptr<AbstractMotor> itopLeftMotor,
std::shared_ptr<AbstractMotor> itopRightMotor,
std::shared_ptr<AbstractMotor> ibottomRightMotor,
std::shared_ptr<AbstractMotor> ibottomLeftMotor,
std::shared_ptr<ContinuousRotarySensor> ileftEnc,
std::shared_ptr<ContinuousRotarySensor> irightEnc,
std::shared_ptr<ContinuousRotarySensor> imiddleEnc,
double imaxVelocity,
double imaxVoltage);
/**
* Read the sensors.
*
* @return sensor readings in the format {left, right, middle}
*/
std::valarray<std::int32_t> getSensorVals() const override;
/**
* Reset the sensors to their zero point.
*/
void resetSensors() override;
protected:
std::shared_ptr<ContinuousRotarySensor> middleSensor;
};
} // namespace okapi

273
SerialTest/include/okapi/api/chassis/model/xDriveModel.hpp Normal file
View File

@ -0,0 +1,273 @@
/*
* 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/model/chassisModel.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/device/rotarysensor/continuousRotarySensor.hpp"
#include "okapi/api/units/QAngle.hpp"
namespace okapi {
class XDriveModel : public ChassisModel {
public:
/**
* Model for an x drive (wheels at 45 deg from a skid steer drive). When all motors are powered
* +100%, the robot should move forward in a straight line.
*
* @param itopLeftMotor The top left motor.
* @param itopRightMotor The top right motor.
* @param ibottomRightMotor The bottom right motor.
* @param ibottomLeftMotor The bottom left motor.
* @param ileftEnc The left side encoder.
* @param irightEnc The right side encoder.
*/
XDriveModel(std::shared_ptr<AbstractMotor> itopLeftMotor,
std::shared_ptr<AbstractMotor> itopRightMotor,
std::shared_ptr<AbstractMotor> ibottomRightMotor,
std::shared_ptr<AbstractMotor> ibottomLeftMotor,
std::shared_ptr<ContinuousRotarySensor> ileftEnc,
std::shared_ptr<ContinuousRotarySensor> irightEnc,
double imaxVelocity,
double imaxVoltage);
/**
* Drive the robot forwards (using open-loop control). Uses velocity mode.
*
* @param ispeed motor power
*/
void forward(double ipower) override;
/**
* Drive the robot in an arc (using open-loop control). Uses velocity mode.
* The algorithm is (approximately):
* leftPower = forwardSpeed + yaw
* rightPower = forwardSpeed - yaw
*
* @param iforwardSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
*/
void driveVector(double iforwardSpeed, double iyaw) override;
/**
* Drive the robot in an arc. Uses voltage mode.
* The algorithm is (approximately):
* leftPower = forwardSpeed + yaw
* rightPower = forwardSpeed - yaw
*
* @param iforwadSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
*/
void driveVectorVoltage(double iforwardSpeed, double iyaw) override;
/**
* Turn the robot clockwise (using open-loop control). Uses velocity mode.
*
* @param ipower motor power
*/
void rotate(double ipower) override;
/**
* Drive the robot side-ways (using open-loop control) where positive ipower is
* to the right and negative ipower is to the left. Uses velocity mode.
*
* @param ispeed motor power
*/
void strafe(double ipower);
/**
* Strafe the robot in an arc (using open-loop control) where positive istrafeSpeed is
* to the right and negative istrafeSpeed is to the left. Uses velocity mode.
* The algorithm is (approximately):
* topLeftPower = -1 * istrafeSpeed + yaw
* topRightPower = istrafeSpeed - yaw
* bottomRightPower = -1 * istrafeSpeed - yaw
* bottomLeftPower = istrafeSpeed + yaw
*
* @param istrafeSpeed speed to the right
* @param iyaw speed around the vertical axis
*/
void strafeVector(double istrafeSpeed, double iyaw);
/**
* Stop the robot (set all the motors to 0). Uses velocity mode.
*/
void stop() override;
/**
* Drive the robot with a tank drive layout. Uses voltage mode.
*
* @param ileftSpeed left side speed
* @param irightSpeed right side speed
* @param ithreshold deadband on joystick values
*/
void tank(double ileftSpeed, double irightSpeed, double ithreshold = 0) override;
/**
* Drive the robot with an arcade drive layout. Uses voltage mode.
*
* @param iforwardSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
* @param ithreshold deadband on joystick values
*/
void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) override;
/**
* Drive the robot with a curvature drive layout. The robot drives in constant radius turns
* where you control the curvature (inverse of radius) you drive in. This is advantageous
* because the forward speed will not affect the rate of turning. The algorithm switches to
* arcade if the forward speed is 0. Uses voltage mode.
*
* @param iforwardSpeed speed forward direction
* @param icurvature curvature (inverse of radius) to drive in
* @param ithreshold deadband on joystick values
*/
void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) override;
/**
* Drive the robot with an arcade drive layout. Uses voltage mode.
*
* @param irightSpeed speed to the right
* @param iforwardSpeed speed in the forward direction
* @param iyaw speed around the vertical axis
* @param ithreshold deadband on joystick values
*/
virtual void
xArcade(double irightSpeed, double iforwardSpeed, double iyaw, double ithreshold = 0);
/**
* Drive the robot with a field-oriented arcade drive layout. Uses voltage mode.
* For example:
* Both `fieldOrientedXArcade(1, 0, 0, 0_deg)` and `fieldOrientedXArcade(1, 0, 0, 90_deg)`
* will drive the chassis in the forward/north direction. In other words, no matter
* the robot's heading, the robot will move forward/north when you tell it
* to move forward/north and will move right/east when you tell it to move right/east.
*
*
* @param ixSpeed forward speed -- (`+1`) forward, (`-1`) backward
* @param iySpeed sideways speed -- (`+1`) right, (`-1`) left
* @param iyaw turn speed -- (`+1`) clockwise, (`-1`) counter-clockwise
* @param iangle current chassis angle (`0_deg` = no correction, winds clockwise)
* @param ithreshold deadband on joystick values
*/
virtual void fieldOrientedXArcade(double ixSpeed,
double iySpeed,
double iyaw,
QAngle iangle,
double ithreshold = 0);
/**
* Power the left side motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
void left(double ispeed) override;
/**
* Power the right side motors. Uses velocity mode.
*
* @param ispeed The motor power.
*/
void right(double ispeed) override;
/**
* Read the sensors.
*
* @return sensor readings in the format {left, right}
*/
std::valarray<std::int32_t> getSensorVals() const override;
/**
* Reset the sensors to their zero point.
*/
void resetSensors() override;
/**
* Set the brake mode for each motor.
*
* @param mode new brake mode
*/
void setBrakeMode(AbstractMotor::brakeMode mode) override;
/**
* Set the encoder units for each motor.
*
* @param units new motor encoder units
*/
void setEncoderUnits(AbstractMotor::encoderUnits units) override;
/**
* Set the gearset for each motor.
*
* @param gearset new motor gearset
*/
void setGearing(AbstractMotor::gearset gearset) override;
/**
* Sets a new maximum velocity in RPM. The usable maximum depends on the maximum velocity of the
* currently installed gearset. If the configured maximum velocity is greater than the attainable
* maximum velocity from the currently installed gearset, the ChassisModel will still scale to
* that velocity.
*
* @param imaxVelocity The new maximum velocity.
*/
void setMaxVelocity(double imaxVelocity) override;
/**
* @return The current maximum velocity.
*/
double getMaxVelocity() const override;
/**
* Sets a new maximum voltage in mV in the range `[0-12000]`.
*
* @param imaxVoltage The new maximum voltage.
*/
void setMaxVoltage(double imaxVoltage) override;
/**
* @return The maximum voltage in mV in the range `[0-12000]`.
*/
double getMaxVoltage() const override;
/**
* Returns the top left motor.
*
* @return the top left motor
*/
std::shared_ptr<AbstractMotor> getTopLeftMotor() const;
/**
* Returns the top right motor.
*
* @return the top right motor
*/
std::shared_ptr<AbstractMotor> getTopRightMotor() const;
/**
* Returns the bottom right motor.
*
* @return the bottom right motor
*/
std::shared_ptr<AbstractMotor> getBottomRightMotor() const;
/**
* Returns the bottom left motor.
*
* @return the bottom left motor
*/
std::shared_ptr<AbstractMotor> getBottomLeftMotor() const;
protected:
double maxVelocity;
double maxVoltage;
std::shared_ptr<AbstractMotor> topLeftMotor;
std::shared_ptr<AbstractMotor> topRightMotor;
std::shared_ptr<AbstractMotor> bottomRightMotor;
std::shared_ptr<AbstractMotor> bottomLeftMotor;
std::shared_ptr<ContinuousRotarySensor> leftSensor;
std::shared_ptr<ContinuousRotarySensor> rightSensor;
};
} // namespace okapi

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

131
SerialTest/include/okapi/api/coreProsAPI.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 <cmath>
#include <cstdbool>
#include <cstddef>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <sstream>
#ifdef THREADS_STD
#include <thread>
#define CROSSPLATFORM_THREAD_T std::thread
#include <mutex>
#define CROSSPLATFORM_MUTEX_T std::mutex
#else
#include "api.h"
#include "pros/apix.h"
#define CROSSPLATFORM_THREAD_T pros::task_t
#define CROSSPLATFORM_MUTEX_T pros::Mutex
#endif
#define NOT_INITIALIZE_TASK \
(strcmp(pros::c::task_get_name(pros::c::task_get_current()), "User Initialization (PROS)") != 0)
#define NOT_COMP_INITIALIZE_TASK \
(strcmp(pros::c::task_get_name(pros::c::task_get_current()), "User Comp. Init. (PROS)") != 0)
class CrossplatformThread {
public:
#ifdef THREADS_STD
CrossplatformThread(void (*ptr)(void *),
void *params,
const char *const = "OkapiLibCrossplatformTask")
#else
CrossplatformThread(void (*ptr)(void *),
void *params,
const char *const name = "OkapiLibCrossplatformTask")
#endif
:
#ifdef THREADS_STD
thread(ptr, params)
#else
thread(
pros::c::task_create(ptr, params, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, name))
#endif
{
}
~CrossplatformThread() {
#ifdef THREADS_STD
thread.join();
#else
if (pros::c::task_get_state(thread) != pros::E_TASK_STATE_DELETED) {
pros::c::task_delete(thread);
}
#endif
}
#ifdef THREADS_STD
void notifyWhenDeleting(CrossplatformThread *) {
}
#else
void notifyWhenDeleting(CrossplatformThread *parent) {
pros::c::task_notify_when_deleting(parent->thread, thread, 1, pros::E_NOTIFY_ACTION_INCR);
}
#endif
#ifdef THREADS_STD
void notifyWhenDeletingRaw(CROSSPLATFORM_THREAD_T *) {
}
#else
void notifyWhenDeletingRaw(CROSSPLATFORM_THREAD_T parent) {
pros::c::task_notify_when_deleting(parent, thread, 1, pros::E_NOTIFY_ACTION_INCR);
}
#endif
#ifdef THREADS_STD
std::uint32_t notifyTake(const std::uint32_t) {
return 0;
}
#else
std::uint32_t notifyTake(const std::uint32_t itimeout) {
return pros::c::task_notify_take(true, itimeout);
}
#endif
static std::string getName() {
#ifdef THREADS_STD
std::ostringstream ss;
ss << std::this_thread::get_id();
return ss.str();
#else
return std::string(pros::c::task_get_name(NULL));
#endif
}
CROSSPLATFORM_THREAD_T thread;
};
class CrossplatformMutex {
public:
CrossplatformMutex() = default;
void lock() {
#ifdef THREADS_STD
mutex.lock();
#else
while (!mutex.take(1)) {
}
#endif
}
void unlock() {
#ifdef THREADS_STD
mutex.unlock();
#else
mutex.give();
#endif
}
protected:
CROSSPLATFORM_MUTEX_T mutex;
};

46
SerialTest/include/okapi/api/device/button/abstractButton.hpp Normal file
View File

@ -0,0 +1,46 @@
/*
* 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"
namespace okapi {
class AbstractButton : public ControllerInput<bool> {
public:
virtual ~AbstractButton();
/**
* Return whether the button is currently pressed.
**/
virtual bool isPressed() = 0;
/**
* Return whether the state of the button changed since the last time this method was
* called.
**/
virtual bool changed() = 0;
/**
* Return whether the state of the button changed to being pressed since the last time this method
* was called.
**/
virtual bool changedToPressed() = 0;
/**
* Return whether the state of the button to being not pressed changed since the last time this
* method was called.
**/
virtual bool changedToReleased() = 0;
/**
* 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. This is the same as the output of the pressed() method.
*/
virtual bool controllerGet() override;
};
} // namespace okapi

52
SerialTest/include/okapi/api/device/button/buttonBase.hpp Normal file
View File

@ -0,0 +1,52 @@
/*
* 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/device/button/abstractButton.hpp"
namespace okapi {
class ButtonBase : public AbstractButton {
public:
/**
* @param iinverted Whether the button is inverted (`true` meaning default pressed and `false`
* meaning default not pressed).
*/
explicit ButtonBase(bool iinverted = false);
/**
* Return whether the button is currently pressed.
**/
bool isPressed() override;
/**
* Return whether the state of the button changed since the last time this method was called.
**/
bool changed() override;
/**
* Return whether the state of the button changed to pressed since the last time this method was
*called.
**/
bool changedToPressed() override;
/**
* Return whether the state of the button to not pressed since the last time this method was
*called.
**/
bool changedToReleased() override;
protected:
bool inverted{false};
bool wasPressedLast_c{false};
bool wasPressedLast_ctp{false};
bool wasPressedLast_ctr{false};
virtual bool currentlyPressed() = 0;
private:
bool changedImpl(bool &prevState);
};
} // namespace okapi

537
SerialTest/include/okapi/api/device/motor/abstractMotor.hpp Normal file
View File

@ -0,0 +1,537 @@
/*
* 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/device/rotarysensor/continuousRotarySensor.hpp"
#include <memory>
namespace okapi {
class AbstractMotor : public ControllerOutput<double> {
public:
/**
* Indicates the 'brake mode' of a motor.
*/
enum class brakeMode {
coast = 0, ///< Motor coasts when stopped, traditional behavior
brake = 1, ///< Motor brakes when stopped
hold = 2, ///< Motor actively holds position when stopped
invalid = INT32_MAX
};
/**
* Indicates the units used by the motor encoders.
*/
enum class encoderUnits {
degrees = 0, ///< degrees
rotations = 1, ///< rotations
counts = 2, ///< counts
invalid = INT32_MAX ///< invalid
};
/**
* Indicates the internal gear ratio of a motor.
*/
enum class gearset {
red = 100, ///< 36:1, 100 RPM, Red gear set
green = 200, ///< 18:1, 200 RPM, Green gear set
blue = 600, ///< 6:1, 600 RPM, Blue gear set
invalid = INT32_MAX
};
/**
* A simple structure representing the full ratio between motor and wheel.
*/
struct GearsetRatioPair {
/**
* A simple structure representing the full ratio between motor and wheel.
*
* The ratio is `motor rotation : wheel rotation`, e.x., if one motor rotation
* corresponds to two wheel rotations, the ratio is `1.0/2.0`.
*
* @param igearset The gearset in the motor.
* @param iratio The ratio of motor rotation to wheel rotation.
*/
GearsetRatioPair(const gearset igearset, const double iratio = 1)
: internalGearset(igearset), ratio(iratio) {
}
~GearsetRatioPair() = default;
gearset internalGearset;
double ratio = 1;
};
virtual ~AbstractMotor();
/******************************************************************************/
/** Motor movement functions **/
/** **/
/** These functions allow programmers to make motors move **/
/******************************************************************************/
/**
* Sets the target absolute position for the motor to move to.
*
* This movement is relative to the position of the motor when initialized or
* the position when it was most recently reset with setZeroPosition().
*
* @note This function simply sets the target for the motor, it does not block program execution
* until the movement finishes.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @param iposition The absolute position to move to in the motor's encoder units
* @param ivelocity The maximum allowable velocity for the movement in RPM
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t moveAbsolute(double iposition, std::int32_t ivelocity) = 0;
/**
* Sets the relative target position for the motor to move to.
*
* This movement is relative to the current position of the motor. Providing 10.0 as the position
* parameter would result in the motor moving clockwise 10 units, no matter what the current
* position is.
*
* @note This function simply sets the target for the motor, it does not block program execution
* until the movement finishes.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @param iposition The relative position to move to in the motor's encoder units
* @param ivelocity The maximum allowable velocity for the movement in RPM
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t moveRelative(double iposition, std::int32_t ivelocity) = 0;
/**
* Sets the velocity for the motor.
*
* This velocity corresponds to different actual speeds depending on the gearset
* used for the motor. This results in a range of +-100 for pros::c::red,
* +-200 for green, and +-600 for blue. The velocity
* is held with PID to ensure consistent speed, as opposed to setting the motor's
* voltage.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @param ivelocity The new motor velocity from -+-100, +-200, or +-600 depending on the motor's
* gearset
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t moveVelocity(std::int16_t ivelocity) = 0;
/**
* Sets the voltage for the motor from -12000 to 12000.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @param ivoltage The new voltage value from -12000 to 12000
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t moveVoltage(std::int16_t ivoltage) = 0;
/**
* Changes the output velocity for a profiled movement (moveAbsolute or moveRelative). This will
* have no effect if the motor is not following a profiled movement.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @param ivelocity The new motor velocity from -+-100, +-200, or +-600 depending on the motor's
* gearset
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t modifyProfiledVelocity(std::int32_t ivelocity) = 0;
/******************************************************************************/
/** Motor telemetry functions **/
/** **/
/** These functions allow programmers to collect telemetry from motors **/
/******************************************************************************/
/**
* Gets the target position set for the motor by the user.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The target position in its encoder units or PROS_ERR_F if the operation failed,
* setting errno.
*/
virtual double getTargetPosition() = 0;
/**
* Gets the absolute position of the motor in its encoder units.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The motor's absolute position in its encoder units or PROS_ERR_F if the operation
* failed, setting errno.
*/
virtual double getPosition() = 0;
/**
* Gets the positional error (target position minus actual position) of the motor in its encoder
* units.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The motor's positional error in its encoder units or PROS_ERR_F if the operation
* failed, setting errno.
*/
double getPositionError();
/**
* Sets the "absolute" zero position of the motor to its current position.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t tarePosition() = 0;
/**
* Gets the velocity commanded to the motor by the user.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The commanded motor velocity from +-100, +-200, or +-600, or PROS_ERR if the operation
* failed, setting errno.
*/
virtual std::int32_t getTargetVelocity() = 0;
/**
* Gets the actual velocity of the motor.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The motor's actual velocity in RPM or PROS_ERR_F if the operation failed, setting
* errno.
*/
virtual double getActualVelocity() = 0;
/**
* Gets the difference between the target velocity of the motor and the actual velocity of the
* motor.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The motor's velocity error in RPM or PROS_ERR_F if the operation failed, setting
* errno.
*/
double getVelocityError();
/**
* Gets the current drawn by the motor in mA.
*
* This function uses the following values of errno when an error state is
* reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The motor's current in mA or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t getCurrentDraw() = 0;
/**
* Gets the direction of movement for the motor.
*
* This function uses the following values of errno when an error state is
* reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return 1 for moving in the positive direction, -1 for moving in the negative direction, and
* PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t getDirection() = 0;
/**
* Gets the efficiency of the motor in percent.
*
* An efficiency of 100% means that the motor is moving electrically while
* drawing no electrical power, and an efficiency of 0% means that the motor
* is drawing power but not moving.
*
* This function uses the following values of errno when an error state is
* reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The motor's efficiency in percent or PROS_ERR_F if the operation failed, setting errno.
*/
virtual double getEfficiency() = 0;
/**
* Checks if the motor is drawing over its current limit.
*
* This function uses the following values of errno when an error state is
* reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return 1 if the motor's current limit is being exceeded and 0 if the current limit is not
* exceeded, or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t isOverCurrent() = 0;
/**
* Checks if the motor's temperature is above its limit.
*
* This function uses the following values of errno when an error state is
* reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return 1 if the temperature limit is exceeded and 0 if the the temperature is below the limit,
* or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t isOverTemp() = 0;
/**
* Checks if the motor is stopped.
*
* Although this function forwards data from the motor, the motor presently does not provide any
* value. This function returns PROS_ERR with errno set to ENOSYS.
*
* @return 1 if the motor is not moving, 0 if the motor is moving, or PROS_ERR if the operation
* failed, setting errno
*/
virtual std::int32_t isStopped() = 0;
/**
* Checks if the motor is at its zero position.
*
* Although this function forwards data from the motor, the motor presently does not provide any
* value. This function returns PROS_ERR with errno set to ENOSYS.
*
* @return 1 if the motor is at zero absolute position, 0 if the motor has moved from its absolute
* zero, or PROS_ERR if the operation failed, setting errno
*/
virtual std::int32_t getZeroPositionFlag() = 0;
/**
* Gets the faults experienced by the motor. Compare this bitfield to the bitmasks in
* pros::motor_fault_e_t.
*
* This function uses the following values of errno when an error state is
* reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return A currently unknown bitfield containing the motor's faults. 0b00000100 = Current Limit
* Hit
*/
virtual uint32_t getFaults() = 0;
/**
* Gets the flags set by the motor's operation. Compare this bitfield to the bitmasks in
* pros::motor_flag_e_t.
*
* This function uses the following values of errno when an error state is
* reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return A currently unknown bitfield containing the motor's flags. These seem to be unrelated
* to the individual get_specific_flag functions
*/
virtual uint32_t getFlags() = 0;
/**
* Gets the raw encoder count of the motor at a given timestamp.
*
* This function uses the following values of errno when an error state is
* reached:
* EACCES - Another resource is currently trying to access the port.
*
* @param timestamp A pointer to a time in milliseconds for which the encoder count will be
* returned. If NULL, the timestamp at which the encoder count was read will not be supplied
*
* @return The raw encoder count at the given timestamp or PROS_ERR if the operation failed.
*/
virtual std::int32_t getRawPosition(std::uint32_t *timestamp) = 0;
/**
* Gets the power drawn by the motor in Watts.
*
* This function uses the following values of errno when an error state is
* reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The motor's power draw in Watts or PROS_ERR_F if the operation failed, setting errno.
*/
virtual double getPower() = 0;
/**
* Gets the temperature of the motor in degrees Celsius.
*
* This function uses the following values of errno when an error state is
* reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The motor's temperature in degrees Celsius or PROS_ERR_F if the operation failed,
* setting errno.
*/
virtual double getTemperature() = 0;
/**
* Gets the torque generated by the motor in Newton Metres (Nm).
*
* This function uses the following values of errno when an error state is
* reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The motor's torque in NM or PROS_ERR_F if the operation failed, setting errno.
*/
virtual double getTorque() = 0;
/**
* Gets the voltage delivered to the motor in millivolts.
*
* This function uses the following values of errno when an error state is
* reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The motor's voltage in V or PROS_ERR_F if the operation failed, setting errno.
*/
virtual std::int32_t getVoltage() = 0;
/******************************************************************************/
/** Motor configuration functions **/
/** **/
/** These functions allow programmers to configure the behavior of motors **/
/******************************************************************************/
/**
* Sets one of brakeMode to the motor.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @param imode The new motor brake mode to set for the motor
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t setBrakeMode(brakeMode imode) = 0;
/**
* Gets the brake mode that was set for the motor.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return One of brakeMode, according to what was set for the motor, or brakeMode::invalid if the
* operation failed, setting errno.
*/
virtual brakeMode getBrakeMode() = 0;
/**
* Sets the current limit for the motor in mA.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @param ilimit The new current limit in mA
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t setCurrentLimit(std::int32_t ilimit) = 0;
/**
* Gets the current limit for the motor in mA.
*
* The default value is 2500 mA.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return The motor's current limit in mA or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t getCurrentLimit() = 0;
/**
* Sets one of encoderUnits for the motor encoder.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @param iunits The new motor encoder units
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t setEncoderUnits(encoderUnits iunits) = 0;
/**
* Gets the encoder units that were set for the motor.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return One of encoderUnits according to what is set for the motor or encoderUnits::invalid if
* the operation failed.
*/
virtual encoderUnits getEncoderUnits() = 0;
/**
* Sets one of gearset for the motor.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @param igearset The new motor gearset
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t setGearing(gearset igearset) = 0;
/**
* Gets the gearset that was set for the motor.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @return One of gearset according to what is set for the motor, or gearset::invalid if the
* operation failed.
*/
virtual gearset getGearing() = 0;
/**
* Sets the reverse flag for the motor.
*
* This will invert its movements and the values returned for its position.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @param ireverse True reverses the motor, false is default
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t setReversed(bool ireverse) = 0;
/**
* Sets the voltage limit for the motor in Volts.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the port.
*
* @param ilimit The new voltage limit in Volts
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t setVoltageLimit(std::int32_t ilimit) = 0;
/**
* Returns the encoder associated with this motor.
*
* @return the encoder for this motor
*/
virtual std::shared_ptr<ContinuousRotarySensor> getEncoder() = 0;
};
AbstractMotor::GearsetRatioPair operator*(AbstractMotor::gearset gearset, double ratio);
} // namespace okapi

20
SerialTest/include/okapi/api/device/rotarysensor/continuousRotarySensor.hpp Normal file
View File

@ -0,0 +1,20 @@
/*
* 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/device/rotarysensor/rotarySensor.hpp"
namespace okapi {
class ContinuousRotarySensor : public RotarySensor {
public:
/**
* Reset the sensor to zero.
*
* @return `1` on success, `PROS_ERR` on fail
*/
virtual std::int32_t reset() = 0;
};
} // namespace okapi

23
SerialTest/include/okapi/api/device/rotarysensor/rotarySensor.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/control/controllerInput.hpp"
#include "okapi/api/coreProsAPI.hpp"
namespace okapi {
class RotarySensor : public ControllerInput<double> {
public:
virtual ~RotarySensor();
/**
* Get the current sensor value.
*
* @return the current sensor value, or `PROS_ERR` on a failure.
*/
virtual double get() const = 0;
};
} // namespace okapi

59
SerialTest/include/okapi/api/filter/averageFilter.hpp Normal file
View File

@ -0,0 +1,59 @@
/*
* 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/filter/filter.hpp"
#include <array>
#include <cstddef>
namespace okapi {
/**
* A filter which returns the average of a list of values.
*
* @tparam n number of taps in the filter
*/
template <std::size_t n> class AverageFilter : public Filter {
public:
/**
* Averaging filter.
*/
AverageFilter() = default;
/**
* Filters a value, like a sensor reading.
*
* @param ireading new measurement
* @return filtered result
*/
double filter(const double ireading) override {
data[index++] = ireading;
if (index >= n) {
index = 0;
}
output = 0.0;
for (size_t i = 0; i < n; i++)
output += data[i];
output /= (double)n;
return output;
}
/**
* Returns the previous output from filter.
*
* @return the previous output from filter
*/
double getOutput() const override {
return output;
}
protected:
std::array<double, n> data{0};
std::size_t index = 0;
double output = 0;
};
} // namespace okapi

50
SerialTest/include/okapi/api/filter/composableFilter.hpp Normal file
View File

@ -0,0 +1,50 @@
/*
* 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/filter/filter.hpp"
#include <functional>
#include <initializer_list>
#include <memory>
#include <vector>
namespace okapi {
class ComposableFilter : public Filter {
public:
/**
* A composable filter is a filter that consists of other filters. The input signal is passed
* through each filter in sequence. The final output of this filter is the output of the last
* filter.
*
* @param ilist The filters to use in sequence.
*/
ComposableFilter(const std::initializer_list<std::shared_ptr<Filter>> &ilist);
/**
* Filters a value.
*
* @param ireading A new measurement.
* @return The filtered result.
*/
double filter(double ireading) override;
/**
* @return The previous output from filter.
*/
double getOutput() const override;
/**
* Adds a filter to the end of the sequence.
*
* @param ifilter The filter to add.
*/
virtual void addFilter(std::shared_ptr<Filter> ifilter);
protected:
std::vector<std::shared_ptr<Filter>> filters;
double output = 0;
};
} // namespace okapi

52
SerialTest/include/okapi/api/filter/demaFilter.hpp Normal file
View File

@ -0,0 +1,52 @@
/*
* 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/filter/filter.hpp"
#include <ratio>
namespace okapi {
class DemaFilter : public Filter {
public:
/**
* Double exponential moving average filter.
*
* @param ialpha alpha gain
* @param ibeta beta gain
*/
DemaFilter(double ialpha, double ibeta);
/**
* Filters a value, like a sensor reading.
*
* @param reading new measurement
* @return filtered result
*/
double filter(double ireading) override;
/**
* Returns the previous output from filter.
*
* @return the previous output from filter
*/
double getOutput() const override;
/**
* Set filter gains.
*
* @param ialpha alpha gain
* @param ibeta beta gain
*/
virtual void setGains(double ialpha, double ibeta);
protected:
double alpha, beta;
double outputS = 0;
double lastOutputS = 0;
double outputB = 0;
double lastOutputB = 0;
};
} // namespace okapi

71
SerialTest/include/okapi/api/filter/ekfFilter.hpp Normal file
View File

@ -0,0 +1,71 @@
/*
* 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/filter/filter.hpp"
#include "okapi/api/util/mathUtil.hpp"
namespace okapi {
class EKFFilter : public Filter {
public:
/**
* One dimensional extended Kalman filter. The default arguments should work fine for most signal
* filtering. It won't hurt to graph your signal and the filtered result, and check if the filter
* is doing its job.
*
* Q is the covariance of the process noise and R is the
* covariance of the observation noise. The default values for Q and R should be a modest balance
* between trust in the sensor and FIR filtering.
*
* Think of R as how noisy your sensor is. Its value can be found mathematically by computing the
* standard deviation of your sensor reading vs. "truth" (of course, "truth" is still an estimate;
* try to calibrate your robot in a controlled setting where you can minimize the error in what
* "truth" is).
*
* Think of Q as how noisy your model is. It decides how much "smoothing" the filter does and how
* far it lags behind the true signal. This parameter is most often used as a "tuning" parameter
* to adjust the response of the filter.
*
* @param iQ process noise covariance
* @param iR measurement noise covariance
*/
explicit EKFFilter(double iQ = 0.0001, double iR = ipow(0.2, 2));
/**
* Filters a value, like a sensor reading. Assumes the control input is zero.
*
* @param ireading new measurement
* @return filtered result
*/
double filter(double ireading) override;
/**
* Filters a reading with a control input.
*
* @param ireading new measurement
* @param icontrol control input
* @return filtered result
*/
virtual double filter(double ireading, double icontrol);
/**
* Returns the previous output from filter.
*
* @return the previous output from filter
*/
double getOutput() const override;
protected:
const double Q, R;
double xHat = 0;
double xHatPrev = 0;
double xHatMinus = 0;
double P = 0;
double Pprev = 1;
double Pminus = 0;
double K = 0;
};
} // namespace okapi

47
SerialTest/include/okapi/api/filter/emaFilter.hpp Normal file
View File

@ -0,0 +1,47 @@
/*
* 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/filter/filter.hpp"
namespace okapi {
class EmaFilter : public Filter {
public:
/**
* Exponential moving average filter.
*
* @param ialpha alpha gain
*/
explicit EmaFilter(double ialpha);
/**
* Filters a value, like a sensor reading.
*
* @param reading new measurement
* @return filtered result
*/
double filter(double ireading) override;
/**
* Returns the previous output from filter.
*
* @return the previous output from filter
*/
double getOutput() const override;
/**
* Set filter gains.
*
* @param ialpha alpha gain
*/
virtual void setGains(double ialpha);
protected:
double alpha;
double output = 0;
double lastOutput = 0;
};
} // namespace okapi

28
SerialTest/include/okapi/api/filter/filter.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
namespace okapi {
class Filter {
public:
virtual ~Filter();
/**
* Filters a value, like a sensor reading.
*
* @param ireading new measurement
* @return filtered result
*/
virtual double filter(double ireading) = 0;
/**
* Returns the previous output from filter.
*
* @return the previous output from filter
*/
virtual double getOutput() const = 0;
};
} // namespace okapi

48
SerialTest/include/okapi/api/filter/filteredControllerInput.hpp Normal file
View File

@ -0,0 +1,48 @@
/*
* 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/filter/filter.hpp"
#include <memory>
namespace okapi {
/**
* A ControllerInput with a filter built in.
*
* @tparam InputType the type of the ControllerInput
* @tparam FilterType the type of the Filter
*/
template <typename InputType, typename FilterType>
class FilteredControllerInput : public ControllerInput<double> {
public:
/**
* A filtered controller input. Applies a filter to the controller input. Useful if you want to
* place a filter between a control input and a control loop.
*
* @param iinput ControllerInput type
* @param ifilter Filter type
*/
FilteredControllerInput(std::unique_ptr<ControllerInput<InputType>> iinput,
std::unique_ptr<FilterType> ifilter)
: input(std::move(iinput)), filter(std::move(ifilter)) {
}
/**
* Gets the sensor value for use in a control loop. This method might be automatically called in
* another thread by the controller.
*
* @return the current filtered sensor value.
*/
double controllerGet() override {
return filter->filter(input->controllerGet());
}
protected:
std::unique_ptr<ControllerInput<InputType>> input;
std::unique_ptr<FilterType> filter;
};
} // namespace okapi

94
SerialTest/include/okapi/api/filter/medianFilter.hpp Normal file
View File

@ -0,0 +1,94 @@
/*
* Uses the median filter algorithm from N. Wirth’s book, implementation by N. Devillard.
*
* 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/filter/filter.hpp"
#include <algorithm>
#include <array>
#include <cstddef>
namespace okapi {
/**
* A filter which returns the median value of list of values.
*
* @tparam n number of taps in the filter
*/
template <std::size_t n> class MedianFilter : public Filter {
public:
MedianFilter() : middleIndex((((n)&1) ? ((n) / 2) : (((n) / 2) - 1))) {
}
/**
* Filters a value, like a sensor reading.
*
* @param ireading new measurement
* @return filtered result
*/
double filter(const double ireading) override {
data[index++] = ireading;
if (index >= n) {
index = 0;
}
output = kth_smallset();
return output;
}
/**
* Returns the previous output from filter.
*
* @return the previous output from filter
*/
double getOutput() const override {
return output;
}
protected:
std::array<double, n> data{0};
std::size_t index = 0;
double output = 0;
const size_t middleIndex;
/**
* Algorithm from N. Wirth’s book, implementation by N. Devillard.
*/
double kth_smallset() {
std::array<double, n> dataCopy = data;
size_t j, l, m;
l = 0;
m = n - 1;
while (l < m) {
double x = dataCopy[middleIndex];
size_t i = l;
j = m;
do {
while (dataCopy[i] < x) {
i++;
}
while (x < dataCopy[j]) {
j--;
}
if (i <= j) {
const double t = dataCopy[i];
dataCopy[i] = dataCopy[j];
dataCopy[j] = t;
i++;
j--;
}
} while (i <= j);
if (j < middleIndex)
l = i;
if (middleIndex < i)
m = j;
}
return dataCopy[middleIndex];
}
};
} // namespace okapi

36
SerialTest/include/okapi/api/filter/passthroughFilter.hpp Normal file
View File

@ -0,0 +1,36 @@
/*
* 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/filter/filter.hpp"
namespace okapi {
class PassthroughFilter : public Filter {
public:
/**
* A simple filter that does no filtering and just passes the input through.
*/
PassthroughFilter();
/**
* Filters a value, like a sensor reading.
*
* @param ireading new measurement
* @return filtered result
*/
double filter(double ireading) override;
/**
* Returns the previous output from filter.
*
* @return the previous output from filter
*/
double getOutput() const override;
protected:
double lastOutput = 0;
};
} // namespace okapi

74
SerialTest/include/okapi/api/filter/velMath.hpp Normal file
View File

@ -0,0 +1,74 @@
/*
* 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/filter/composableFilter.hpp"
#include "okapi/api/units/QAngularAcceleration.hpp"
#include "okapi/api/units/QAngularSpeed.hpp"
#include "okapi/api/units/QTime.hpp"
#include "okapi/api/util/abstractTimer.hpp"
#include "okapi/api/util/logging.hpp"
#include <memory>
namespace okapi {
class VelMath {
public:
/**
* Velocity math helper. Calculates filtered velocity. Throws a `std::invalid_argument` exception
* if `iticksPerRev` is zero.
*
* @param iticksPerRev The number of ticks per revolution (or whatever units you are using).
* @param ifilter The filter used for filtering the calculated velocity.
* @param isampleTime The minimum time between velocity measurements.
* @param ilogger The logger this instance will log to.
*/
VelMath(double iticksPerRev,
std::unique_ptr<Filter> ifilter,
QTime isampleTime,
std::unique_ptr<AbstractTimer> iloopDtTimer,
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
virtual ~VelMath();
/**
* Calculates the current velocity and acceleration. Returns the (filtered) velocity.
*
* @param inewPos The new position measurement.
* @return The new velocity estimate.
*/
virtual QAngularSpeed step(double inewPos);
/**
* Sets ticks per revolution (or whatever units you are using). Throws a `std::invalid_argument`
* exception if iticksPerRev is zero.
*
* @param iTPR The number of ticks per revolution.
*/
virtual void setTicksPerRev(double iTPR);
/**
* @return The last calculated velocity.
*/
virtual QAngularSpeed getVelocity() const;
/**
* @return The last calculated acceleration.
*/
virtual QAngularAcceleration getAccel() const;
protected:
std::shared_ptr<Logger> logger;
QAngularSpeed vel{0_rpm};
QAngularSpeed lastVel{0_rpm};
QAngularAcceleration accel{0.0};
double lastPos{0};
double ticksPerRev;
QTime sampleTime;
std::unique_ptr<AbstractTimer> loopDtTimer;
std::unique_ptr<Filter> filter;
};
} // namespace okapi

95
SerialTest/include/okapi/api/odometry/odomMath.hpp Normal file
View File

@ -0,0 +1,95 @@
/*
* 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/odometry/odomState.hpp"
#include "okapi/api/odometry/point.hpp"
#include "okapi/api/util/logging.hpp"
#include <tuple>
namespace okapi {
class OdomMath {
public:
/**
* Computes the distance from the given Odometry state to the given point. The point and the
* OdomState must be in `StateMode::FRAME_TRANSFORMATION`.
*
* @param ipoint The point.
* @param istate The Odometry state.
* @return The distance between the Odometry state and the point.
*/
static QLength computeDistanceToPoint(const Point &ipoint, const OdomState &istate);
/**
* Computes the angle from the given Odometry state to the given point. The point and the
* OdomState must be in `StateMode::FRAME_TRANSFORMATION`.
*
* @param ipoint The point.
* @param istate The Odometry state.
* @return The angle between the Odometry state and the point.
*/
static QAngle computeAngleToPoint(const Point &ipoint, const OdomState &istate);
/**
* Computes the distance and angle from the given Odometry state to the given point. The point and
* the OdomState must be in `StateMode::FRAME_TRANSFORMATION`.
*
* @param ipoint The point.
* @param istate The Odometry state.
* @return The distance and angle between the Odometry state and the point.
*/
static std::pair<QLength, QAngle> computeDistanceAndAngleToPoint(const Point &ipoint,
const OdomState &istate);
/**
* Constraints the angle to [0,360] degrees.
*
* @param angle The input angle.
* @return The angle normalized to [0,360] degrees.
*/
static QAngle constrainAngle360(const QAngle &angle);
/**
* Constraints the angle to [-180,180) degrees.
*
* @param angle The input angle.
* @return The angle normalized to [-180,180) degrees.
*/
static QAngle constrainAngle180(const QAngle &angle);
private:
OdomMath();
~OdomMath();
/**
* Computes the x and y diffs in meters between the points.
*
* @param ipoint The point.
* @param istate The Odometry state.
* @return The diffs in the order `{xDiff, yDiff}`.
*/
static std::pair<double, double> computeDiffs(const Point &ipoint, const OdomState &istate);
/**
* Computes the distance between the points.
*
* @param xDiff The x-axis diff in meters.
* @param yDiff The y-axis diff in meters.
* @return The cartesian distance in meters.
*/
static double computeDistance(double xDiff, double yDiff);
/**
* Compites the angle between the points.
*
* @param xDiff The x-axis diff in meters.
* @param yDiff The y-axis diff in meters.
* @param theta The current robot's theta in radians.
* @return The angle in radians.
*/
static double computeAngle(double xDiff, double yDiff, double theta);
};
} // namespace okapi

57
SerialTest/include/okapi/api/odometry/odomState.hpp Normal file
View File

@ -0,0 +1,57 @@
/*
* 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"
#include "okapi/api/units/RQuantityName.hpp"
#include <string>
namespace okapi {
struct OdomState {
QLength x{0_m};
QLength y{0_m};
QAngle theta{0_deg};
/**
* Get a string for the current odometry state (optionally with the specified units).
*
* Examples:
* - `OdomState::str(1_m, 1_deg)`: The default (no arguments specified).
* - `OdomState::str(1_tile, 1_radian)`: distance tiles and angle radians.
*
* Throws std::domain_error if the units passed are undefined.
*
* @param idistanceUnit The units you want your distance to be in. This must be an exact, predefined QLength (such as foot, meter, inch, tile etc.).
* @param iangleUnit The units you want your angle to be in. This must be an exact, predefined QAngle (degree or radian).
* @return A string representing the state.
*/
std::string str(QLength idistanceUnit, QAngle iangleUnit) const;
/**
* Get a string for the current odometry state (optionally with the specified units).
*
* Examples:
* - `OdomState::str(1_m, "_m", 1_deg, "_deg")`: The default (no arguments specified), prints in meters and degrees.
* - `OdomState::str(1_in, "_in", 1_deg, "_deg")` or `OdomState::str(1_in, "\"", 1_deg, "°")`: to print values in inches and degrees with different suffixes.
* - `OdomState::str(6_tile / 100, "%", 360_deg / 100, "%")` to get the distance values in % of the vex field, and angle values in % of a full rotation.
*
* @param idistanceUnit The units you want your distance to be in. The x or y position will be output in multiples of this length.
* @param distUnitName The suffix you as your distance unit.
* @param iangleUnit The units you want your angle to be in. The angle will be output in multiples of this unit.
* @param angleUnitName The suffix you want as your angle unit.
* @return A string representing the state.
*/
std::string str(QLength idistanceUnit = meter,
std::string distUnitName = "_m",
QAngle iangleUnit = degree,
std::string angleUnitName = "_deg") const;
bool operator==(const OdomState &rhs) const;
bool operator!=(const OdomState &rhs) const;
};
} // namespace okapi

61
SerialTest/include/okapi/api/odometry/odometry.hpp Normal file
View File

@ -0,0 +1,61 @@
/*
* 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/readOnlyChassisModel.hpp"
#include "okapi/api/odometry/odomState.hpp"
#include "okapi/api/odometry/stateMode.hpp"
namespace okapi {
class Odometry {
public:
/**
* Odometry. Tracks the movement of the robot and estimates its position in coordinates
* relative to the start (assumed to be (0, 0, 0)).
*/
explicit Odometry() = default;
virtual ~Odometry() = default;
/**
* Sets the drive and turn scales.
*/
virtual void setScales(const ChassisScales &ichassisScales) = 0;
/**
* Do one odometry step.
*/
virtual void step() = 0;
/**
* Returns the current state.
*
* @param imode The mode to return the state in.
* @return The current state in the given format.
*/
virtual OdomState getState(const StateMode &imode = StateMode::FRAME_TRANSFORMATION) const = 0;
/**
* Sets a new state to be the current state.
*
* @param istate The new state in the given format.
* @param imode The mode to treat the input state as.
*/
virtual void setState(const OdomState &istate,
const StateMode &imode = StateMode::FRAME_TRANSFORMATION) = 0;
/**
* @return The internal ChassisModel.
*/
virtual std::shared_ptr<ReadOnlyChassisModel> getModel() = 0;
/**
* @return The internal ChassisScales.
*/
virtual ChassisScales getScales() = 0;
};
} // namespace okapi

30
SerialTest/include/okapi/api/odometry/point.hpp Normal file
View File

@ -0,0 +1,30 @@
/*
* 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/odometry/stateMode.hpp"
#include "okapi/api/units/QLength.hpp"
namespace okapi {
struct Point {
QLength x{0_m};
QLength y{0_m};
/**
* Computes the value of this point in `StateMode::FRAME_TRANSFORMATION`.
*
* @param imode The StateMode this Point is currently specified in.
* @return This point specified in `StateMode::FRAME_TRANSFORMATION`.
*/
Point inFT(const StateMode &imode) const {
if (imode == StateMode::FRAME_TRANSFORMATION) {
return *this;
} else {
return {y, x};
}
}
};
} // namespace okapi

17
SerialTest/include/okapi/api/odometry/stateMode.hpp Normal file
View File

@ -0,0 +1,17 @@
/*
* 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 {
/**
* The mode for the OdomState calculated by Odometry.
*/
enum class StateMode {
FRAME_TRANSFORMATION, ///< +x is forward, +y is right, 0 degrees is along +x
CARTESIAN ///< +x is right, +y is forward, 0 degrees is along +y
};
} // namespace okapi

43
SerialTest/include/okapi/api/odometry/threeEncoderOdometry.hpp Normal file
View File

@ -0,0 +1,43 @@
/*
* 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/model/threeEncoderSkidSteerModel.hpp"
#include "okapi/api/odometry/twoEncoderOdometry.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <functional>
namespace okapi {
class ThreeEncoderOdometry : public TwoEncoderOdometry {
public:
/**
* Odometry. Tracks the movement of the robot and estimates its position in coordinates
* relative to the start (assumed to be (0, 0)).
*
* @param itimeUtil The TimeUtil.
* @param imodel The chassis model for reading sensors.
* @param ichassisScales See ChassisScales docs (the middle wheel scale is the third member)
* @param iwheelVelDelta The maximum delta between wheel velocities to consider the robot as
* driving straight.
* @param ilogger The logger this instance will log to.
*/
ThreeEncoderOdometry(const TimeUtil &itimeUtil,
const std::shared_ptr<ReadOnlyChassisModel> &imodel,
const ChassisScales &ichassisScales,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
protected:
/**
* Does the math, side-effect free, for one odom step.
*
* @param itickDiff The tick difference from the previous step to this step.
* @param ideltaT The time difference from the previous step to this step.
* @return The newly computed OdomState.
*/
OdomState odomMathStep(const std::valarray<std::int32_t> &itickDiff,
const QTime &ideltaT) override;
};
} // namespace okapi

93
SerialTest/include/okapi/api/odometry/twoEncoderOdometry.hpp Normal file
View File

@ -0,0 +1,93 @@
/*
* 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/odometry/odometry.hpp"
#include "okapi/api/units/QSpeed.hpp"
#include "okapi/api/util/abstractRate.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <atomic>
#include <memory>
#include <valarray>
namespace okapi {
class TwoEncoderOdometry : public Odometry {
public:
/**
* TwoEncoderOdometry. Tracks the movement of the robot and estimates its position in coordinates
* relative to the start (assumed to be (0, 0, 0)).
*
* @param itimeUtil The TimeUtil.
* @param imodel The chassis model for reading sensors.
* @param ichassisScales The chassis dimensions.
* @param ilogger The logger this instance will log to.
*/
TwoEncoderOdometry(const TimeUtil &itimeUtil,
const std::shared_ptr<ReadOnlyChassisModel> &imodel,
const ChassisScales &ichassisScales,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
virtual ~TwoEncoderOdometry() = default;
/**
* Sets the drive and turn scales.
*/
void setScales(const ChassisScales &ichassisScales) override;
/**
* Do one odometry step.
*/
void step() override;
/**
* Returns the current state.
*
* @param imode The mode to return the state in.
* @return The current state in the given format.
*/
OdomState getState(const StateMode &imode = StateMode::FRAME_TRANSFORMATION) const override;
/**
* Sets a new state to be the current state.
*
* @param istate The new state in the given format.
* @param imode The mode to treat the input state as.
*/
void setState(const OdomState &istate,
const StateMode &imode = StateMode::FRAME_TRANSFORMATION) override;
/**
* @return The internal ChassisModel.
*/
std::shared_ptr<ReadOnlyChassisModel> getModel() override;
/**
* @return The internal ChassisScales.
*/
ChassisScales getScales() override;
protected:
std::shared_ptr<Logger> logger;
std::unique_ptr<AbstractRate> rate;
std::unique_ptr<AbstractTimer> timer;
std::shared_ptr<ReadOnlyChassisModel> model;
ChassisScales chassisScales;
OdomState state;
std::valarray<std::int32_t> newTicks{0, 0, 0}, tickDiff{0, 0, 0}, lastTicks{0, 0, 0};
const std::int32_t maximumTickDiff{1000};
/**
* Does the math, side-effect free, for one odom step.
*
* @param itickDiff The tick difference from the previous step to this step.
* @param ideltaT The time difference from the previous step to this step.
* @return The newly computed OdomState.
*/
virtual OdomState odomMathStep(const std::valarray<std::int32_t> &itickDiff,
const QTime &ideltaT);
};
} // namespace okapi

36
SerialTest/include/okapi/api/units/QAcceleration.hpp Normal file
View File

@ -0,0 +1,36 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/QLength.hpp"
#include "okapi/api/units/QTime.hpp"
#include "okapi/api/units/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(0, 1, -2, 0, QAcceleration)
constexpr QAcceleration mps2 = meter / (second * second);
constexpr QAcceleration G = 9.80665 * mps2;
inline namespace literals {
constexpr QAcceleration operator"" _mps2(long double x) {
return QAcceleration(x);
}
constexpr QAcceleration operator"" _mps2(unsigned long long int x) {
return QAcceleration(static_cast<double>(x));
}
constexpr QAcceleration operator"" _G(long double x) {
return static_cast<double>(x) * G;
}
constexpr QAcceleration operator"" _G(unsigned long long int x) {
return static_cast<double>(x) * G;
}
} // namespace literals
} // namespace okapi

35
SerialTest/include/okapi/api/units/QAngle.hpp Normal file
View File

@ -0,0 +1,35 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/RQuantity.hpp"
#include <cmath>
namespace okapi {
QUANTITY_TYPE(0, 0, 0, 1, QAngle)
constexpr QAngle radian(1.0);
constexpr QAngle degree = static_cast<double>(2_pi / 360.0) * radian;
inline namespace literals {
constexpr QAngle operator"" _rad(long double x) {
return QAngle(x);
}
constexpr QAngle operator"" _rad(unsigned long long int x) {
return QAngle(static_cast<double>(x));
}
constexpr QAngle operator"" _deg(long double x) {
return static_cast<double>(x) * degree;
}
constexpr QAngle operator"" _deg(unsigned long long int x) {
return static_cast<double>(x) * degree;
}
} // namespace literals
} // namespace okapi

16
SerialTest/include/okapi/api/units/QAngularAcceleration.hpp Normal file
View File

@ -0,0 +1,16 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(0, 0, -2, 1, QAngularAcceleration)
}

16
SerialTest/include/okapi/api/units/QAngularJerk.hpp Normal file
View File

@ -0,0 +1,16 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(0, 0, -3, 1, QAngularJerk)
}

39
SerialTest/include/okapi/api/units/QAngularSpeed.hpp Normal file
View File

@ -0,0 +1,39 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/QFrequency.hpp"
#include "okapi/api/units/QTime.hpp"
#include "okapi/api/units/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(0, 0, -1, 1, QAngularSpeed)
constexpr QAngularSpeed radps = radian / second;
constexpr QAngularSpeed rpm = (360 * degree) / minute;
constexpr QAngularSpeed cps = (0.01 * degree) / second; // centidegree per second
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-function"
static QAngularSpeed convertHertzToRadPerSec(QFrequency in) {
return (in.convert(Hz) / 2_pi) * radps;
}
#pragma GCC diagnostic pop
inline namespace literals {
constexpr QAngularSpeed operator"" _rpm(long double x) {
return x * rpm;
}
constexpr QAngularSpeed operator"" _rpm(unsigned long long int x) {
return static_cast<double>(x) * rpm;
}
} // namespace literals
} // namespace okapi

26
SerialTest/include/okapi/api/units/QArea.hpp Normal file
View File

@ -0,0 +1,26 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/QLength.hpp"
#include "okapi/api/units/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(0, 2, 0, 0, QArea)
constexpr QArea kilometer2 = kilometer * kilometer;
constexpr QArea meter2 = meter * meter;
constexpr QArea decimeter2 = decimeter * decimeter;
constexpr QArea centimeter2 = centimeter * centimeter;
constexpr QArea millimeter2 = millimeter * millimeter;
constexpr QArea inch2 = inch * inch;
constexpr QArea foot2 = foot * foot;
constexpr QArea mile2 = mile * mile;
} // namespace okapi

43
SerialTest/include/okapi/api/units/QForce.hpp Normal file
View File

@ -0,0 +1,43 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/QAcceleration.hpp"
#include "okapi/api/units/QMass.hpp"
#include "okapi/api/units/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(1, 1, -2, 0, QForce)
constexpr QForce newton = (kg * meter) / (second * second);
constexpr QForce poundforce = pound * G;
constexpr QForce kilopond = kg * G;
inline namespace literals {
constexpr QForce operator"" _n(long double x) {
return QForce(x);
}
constexpr QForce operator"" _n(unsigned long long int x) {
return QForce(static_cast<double>(x));
}
constexpr QForce operator"" _lbf(long double x) {
return static_cast<double>(x) * poundforce;
}
constexpr QForce operator"" _lbf(unsigned long long int x) {
return static_cast<double>(x) * poundforce;
}
constexpr QForce operator"" _kp(long double x) {
return static_cast<double>(x) * kilopond;
}
constexpr QForce operator"" _kp(unsigned long long int x) {
return static_cast<double>(x) * kilopond;
}
} // namespace literals
} // namespace okapi

27
SerialTest/include/okapi/api/units/QFrequency.hpp Normal file
View File

@ -0,0 +1,27 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(0, 0, -1, 0, QFrequency)
constexpr QFrequency Hz(1.0);
inline namespace literals {
constexpr QFrequency operator"" _Hz(long double x) {
return QFrequency(x);
}
constexpr QFrequency operator"" _Hz(unsigned long long int x) {
return QFrequency(static_cast<long double>(x));
}
} // namespace literals
} // namespace okapi

18
SerialTest/include/okapi/api/units/QJerk.hpp Normal file
View File

@ -0,0 +1,18 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/QLength.hpp"
#include "okapi/api/units/QTime.hpp"
#include "okapi/api/units/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(0, 1, -3, 0, QJerk)
}

84
SerialTest/include/okapi/api/units/QLength.hpp Normal file
View File

@ -0,0 +1,84 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(0, 1, 0, 0, QLength)
constexpr QLength meter(1.0); // SI base unit
constexpr QLength decimeter = meter / 10;
constexpr QLength centimeter = meter / 100;
constexpr QLength millimeter = meter / 1000;
constexpr QLength kilometer = 1000 * meter;
constexpr QLength inch = 2.54 * centimeter;
constexpr QLength foot = 12 * inch;
constexpr QLength yard = 3 * foot;
constexpr QLength mile = 5280 * foot;
constexpr QLength tile = 24 * inch;
inline namespace literals {
constexpr QLength operator"" _mm(long double x) {
return static_cast<double>(x) * millimeter;
}
constexpr QLength operator"" _cm(long double x) {
return static_cast<double>(x) * centimeter;
}
constexpr QLength operator"" _m(long double x) {
return static_cast<double>(x) * meter;
}
constexpr QLength operator"" _km(long double x) {
return static_cast<double>(x) * kilometer;
}
constexpr QLength operator"" _mi(long double x) {
return static_cast<double>(x) * mile;
}
constexpr QLength operator"" _yd(long double x) {
return static_cast<double>(x) * yard;
}
constexpr QLength operator"" _ft(long double x) {
return static_cast<double>(x) * foot;
}
constexpr QLength operator"" _in(long double x) {
return static_cast<double>(x) * inch;
}
constexpr QLength operator"" _tile(long double x) {
return static_cast<double>(x) * tile;
}
constexpr QLength operator"" _mm(unsigned long long int x) {
return static_cast<double>(x) * millimeter;
}
constexpr QLength operator"" _cm(unsigned long long int x) {
return static_cast<double>(x) * centimeter;
}
constexpr QLength operator"" _m(unsigned long long int x) {
return static_cast<double>(x) * meter;
}
constexpr QLength operator"" _km(unsigned long long int x) {
return static_cast<double>(x) * kilometer;
}
constexpr QLength operator"" _mi(unsigned long long int x) {
return static_cast<double>(x) * mile;
}
constexpr QLength operator"" _yd(unsigned long long int x) {
return static_cast<double>(x) * yard;
}
constexpr QLength operator"" _ft(unsigned long long int x) {
return static_cast<double>(x) * foot;
}
constexpr QLength operator"" _in(unsigned long long int x) {
return static_cast<double>(x) * inch;
}
constexpr QLength operator"" _tile(unsigned long long int x) {
return static_cast<double>(x) * tile;
}
} // namespace literals
} // namespace okapi

62
SerialTest/include/okapi/api/units/QMass.hpp Normal file
View File

@ -0,0 +1,62 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(1, 0, 0, 0, QMass)
constexpr QMass kg(1.0); // SI base unit
constexpr QMass gramme = 0.001 * kg;
constexpr QMass tonne = 1000 * kg;
constexpr QMass ounce = 0.028349523125 * kg;
constexpr QMass pound = 16 * ounce;
constexpr QMass stone = 14 * pound;
inline namespace literals {
constexpr QMass operator"" _kg(long double x) {
return QMass(x);
}
constexpr QMass operator"" _g(long double x) {
return static_cast<double>(x) * gramme;
}
constexpr QMass operator"" _t(long double x) {
return static_cast<double>(x) * tonne;
}
constexpr QMass operator"" _oz(long double x) {
return static_cast<double>(x) * ounce;
}
constexpr QMass operator"" _lb(long double x) {
return static_cast<double>(x) * pound;
}
constexpr QMass operator"" _st(long double x) {
return static_cast<double>(x) * stone;
}
constexpr QMass operator"" _kg(unsigned long long int x) {
return QMass(static_cast<double>(x));
}
constexpr QMass operator"" _g(unsigned long long int x) {
return static_cast<double>(x) * gramme;
}
constexpr QMass operator"" _t(unsigned long long int x) {
return static_cast<double>(x) * tonne;
}
constexpr QMass operator"" _oz(unsigned long long int x) {
return static_cast<double>(x) * ounce;
}
constexpr QMass operator"" _lb(unsigned long long int x) {
return static_cast<double>(x) * pound;
}
constexpr QMass operator"" _st(unsigned long long int x) {
return static_cast<double>(x) * stone;
}
} // namespace literals
} // namespace okapi

44
SerialTest/include/okapi/api/units/QPressure.hpp Normal file
View File

@ -0,0 +1,44 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/QAcceleration.hpp"
#include "okapi/api/units/QArea.hpp"
#include "okapi/api/units/QMass.hpp"
#include "okapi/api/units/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(1, -1, -2, 0, QPressure)
constexpr QPressure pascal(1.0);
constexpr QPressure bar = 100000 * pascal;
constexpr QPressure psi = pound * G / inch2;
inline namespace literals {
constexpr QPressure operator"" _Pa(long double x) {
return QPressure(x);
}
constexpr QPressure operator"" _Pa(unsigned long long int x) {
return QPressure(static_cast<double>(x));
}
constexpr QPressure operator"" _bar(long double x) {
return static_cast<double>(x) * bar;
}
constexpr QPressure operator"" _bar(unsigned long long int x) {
return static_cast<double>(x) * bar;
}
constexpr QPressure operator"" _psi(long double x) {
return static_cast<double>(x) * psi;
}
constexpr QPressure operator"" _psi(unsigned long long int x) {
return static_cast<double>(x) * psi;
}
} // namespace literals
} // namespace okapi

43
SerialTest/include/okapi/api/units/QSpeed.hpp Normal file
View File

@ -0,0 +1,43 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/QLength.hpp"
#include "okapi/api/units/QTime.hpp"
#include "okapi/api/units/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(0, 1, -1, 0, QSpeed)
constexpr QSpeed mps = meter / second;
constexpr QSpeed miph = mile / hour;
constexpr QSpeed kmph = kilometer / hour;
inline namespace literals {
constexpr QSpeed operator"" _mps(long double x) {
return static_cast<double>(x) * mps;
}
constexpr QSpeed operator"" _miph(long double x) {
return static_cast<double>(x) * mile / hour;
}
constexpr QSpeed operator"" _kmph(long double x) {
return static_cast<double>(x) * kilometer / hour;
}
constexpr QSpeed operator"" _mps(unsigned long long int x) {
return static_cast<double>(x) * mps;
}
constexpr QSpeed operator"" _miph(unsigned long long int x) {
return static_cast<double>(x) * mile / hour;
}
constexpr QSpeed operator"" _kmph(unsigned long long int x) {
return static_cast<double>(x) * kilometer / hour;
}
} // namespace literals
} // namespace okapi

55
SerialTest/include/okapi/api/units/QTime.hpp Normal file
View File

@ -0,0 +1,55 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(0, 0, 1, 0, QTime)
constexpr QTime second(1.0); // SI base unit
constexpr QTime millisecond = second / 1000;
constexpr QTime minute = 60 * second;
constexpr QTime hour = 60 * minute;
constexpr QTime day = 24 * hour;
inline namespace literals {
constexpr QTime operator"" _s(long double x) {
return QTime(x);
}
constexpr QTime operator"" _ms(long double x) {
return static_cast<double>(x) * millisecond;
}
constexpr QTime operator"" _min(long double x) {
return static_cast<double>(x) * minute;
}
constexpr QTime operator"" _h(long double x) {
return static_cast<double>(x) * hour;
}
constexpr QTime operator"" _day(long double x) {
return static_cast<double>(x) * day;
}
constexpr QTime operator"" _s(unsigned long long int x) {
return QTime(static_cast<double>(x));
}
constexpr QTime operator"" _ms(unsigned long long int x) {
return static_cast<double>(x) * millisecond;
}
constexpr QTime operator"" _min(unsigned long long int x) {
return static_cast<double>(x) * minute;
}
constexpr QTime operator"" _h(unsigned long long int x) {
return static_cast<double>(x) * hour;
}
constexpr QTime operator"" _day(unsigned long long int x) {
return static_cast<double>(x) * day;
}
} // namespace literals
} // namespace okapi

43
SerialTest/include/okapi/api/units/QTorque.hpp Normal file
View File

@ -0,0 +1,43 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/QForce.hpp"
#include "okapi/api/units/QLength.hpp"
#include "okapi/api/units/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(1, 2, -2, 0, QTorque)
constexpr QTorque newtonMeter = newton * meter;
constexpr QTorque footPound = 1.355817948 * newtonMeter;
constexpr QTorque inchPound = 0.083333333 * footPound;
inline namespace literals {
constexpr QTorque operator"" _nM(long double x) {
return QTorque(x);
}
constexpr QTorque operator"" _nM(unsigned long long int x) {
return QTorque(static_cast<double>(x));
}
constexpr QTorque operator"" _inLb(long double x) {
return static_cast<double>(x) * inchPound;
}
constexpr QTorque operator"" _inLb(unsigned long long int x) {
return static_cast<double>(x) * inchPound;
}
constexpr QTorque operator"" _ftLb(long double x) {
return static_cast<double>(x) * footPound;
}
constexpr QTorque operator"" _ftLb(unsigned long long int x) {
return static_cast<double>(x) * footPound;
}
} // namespace literals
} // namespace okapi

28
SerialTest/include/okapi/api/units/QVolume.hpp Normal file
View File

@ -0,0 +1,28 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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/QArea.hpp"
#include "okapi/api/units/QLength.hpp"
#include "okapi/api/units/RQuantity.hpp"
namespace okapi {
QUANTITY_TYPE(0, 3, 0, 0, QVolume)
constexpr QVolume kilometer3 = kilometer2 * kilometer;
constexpr QVolume meter3 = meter2 * meter;
constexpr QVolume decimeter3 = decimeter2 * decimeter;
constexpr QVolume centimeter3 = centimeter2 * centimeter;
constexpr QVolume millimeter3 = millimeter2 * millimeter;
constexpr QVolume inch3 = inch2 * inch;
constexpr QVolume foot3 = foot2 * foot;
constexpr QVolume mile3 = mile2 * mile;
constexpr QVolume litre = decimeter3;
} // namespace okapi

419
SerialTest/include/okapi/api/units/RQuantity.hpp Normal file
View File

@ -0,0 +1,419 @@
/*
* This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
* here:
* https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
*
* 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 <cmath>
#include <ratio>
namespace okapi {
template <typename MassDim, typename LengthDim, typename TimeDim, typename AngleDim>
class RQuantity {
public:
explicit constexpr RQuantity() : value(0.0) {
}
explicit constexpr RQuantity(double val) : value(val) {
}
explicit constexpr RQuantity(long double val) : value(static_cast<double>(val)) {
}
// The intrinsic operations for a quantity with a unit is addition and subtraction
constexpr RQuantity const &operator+=(const RQuantity &rhs) {
value += rhs.value;
return *this;
}
constexpr RQuantity const &operator-=(const RQuantity &rhs) {
value -= rhs.value;
return *this;
}
constexpr RQuantity operator-() {
return RQuantity(value * -1);
}
constexpr RQuantity const &operator*=(const double rhs) {
value *= rhs;
return *this;
}
constexpr RQuantity const &operator/=(const double rhs) {
value /= rhs;
return *this;
}
// Returns the value of the quantity in multiples of the specified unit
constexpr double convert(const RQuantity &rhs) const {
return value / rhs.value;
}
// returns the raw value of the quantity (should not be used)
constexpr double getValue() const {
return value;
}
constexpr RQuantity<MassDim, LengthDim, TimeDim, AngleDim> abs() const {
return RQuantity<MassDim, LengthDim, TimeDim, AngleDim>(std::fabs(value));
}
constexpr RQuantity<std::ratio_divide<MassDim, std::ratio<2>>,
std::ratio_divide<LengthDim, std::ratio<2>>,
std::ratio_divide<TimeDim, std::ratio<2>>,
std::ratio_divide<AngleDim, std::ratio<2>>>
sqrt() const {
return RQuantity<std::ratio_divide<MassDim, std::ratio<2>>,
std::ratio_divide<LengthDim, std::ratio<2>>,
std::ratio_divide<TimeDim, std::ratio<2>>,
std::ratio_divide<AngleDim, std::ratio<2>>>(std::sqrt(value));
}
private:
double value;
};
// Predefined (physical unit) quantity types:
// ------------------------------------------
#define QUANTITY_TYPE(_Mdim, _Ldim, _Tdim, _Adim, name) \
typedef RQuantity<std::ratio<_Mdim>, std::ratio<_Ldim>, std::ratio<_Tdim>, std::ratio<_Adim>> \
name;
// Unitless
QUANTITY_TYPE(0, 0, 0, 0, Number)
constexpr Number number(1.0);
// Standard arithmetic operators:
// ------------------------------
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<M, L, T, A> operator+(const RQuantity<M, L, T, A> &lhs,
const RQuantity<M, L, T, A> &rhs) {
return RQuantity<M, L, T, A>(lhs.getValue() + rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<M, L, T, A> operator-(const RQuantity<M, L, T, A> &lhs,
const RQuantity<M, L, T, A> &rhs) {
return RQuantity<M, L, T, A>(lhs.getValue() - rhs.getValue());
}
template <typename M1,
typename L1,
typename T1,
typename A1,
typename M2,
typename L2,
typename T2,
typename A2>
constexpr RQuantity<std::ratio_add<M1, M2>,
std::ratio_add<L1, L2>,
std::ratio_add<T1, T2>,
std::ratio_add<A1, A2>>
operator*(const RQuantity<M1, L1, T1, A1> &lhs, const RQuantity<M2, L2, T2, A2> &rhs) {
return RQuantity<std::ratio_add<M1, M2>,
std::ratio_add<L1, L2>,
std::ratio_add<T1, T2>,
std::ratio_add<A1, A2>>(lhs.getValue() * rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<M, L, T, A> operator*(const double &lhs, const RQuantity<M, L, T, A> &rhs) {
return RQuantity<M, L, T, A>(lhs * rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<M, L, T, A> operator*(const RQuantity<M, L, T, A> &lhs, const double &rhs) {
return RQuantity<M, L, T, A>(lhs.getValue() * rhs);
}
template <typename M1,
typename L1,
typename T1,
typename A1,
typename M2,
typename L2,
typename T2,
typename A2>
constexpr RQuantity<std::ratio_subtract<M1, M2>,
std::ratio_subtract<L1, L2>,
std::ratio_subtract<T1, T2>,
std::ratio_subtract<A1, A2>>
operator/(const RQuantity<M1, L1, T1, A1> &lhs, const RQuantity<M2, L2, T2, A2> &rhs) {
return RQuantity<std::ratio_subtract<M1, M2>,
std::ratio_subtract<L1, L2>,
std::ratio_subtract<T1, T2>,
std::ratio_subtract<A1, A2>>(lhs.getValue() / rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<std::ratio_subtract<std::ratio<0>, M>,
std::ratio_subtract<std::ratio<0>, L>,
std::ratio_subtract<std::ratio<0>, T>,
std::ratio_subtract<std::ratio<0>, A>>
operator/(const double &x, const RQuantity<M, L, T, A> &rhs) {
return RQuantity<std::ratio_subtract<std::ratio<0>, M>,
std::ratio_subtract<std::ratio<0>, L>,
std::ratio_subtract<std::ratio<0>, T>,
std::ratio_subtract<std::ratio<0>, A>>(x / rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<M, L, T, A> operator/(const RQuantity<M, L, T, A> &rhs, const double &x) {
return RQuantity<M, L, T, A>(rhs.getValue() / x);
}
// Comparison operators for quantities:
// ------------------------------------
template <typename M, typename L, typename T, typename A>
constexpr bool operator==(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
return (lhs.getValue() == rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr bool operator!=(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
return (lhs.getValue() != rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr bool operator<=(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
return (lhs.getValue() <= rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr bool operator>=(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
return (lhs.getValue() >= rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr bool operator<(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
return (lhs.getValue() < rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr bool operator>(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
return (lhs.getValue() > rhs.getValue());
}
// Common math functions:
// ------------------------------
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<M, L, T, A> abs(const RQuantity<M, L, T, A> &rhs) {
return RQuantity<M, L, T, A>(std::abs(rhs.getValue()));
}
template <typename R, typename M, typename L, typename T, typename A>
constexpr RQuantity<std::ratio_multiply<M, R>,
std::ratio_multiply<L, R>,
std::ratio_multiply<T, R>,
std::ratio_multiply<A, R>>
pow(const RQuantity<M, L, T, A> &lhs) {
return RQuantity<std::ratio_multiply<M, R>,
std::ratio_multiply<L, R>,
std::ratio_multiply<T, R>,
std::ratio_multiply<A, R>>(std::pow(lhs.getValue(), double(R::num) / R::den));
}
template <int R, typename M, typename L, typename T, typename A>
constexpr RQuantity<std::ratio_multiply<M, std::ratio<R>>,
std::ratio_multiply<L, std::ratio<R>>,
std::ratio_multiply<T, std::ratio<R>>,
std::ratio_multiply<A, std::ratio<R>>>
pow(const RQuantity<M, L, T, A> &lhs) {
return RQuantity<std::ratio_multiply<M, std::ratio<R>>,
std::ratio_multiply<L, std::ratio<R>>,
std::ratio_multiply<T, std::ratio<R>>,
std::ratio_multiply<A, std::ratio<R>>>(std::pow(lhs.getValue(), R));
}
template <int R, typename M, typename L, typename T, typename A>
constexpr RQuantity<std::ratio_divide<M, std::ratio<R>>,
std::ratio_divide<L, std::ratio<R>>,
std::ratio_divide<T, std::ratio<R>>,
std::ratio_divide<A, std::ratio<R>>>
root(const RQuantity<M, L, T, A> &lhs) {
return RQuantity<std::ratio_divide<M, std::ratio<R>>,
std::ratio_divide<L, std::ratio<R>>,
std::ratio_divide<T, std::ratio<R>>,
std::ratio_divide<A, std::ratio<R>>>(std::pow(lhs.getValue(), 1.0 / R));
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<std::ratio_divide<M, std::ratio<2>>,
std::ratio_divide<L, std::ratio<2>>,
std::ratio_divide<T, std::ratio<2>>,
std::ratio_divide<A, std::ratio<2>>>
sqrt(const RQuantity<M, L, T, A> &rhs) {
return RQuantity<std::ratio_divide<M, std::ratio<2>>,
std::ratio_divide<L, std::ratio<2>>,
std::ratio_divide<T, std::ratio<2>>,
std::ratio_divide<A, std::ratio<2>>>(std::sqrt(rhs.getValue()));
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<std::ratio_divide<M, std::ratio<3>>,
std::ratio_divide<L, std::ratio<3>>,
std::ratio_divide<T, std::ratio<3>>,
std::ratio_divide<A, std::ratio<3>>>
cbrt(const RQuantity<M, L, T, A> &rhs) {
return RQuantity<std::ratio_divide<M, std::ratio<3>>,
std::ratio_divide<L, std::ratio<3>>,
std::ratio_divide<T, std::ratio<3>>,
std::ratio_divide<A, std::ratio<3>>>(std::cbrt(rhs.getValue()));
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<std::ratio_multiply<M, std::ratio<2>>,
std::ratio_multiply<L, std::ratio<2>>,
std::ratio_multiply<T, std::ratio<2>>,
std::ratio_multiply<A, std::ratio<2>>>
square(const RQuantity<M, L, T, A> &rhs) {
return RQuantity<std::ratio_multiply<M, std::ratio<2>>,
std::ratio_multiply<L, std::ratio<2>>,
std::ratio_multiply<T, std::ratio<2>>,
std::ratio_multiply<A, std::ratio<2>>>(std::pow(rhs.getValue(), 2));
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<std::ratio_multiply<M, std::ratio<3>>,
std::ratio_multiply<L, std::ratio<3>>,
std::ratio_multiply<T, std::ratio<3>>,
std::ratio_multiply<A, std::ratio<3>>>
cube(const RQuantity<M, L, T, A> &rhs) {
return RQuantity<std::ratio_multiply<M, std::ratio<3>>,
std::ratio_multiply<L, std::ratio<3>>,
std::ratio_multiply<T, std::ratio<3>>,
std::ratio_multiply<A, std::ratio<3>>>(std::pow(rhs.getValue(), 3));
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<M, L, T, A> hypot(const RQuantity<M, L, T, A> &lhs,
const RQuantity<M, L, T, A> &rhs) {
return RQuantity<M, L, T, A>(std::hypot(lhs.getValue(), rhs.getValue()));
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<M, L, T, A> mod(const RQuantity<M, L, T, A> &lhs,
const RQuantity<M, L, T, A> &rhs) {
return RQuantity<M, L, T, A>(std::fmod(lhs.getValue(), rhs.getValue()));
}
template <typename M1,
typename L1,
typename T1,
typename A1,
typename M2,
typename L2,
typename T2,
typename A2>
constexpr RQuantity<M1, L1, T1, A1> copysign(const RQuantity<M1, L1, T1, A1> &lhs,
const RQuantity<M2, L2, T2, A2> &rhs) {
return RQuantity<M1, L1, T1, A1>(std::copysign(lhs.getValue(), rhs.getValue()));
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<M, L, T, A> ceil(const RQuantity<M, L, T, A> &lhs,
const RQuantity<M, L, T, A> &rhs) {
return RQuantity<M, L, T, A>(std::ceil(lhs.getValue() / rhs.getValue()) * rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<M, L, T, A> floor(const RQuantity<M, L, T, A> &lhs,
const RQuantity<M, L, T, A> &rhs) {
return RQuantity<M, L, T, A>(std::floor(lhs.getValue() / rhs.getValue()) * rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<M, L, T, A> trunc(const RQuantity<M, L, T, A> &lhs,
const RQuantity<M, L, T, A> &rhs) {
return RQuantity<M, L, T, A>(std::trunc(lhs.getValue() / rhs.getValue()) * rhs.getValue());
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<M, L, T, A> round(const RQuantity<M, L, T, A> &lhs,
const RQuantity<M, L, T, A> &rhs) {
return RQuantity<M, L, T, A>(std::round(lhs.getValue() / rhs.getValue()) * rhs.getValue());
}
// Common trig functions:
// ------------------------------
constexpr Number
sin(const RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>> &rhs) {
return Number(std::sin(rhs.getValue()));
}
constexpr Number
cos(const RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>> &rhs) {
return Number(std::cos(rhs.getValue()));
}
constexpr Number
tan(const RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>> &rhs) {
return Number(std::tan(rhs.getValue()));
}
constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
asin(const Number &rhs) {
return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
std::asin(rhs.getValue()));
}
constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
acos(const Number &rhs) {
return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
std::acos(rhs.getValue()));
}
constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
atan(const Number &rhs) {
return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
std::atan(rhs.getValue()));
}
constexpr Number
sinh(const RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>> &rhs) {
return Number(std::sinh(rhs.getValue()));
}
constexpr Number
cosh(const RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>> &rhs) {
return Number(std::cosh(rhs.getValue()));
}
constexpr Number
tanh(const RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>> &rhs) {
return Number(std::tanh(rhs.getValue()));
}
constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
asinh(const Number &rhs) {
return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
std::asinh(rhs.getValue()));
}
constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
acosh(const Number &rhs) {
return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
std::acosh(rhs.getValue()));
}
constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
atanh(const Number &rhs) {
return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
std::atanh(rhs.getValue()));
}
template <typename M, typename L, typename T, typename A>
constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
atan2(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
std::atan2(lhs.getValue(), rhs.getValue()));
}
inline namespace literals {
constexpr long double operator"" _pi(long double x) {
return static_cast<double>(x) * 3.1415926535897932384626433832795;
}
constexpr long double operator"" _pi(unsigned long long int x) {
return static_cast<double>(x) * 3.1415926535897932384626433832795;
}
} // namespace literals
} // namespace okapi
// Conversion macro, which utilizes the string literals
#define ConvertTo(_x, _y) (_x).convert(1.0_##_y)

46
SerialTest/include/okapi/api/units/RQuantityName.hpp Normal file
View File

@ -0,0 +1,46 @@
#include "okapi/api/units/QAngle.hpp"
#include "okapi/api/units/QLength.hpp"
#include "okapi/api/units/QSpeed.hpp"
#include <stdexcept>
#include <typeindex>
#include <unordered_map>
#pragma once
namespace okapi {
/**
* Returns a short name for a unit.
* For example: `str(1_ft)` will return "ft", so will `1 * foot` or `0.3048_m`.
* Throws std::domain_error when `q` is a unit not defined in this function.
*
* @param q Your unit. Currently only QLength and QAngle are supported.
* @return The short string suffix for that unit.
*/
template <class QType> std::string getShortUnitName(QType q) {
const std::unordered_map<std::type_index, std::unordered_map<double, const char *>> shortNameMap =
{{typeid(meter),
{
{meter.getValue(), "m"},
{decimeter.getValue(), "dm"},
{centimeter.getValue(), "cm"},
{millimeter.getValue(), "mm"},
{kilometer.getValue(), "km"},
{inch.getValue(), "in"},
{foot.getValue(), "ft"},
{yard.getValue(), "yd"},
{mile.getValue(), "mi"},
{tile.getValue(), "tile"},
}},
{typeid(degree), {{degree.getValue(), "deg"}, {radian.getValue(), "rad"}}}};
try {
return shortNameMap.at(typeid(q)).at(q.getValue());
} catch (const std::out_of_range &e) {
throw std::domain_error(
"You have requested the shortname of an unknown unit somewhere (likely odometry strings). "
"Shortname for provided unit is unspecified. You can override this function to add more "
"names or manually specify the name instead.");
}
}
} // namespace okapi

41
SerialTest/include/okapi/api/util/abstractRate.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/coreProsAPI.hpp"
#include "okapi/api/units/QFrequency.hpp"
#include "okapi/api/units/QTime.hpp"
namespace okapi {
class AbstractRate {
public:
virtual ~AbstractRate();
/**
* Delay the current task such that it runs at the given frequency. The first delay will run for
* 1000/(ihz). Subsequent delays will adjust according to the previous runtime of the task.
*
* @param ihz the frequency
*/
virtual void delay(QFrequency ihz) = 0;
/**
* Delay the current task until itime has passed. This method can be used by periodic tasks to
* ensure a consistent execution frequency.
*
* @param itime the time period
*/
virtual void delayUntil(QTime itime) = 0;
/**
* Delay the current task until ims milliseconds have passed. This method can be used by
* periodic tasks to ensure a consistent execution frequency.
*
* @param ims the time period
*/
virtual void delayUntil(uint32_t ims) = 0;
};
} // namespace okapi

125
SerialTest/include/okapi/api/util/abstractTimer.hpp Normal file
View File

@ -0,0 +1,125 @@
/*
* 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/QFrequency.hpp"
#include "okapi/api/units/QTime.hpp"
namespace okapi {
class AbstractTimer {
public:
/**
* A Timer base class which implements its methods in terms of millis().
*
* @param ifirstCalled the current time
*/
explicit AbstractTimer(QTime ifirstCalled);
virtual ~AbstractTimer();
/**
* Returns the current time in units of QTime.
*
* @return the current time
*/
virtual QTime millis() const = 0;
/**
* Returns the time passed in ms since the previous call of this function.
*
* @return The time passed in ms since the previous call of this function
*/
virtual QTime getDt();
/**
* Returns the time passed in ms since the previous call of getDt(). Does not change the time
* recorded by getDt().
*
* @return The time passed in ms since the previous call of getDt()
*/
virtual QTime readDt() const;
/**
* Returns the time the timer was first constructed.
*
* @return The time the timer was first constructed
*/
virtual QTime getStartingTime() const;
/**
* Returns the time since the timer was first constructed.
*
* @return The time since the timer was first constructed
*/
virtual QTime getDtFromStart() const;
/**
* Place a time marker. Placing another marker will overwrite the previous one.
*/
virtual void placeMark();
/**
* Clears the marker.
*
* @return The old marker
*/
virtual QTime clearMark();
/**
* Place a hard time marker. Placing another hard marker will not overwrite the previous one;
* instead, call clearHardMark() and then place another.
*/
virtual void placeHardMark();
/**
* Clears the hard marker.
*
* @return The old hard marker
*/
virtual QTime clearHardMark();
/**
* Returns the time since the time marker. Returns 0_ms if there is no marker.
*
* @return The time since the time marker
*/
virtual QTime getDtFromMark() const;
/**
* Returns the time since the hard time marker. Returns 0_ms if there is no hard marker set.
*
* @return The time since the hard time marker
*/
virtual QTime getDtFromHardMark() const;
/**
* Returns true when the input time period has passed, then resets. Meant to be used in loops
* to run an action every time period without blocking.
*
* @param time time period
* @return true when the input time period has passed, false after reading true until the
* period has passed again
*/
virtual bool repeat(QTime time);
/**
* Returns true when the input time period has passed, then resets. Meant to be used in loops
* to run an action every time period without blocking.
*
* @param frequency the repeat frequency
* @return true when the input time period has passed, false after reading true until the
* period has passed again
*/
virtual bool repeat(QFrequency frequency);
protected:
QTime firstCalled;
QTime lastCalled;
QTime mark;
QTime hardMark;
QTime repeatMark;
};
} // namespace okapi

192
SerialTest/include/okapi/api/util/logging.hpp Normal file
View File

@ -0,0 +1,192 @@
/*
* 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/coreProsAPI.hpp"
#include "okapi/api/util/abstractTimer.hpp"
#include "okapi/api/util/mathUtil.hpp"
#include <memory>
#include <mutex>
#if defined(THREADS_STD)
#else
#include "okapi/impl/util/timer.hpp"
#endif
#define LOG_DEBUG(msg) logger->debug([=]() { return msg; })
#define LOG_INFO(msg) logger->info([=]() { return msg; })
#define LOG_WARN(msg) logger->warn([=]() { return msg; })
#define LOG_ERROR(msg) logger->error([=]() { return msg; })
#define LOG_DEBUG_S(msg) LOG_DEBUG(std::string(msg))
#define LOG_INFO_S(msg) LOG_INFO(std::string(msg))
#define LOG_WARN_S(msg) LOG_WARN(std::string(msg))
#define LOG_ERROR_S(msg) LOG_ERROR(std::string(msg))
namespace okapi {
class Logger {
public:
enum class LogLevel {
debug = 4, ///< debug
info = 3, ///< info
warn = 2, ///< warn
error = 1, ///< error
off = 0 ///< off
};
/**
* A logger that does nothing.
*/
Logger() noexcept;
/**
* A logger that opens the input file by name. If the file contains `/ser/`, the file will be
* opened in write mode. Otherwise, the file will be opened in append mode. The file will be
* closed when the logger is destructed.
*
* @param itimer A timer used to get the current time for log statements.
* @param ifileName The name of the log file to open.
* @param ilevel The log level. Log statements more verbose than this level will be disabled.
*/
Logger(std::unique_ptr<AbstractTimer> itimer,
std::string_view ifileName,
const LogLevel &ilevel) noexcept;
/**
* A logger that uses an existing file handle. The file will be closed when the logger is
* destructed.
*
* @param itimer A timer used to get the current time for log statements.
* @param ifile The log file to open. Will be closed by the logger!
* @param ilevel The log level. Log statements more verbose than this level will be disabled.
*/
Logger(std::unique_ptr<AbstractTimer> itimer, FILE *ifile, const LogLevel &ilevel) noexcept;
~Logger();
constexpr bool isDebugLevelEnabled() const noexcept {
return toUnderlyingType(logLevel) >= toUnderlyingType(LogLevel::debug);
}
template <typename T> void debug(T ilazyMessage) noexcept {
if (isDebugLevelEnabled() && logfile && timer) {
std::scoped_lock lock(logfileMutex);
fprintf(logfile,
"%ld (%s) DEBUG: %s\n",
static_cast<long>(timer->millis().convert(millisecond)),
CrossplatformThread::getName().c_str(),
ilazyMessage().c_str());
}
}
constexpr bool isInfoLevelEnabled() const noexcept {
return toUnderlyingType(logLevel) >= toUnderlyingType(LogLevel::info);
}
template <typename T> void info(T ilazyMessage) noexcept {
if (isInfoLevelEnabled() && logfile && timer) {
std::scoped_lock lock(logfileMutex);
fprintf(logfile,
"%ld (%s) INFO: %s\n",
static_cast<long>(timer->millis().convert(millisecond)),
CrossplatformThread::getName().c_str(),
ilazyMessage().c_str());
}
}
constexpr bool isWarnLevelEnabled() const noexcept {
return toUnderlyingType(logLevel) >= toUnderlyingType(LogLevel::warn);
}
template <typename T> void warn(T ilazyMessage) noexcept {
if (isWarnLevelEnabled() && logfile && timer) {
std::scoped_lock lock(logfileMutex);
fprintf(logfile,
"%ld (%s) WARN: %s\n",
static_cast<long>(timer->millis().convert(millisecond)),
CrossplatformThread::getName().c_str(),
ilazyMessage().c_str());
}
}
constexpr bool isErrorLevelEnabled() const noexcept {
return toUnderlyingType(logLevel) >= toUnderlyingType(LogLevel::error);
}
template <typename T> void error(T ilazyMessage) noexcept {
if (isErrorLevelEnabled() && logfile && timer) {
std::scoped_lock lock(logfileMutex);
fprintf(logfile,
"%ld (%s) ERROR: %s\n",
static_cast<long>(timer->millis().convert(millisecond)),
CrossplatformThread::getName().c_str(),
ilazyMessage().c_str());
}
}
/**
* Closes the connection to the log file.
*/
constexpr void close() noexcept {
if (logfile) {
fclose(logfile);
logfile = nullptr;
}
}
/**
* @return The default logger.
*/
static std::shared_ptr<Logger> getDefaultLogger();
/**
* Sets a new default logger. OkapiLib classes use the default logger unless given another logger
* in their constructor.
*
* @param ilogger The new logger instance.
*/
static void setDefaultLogger(std::shared_ptr<Logger> ilogger);
private:
const std::unique_ptr<AbstractTimer> timer;
const LogLevel logLevel;
FILE *logfile;
CrossplatformMutex logfileMutex;
static bool isSerialStream(std::string_view filename);
};
extern std::shared_ptr<Logger> defaultLogger;
struct DefaultLoggerInitializer {
DefaultLoggerInitializer() {
if (count++ == 0) {
init();
}
}
~DefaultLoggerInitializer() {
if (--count == 0) {
cleanup();
}
}
static int count;
static void init() {
#if defined(THREADS_STD)
defaultLogger = std::make_shared<Logger>();
#else
defaultLogger =
std::make_shared<Logger>(std::make_unique<Timer>(), "/ser/sout", Logger::LogLevel::warn);
#endif
}
static void cleanup() {
}
};
static DefaultLoggerInitializer defaultLoggerInitializer; // NOLINT(cert-err58-cpp)
} // namespace okapi

255
SerialTest/include/okapi/api/util/mathUtil.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/device/motor/abstractMotor.hpp"
#include <algorithm>
#include <cstdint>
#include <math.h>
#include <type_traits>
namespace okapi {
/**
* Converts inches to millimeters.
*/
static constexpr double inchToMM = 25.4;
/**
* Converts millimeters to inches.
*/
static constexpr double mmToInch = 0.0393700787;
/**
* Converts degrees to radians.
*/
static constexpr double degreeToRadian = 0.01745329252;
/**
* Converts radians to degrees.
*/
static constexpr double radianToDegree = 57.2957795;
/**
* The ticks per rotation of the 393 IME with torque gearing.
*/
static constexpr double imeTorqueTPR = 627.2;
/**
* The ticks per rotation of the 393 IME with speed gearing.
*/
static constexpr std::int32_t imeSpeedTPR = 392;
/**
* The ticks per rotation of the 393 IME with turbo gearing.
*/
static constexpr double imeTurboTPR = 261.333;
/**
* The ticks per rotation of the 269 IME.
*/
static constexpr double ime269TPR = 240.448;
/**
* The ticks per rotation of the V5 motor with a red gearset.
*/
static constexpr std::int32_t imev5RedTPR = 1800;
/**
* The ticks per rotation of the V5 motor with a green gearset.
*/
static constexpr std::int32_t imev5GreenTPR = 900;
/**
* The ticks per rotation of the V5 motor with a blue gearset.
*/
static constexpr std::int32_t imev5BlueTPR = 300;
/**
* The ticks per rotation of the red quadrature encoders.
*/
static constexpr std::int32_t quadEncoderTPR = 360;
/**
* The value of pi.
*/
static constexpr double pi = 3.1415926535897932;
/**
* The value of pi divided by 2.
*/
static constexpr double pi2 = 1.5707963267948966;
/**
* The conventional value of gravity of Earth.
*/
static constexpr double gravity = 9.80665;
/**
* Same as PROS_ERR.
*/
static constexpr auto OKAPI_PROS_ERR = INT32_MAX;
/**
* Same as PROS_ERR_F.
*/
static constexpr auto OKAPI_PROS_ERR_F = INFINITY;
/**
* The maximum voltage that can be sent to V5 motors.
*/
static constexpr double v5MotorMaxVoltage = 12000;
/**
* The polling frequency of V5 motors in milliseconds.
*/
static constexpr std::int8_t motorUpdateRate = 10;
/**
* The polling frequency of the ADI ports in milliseconds.
*/
static constexpr std::int8_t adiUpdateRate = 10;
/**
* Integer power function. Computes `base^expo`.
*
* @param base The base.
* @param expo The exponent.
* @return `base^expo`.
*/
constexpr double ipow(const double base, const int expo) {
return (expo == 0) ? 1
: expo == 1 ? base
: expo > 1 ? ((expo & 1) ? base * ipow(base, expo - 1)
: ipow(base, expo / 2) * ipow(base, expo / 2))
: 1 / ipow(base, -expo);
}
/**
* Cuts out a range from the number. The new range of the input number will be
* `(-inf, min]U[max, +inf)`. If value sits equally between `min` and `max`, `max` will be returned.
*
* @param value The number to bound.
* @param min The lower bound of range.
* @param max The upper bound of range.
* @return The remapped value.
*/
constexpr double cutRange(const double value, const double min, const double max) {
const double middle = max - ((max - min) / 2);
if (value > min && value < middle) {
return min;
} else if (value <= max && value >= middle) {
return max;
}
return value;
}
/**
* Deadbands a range of the number. Returns the input value, or `0` if it is in the range `[min,
* max]`.
*
* @param value The number to deadband.
* @param min The lower bound of deadband.
* @param max The upper bound of deadband.
* @return The input value or `0` if it is in the range `[min, max]`.
*/
constexpr double deadband(const double value, const double min, const double max) {
return std::clamp(value, min, max) == value ? 0 : value;
}
/**
* Remap a value in the range `[oldMin, oldMax]` to the range `[newMin, newMax]`.
*
* @param value The value in the old range.
* @param oldMin The old range lower bound.
* @param oldMax The old range upper bound.
* @param newMin The new range lower bound.
* @param newMax The new range upper bound.
* @return The input value in the new range `[newMin, newMax]`.
*/
constexpr double remapRange(const double value,
const double oldMin,
const double oldMax,
const double newMin,
const double newMax) {
return (value - oldMin) * ((newMax - newMin) / (oldMax - oldMin)) + newMin;
}
/**
* Converts an enum to its value type.
*
* @param e The enum value.
* @return The corresponding value.
*/
template <typename E> constexpr auto toUnderlyingType(const E e) noexcept {
return static_cast<std::underlying_type_t<E>>(e);
}
/**
* Converts a bool to a sign.
*
* @param b The bool.
* @return True corresponds to `1` and false corresponds to `-1`.
*/
constexpr auto boolToSign(const bool b) noexcept {
return b ? 1 : -1;
}
/**
* Computes `lhs mod rhs` using Euclidean division. C's `%` symbol computes the remainder, not
* modulus.
*
* @param lhs The left-hand side.
* @param rhs The right-hand side.
* @return `lhs` mod `rhs`.
*/
constexpr long modulus(const long lhs, const long rhs) noexcept {
return ((lhs % rhs) + rhs) % rhs;
}
/**
* Converts a gearset to its TPR.
*
* @param igearset The gearset.
* @return The corresponding TPR.
*/
constexpr std::int32_t gearsetToTPR(const AbstractMotor::gearset igearset) noexcept {
switch (igearset) {
case AbstractMotor::gearset::red:
return imev5RedTPR;
case AbstractMotor::gearset::green:
return imev5GreenTPR;
case AbstractMotor::gearset::blue:
case AbstractMotor::gearset::invalid:
default:
return imev5BlueTPR;
}
}
/**
* Maps ADI port numbers/chars to numbers:
* ```
* when (port) {
* in ['a', 'h'] -> [1, 8]
* in ['A', 'H'] -> [1, 8]
* else -> [1, 8]
* }
* ```
*
* @param port The ADI port number or char.
* @return An equivalent ADI port number.
*/
constexpr std::int8_t transformADIPort(const std::int8_t port) {
if (port >= 'a' && port <= 'h') {
return port - ('a' - 1);
} else if (port >= 'A' && port <= 'H') {
return port - ('A' - 1);
} else {
return port;
}
}
} // namespace okapi

34
SerialTest/include/okapi/api/util/supplier.hpp Normal file
View File

@ -0,0 +1,34 @@
/*
* 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 {
/**
* A supplier of instances of T.
*
* @tparam T the type to supply
*/
template <typename T> class Supplier {
public:
explicit Supplier(std::function<T(void)> ifunc) : func(ifunc) {
}
virtual ~Supplier() = default;
/**
* Get an instance of type T. This is usually a new instance, but it does not have to be.
* @return an instance of T
*/
T get() const {
return func();
}
protected:
std::function<T(void)> func;
};
} // namespace okapi

41
SerialTest/include/okapi/api/util/timeUtil.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/util/settledUtil.hpp"
#include "okapi/api/util/abstractRate.hpp"
#include "okapi/api/util/abstractTimer.hpp"
#include "okapi/api/util/supplier.hpp"
namespace okapi {
/**
* Utility class for holding an AbstractTimer, AbstractRate, and SettledUtil together in one
* class since they are commonly used together.
*/
class TimeUtil {
public:
TimeUtil(const Supplier<std::unique_ptr<AbstractTimer>> &itimerSupplier,
const Supplier<std::unique_ptr<AbstractRate>> &irateSupplier,
const Supplier<std::unique_ptr<SettledUtil>> &isettledUtilSupplier);
std::unique_ptr<AbstractTimer> getTimer() const;
std::unique_ptr<AbstractRate> getRate() const;
std::unique_ptr<SettledUtil> getSettledUtil() const;
Supplier<std::unique_ptr<AbstractTimer>> getTimerSupplier() const;
Supplier<std::unique_ptr<AbstractRate>> getRateSupplier() const;
Supplier<std::unique_ptr<SettledUtil>> getSettledUtilSupplier() const;
protected:
Supplier<std::unique_ptr<AbstractTimer>> timerSupplier;
Supplier<std::unique_ptr<AbstractRate>> rateSupplier;
Supplier<std::unique_ptr<SettledUtil>> settledUtilSupplier;
};
} // namespace okapi

506
SerialTest/include/okapi/impl/chassis/controller/chassisControllerBuilder.hpp Normal file
View File

@ -0,0 +1,506 @@
/*
* 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/chassisControllerIntegrated.hpp"
#include "okapi/api/chassis/controller/chassisControllerPid.hpp"
#include "okapi/api/chassis/controller/defaultOdomChassisController.hpp"
#include "okapi/api/chassis/model/hDriveModel.hpp"
#include "okapi/api/chassis/model/skidSteerModel.hpp"
#include "okapi/api/chassis/model/xDriveModel.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/mathUtil.hpp"
#include "okapi/impl/device/motor/motor.hpp"
#include "okapi/impl/device/motor/motorGroup.hpp"
#include "okapi/impl/device/rotarysensor/adiEncoder.hpp"
#include "okapi/impl/device/rotarysensor/integratedEncoder.hpp"
#include "okapi/impl/device/rotarysensor/rotationSensor.hpp"
#include "okapi/impl/util/timeUtilFactory.hpp"
namespace okapi {
class ChassisControllerBuilder {
public:
/**
* A builder that creates ChassisControllers. Use this to create your ChassisController.
*
* @param ilogger The logger this instance will log to.
*/
explicit ChassisControllerBuilder(
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* Sets the motors using a skid-steer layout.
*
* @param ileft The left motor.
* @param iright The right motor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withMotors(const Motor &ileft, const Motor &iright);
/**
* Sets the motors using a skid-steer layout.
*
* @param ileft The left motor.
* @param iright The right motor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withMotors(const MotorGroup &ileft, const MotorGroup &iright);
/**
* Sets the motors using a skid-steer layout.
*
* @param ileft The left motor.
* @param iright The right motor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withMotors(const std::shared_ptr<AbstractMotor> &ileft,
const std::shared_ptr<AbstractMotor> &iright);
/**
* Sets the motors using an x-drive layout.
*
* @param itopLeft The top left motor.
* @param itopRight The top right motor.
* @param ibottomRight The bottom right motor.
* @param ibottomLeft The bottom left motor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withMotors(const Motor &itopLeft,
const Motor &itopRight,
const Motor &ibottomRight,
const Motor &ibottomLeft);
/**
* Sets the motors using an x-drive layout.
*
* @param itopLeft The top left motor.
* @param itopRight The top right motor.
* @param ibottomRight The bottom right motor.
* @param ibottomLeft The bottom left motor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withMotors(const MotorGroup &itopLeft,
const MotorGroup &itopRight,
const MotorGroup &ibottomRight,
const MotorGroup &ibottomLeft);
/**
* Sets the motors using an x-drive layout.
*
* @param itopLeft The top left motor.
* @param itopRight The top right motor.
* @param ibottomRight The bottom right motor.
* @param ibottomLeft The bottom left motor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withMotors(const std::shared_ptr<AbstractMotor> &itopLeft,
const std::shared_ptr<AbstractMotor> &itopRight,
const std::shared_ptr<AbstractMotor> &ibottomRight,
const std::shared_ptr<AbstractMotor> &ibottomLeft);
/**
* Sets the motors using an h-drive layout.
*
* @param ileft The left motor.
* @param iright The right motor.
* @param imiddle The middle motor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &
withMotors(const Motor &ileft, const Motor &iright, const Motor &imiddle);
/**
* Sets the motors using an h-drive layout.
*
* @param ileft The left motor.
* @param iright The right motor.
* @param imiddle The middle motor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &
withMotors(const MotorGroup &ileft, const MotorGroup &iright, const MotorGroup &imiddle);
/**
* Sets the motors using an h-drive layout.
*
* @param ileft The left motor.
* @param iright The right motor.
* @param imiddle The middle motor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &
withMotors(const MotorGroup &ileft, const MotorGroup &iright, const Motor &imiddle);
/**
* Sets the motors using an h-drive layout.
*
* @param ileft The left motor.
* @param iright The right motor.
* @param imiddle The middle motor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withMotors(const std::shared_ptr<AbstractMotor> &ileft,
const std::shared_ptr<AbstractMotor> &iright,
const std::shared_ptr<AbstractMotor> &imiddle);
/**
* Sets the sensors. The default sensors are the motor's integrated encoders.
*
* @param ileft The left side sensor.
* @param iright The right side sensor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withSensors(const ADIEncoder &ileft, const ADIEncoder &iright);
/**
* Sets the sensors. The default sensors are the motor's integrated encoders.
*
* @param ileft The left side sensor.
* @param iright The right side sensor.
* @param imiddle The middle sensor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &
withSensors(const ADIEncoder &ileft, const ADIEncoder &iright, const ADIEncoder &imiddle);
/**
* Sets the sensors. The default sensors are the motor's integrated encoders.
*
* @param ileft The left side sensor.
* @param iright The right side sensor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withSensors(const RotationSensor &ileft, const RotationSensor &iright);
/**
* Sets the sensors. The default sensors are the motor's integrated encoders.
*
* @param ileft The left side sensor.
* @param iright The right side sensor.
* @param imiddle The middle sensor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withSensors(const RotationSensor &ileft,
const RotationSensor &iright,
const RotationSensor &imiddle);
/**
* Sets the sensors. The default sensors are the motor's integrated encoders.
*
* @param ileft The left side sensor.
* @param iright The right side sensor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withSensors(const IntegratedEncoder &ileft,
const IntegratedEncoder &iright);
/**
* Sets the sensors. The default sensors are the motor's integrated encoders.
*
* @param ileft The left side sensor.
* @param iright The right side sensor.
* @param imiddle The middle sensor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withSensors(const IntegratedEncoder &ileft,
const IntegratedEncoder &iright,
const ADIEncoder &imiddle);
/**
* Sets the sensors. The default sensors are the motor's integrated encoders.
*
* @param ileft The left side sensor.
* @param iright The right side sensor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withSensors(const std::shared_ptr<ContinuousRotarySensor> &ileft,
const std::shared_ptr<ContinuousRotarySensor> &iright);
/**
* Sets the sensors. The default sensors are the motor's integrated encoders.
*
* @param ileft The left side sensor.
* @param iright The right side sensor.
* @param imiddle The middle sensor.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withSensors(const std::shared_ptr<ContinuousRotarySensor> &ileft,
const std::shared_ptr<ContinuousRotarySensor> &iright,
const std::shared_ptr<ContinuousRotarySensor> &imiddle);
/**
* Sets the PID controller gains, causing the builder to generate a ChassisControllerPID. Uses the
* turn controller's gains for the angle controller's gains.
*
* @param idistanceGains The distance controller's gains.
* @param iturnGains The turn controller's gains.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withGains(const IterativePosPIDController::Gains &idistanceGains,
const IterativePosPIDController::Gains &iturnGains);
/**
* Sets the PID controller gains, causing the builder to generate a ChassisControllerPID.
*
* @param idistanceGains The distance controller's gains.
* @param iturnGains The turn controller's gains.
* @param iangleGains The angle controller's gains.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withGains(const IterativePosPIDController::Gains &idistanceGains,
const IterativePosPIDController::Gains &iturnGains,
const IterativePosPIDController::Gains &iangleGains);
/**
* Sets the odometry information, causing the builder to generate an Odometry variant.
*
* @param imode The new default StateMode used to interpret target points and query the Odometry
* state.
* @param imoveThreshold The minimum length movement.
* @param iturnThreshold The minimum angle turn.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withOdometry(const StateMode &imode = StateMode::FRAME_TRANSFORMATION,
const QLength &imoveThreshold = 0_mm,
const QAngle &iturnThreshold = 0_deg);
/**
* Sets the odometry information, causing the builder to generate an Odometry variant.
*
* @param iodomScales The ChassisScales used just for odometry (if they are different than those
* for the drive).
* @param imode The new default StateMode used to interpret target points and query the Odometry
* state.
* @param imoveThreshold The minimum length movement.
* @param iturnThreshold The minimum angle turn.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withOdometry(const ChassisScales &iodomScales,
const StateMode &imode = StateMode::FRAME_TRANSFORMATION,
const QLength &imoveThreshold = 0_mm,
const QAngle &iturnThreshold = 0_deg);
/**
* Sets the odometry information, causing the builder to generate an Odometry variant.
*
* @param iodometry The odometry object.
* @param imode The new default StateMode used to interpret target points and query the Odometry
* state.
* @param imoveThreshold The minimum length movement.
* @param iturnThreshold The minimum angle turn.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withOdometry(std::shared_ptr<Odometry> iodometry,
const StateMode &imode = StateMode::FRAME_TRANSFORMATION,
const QLength &imoveThreshold = 0_mm,
const QAngle &iturnThreshold = 0_deg);
/**
* Sets the derivative filters. Uses a PassthroughFilter by default.
*
* @param idistanceFilter The distance controller's filter.
* @param iturnFilter The turn controller's filter.
* @param iangleFilter The angle controller's filter.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withDerivativeFilters(
std::unique_ptr<Filter> idistanceFilter,
std::unique_ptr<Filter> iturnFilter = std::make_unique<PassthroughFilter>(),
std::unique_ptr<Filter> iangleFilter = std::make_unique<PassthroughFilter>());
/**
* Sets the chassis dimensions.
*
* @param igearset The gearset in the drive motors.
* @param iscales The ChassisScales for the base.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withDimensions(const AbstractMotor::GearsetRatioPair &igearset,
const ChassisScales &iscales);
/**
* Sets the max velocity. Overrides the max velocity of the gearset.
*
* @param imaxVelocity The max velocity.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withMaxVelocity(double imaxVelocity);
/**
* Sets the max voltage. The default is `12000`.
*
* @param imaxVoltage The max voltage.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withMaxVoltage(double imaxVoltage);
/**
* Sets the TimeUtilFactory used when building a ChassisController. This instance will be given
* to the ChassisController (not to controllers it uses). The default is the static
* TimeUtilFactory.
*
* @param itimeUtilFactory The TimeUtilFactory.
* @return An ongoing builder.
*/
ChassisControllerBuilder &
withChassisControllerTimeUtilFactory(const TimeUtilFactory &itimeUtilFactory);
/**
* Sets the TimeUtilFactory used when building a ClosedLoopController. This instance will be given
* to any ClosedLoopController instances. The default is the static TimeUtilFactory.
*
* @param itimeUtilFactory The TimeUtilFactory.
* @return An ongoing builder.
*/
ChassisControllerBuilder &
withClosedLoopControllerTimeUtilFactory(const TimeUtilFactory &itimeUtilFactory);
/**
* Creates a new ConfigurableTimeUtilFactory with the given parameters. Given to any
* ClosedLoopController instances.
*
* @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.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withClosedLoopControllerTimeUtil(double iatTargetError = 50,
double iatTargetDerivative = 5,
const QTime &iatTargetTime = 250_ms);
/**
* Sets the TimeUtilFactory used when building an Odometry. The default is the static
* TimeUtilFactory.
*
* @param itimeUtilFactory The TimeUtilFactory.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withOdometryTimeUtilFactory(const TimeUtilFactory &itimeUtilFactory);
/**
* Sets the logger used for the ChassisController and ClosedLoopControllers.
*
* @param ilogger The logger.
* @return An ongoing builder.
*/
ChassisControllerBuilder &withLogger(const std::shared_ptr<Logger> &ilogger);
/**
* Parents the internal tasks started by this builder to the current task, meaning they will be
* deleted once the current task is deleted. The `initialize` and `competition_initialize` tasks
* are never parented to. This is the default behavior.
*
* Read more about this in the [builders and tasks tutorial]
* (docs/tutorials/concepts/builders-and-tasks.md).
*
* @return An ongoing builder.
*/
ChassisControllerBuilder &parentedToCurrentTask();
/**
* Prevents parenting the internal tasks started by this builder to the current task, meaning they
* will not be deleted once the current task is deleted. This can cause runaway tasks, but is
* sometimes the desired behavior (e.x., if you want to use this builder once in `autonomous` and
* then again in `opcontrol`).
*
* Read more about this in the [builders and tasks tutorial]
* (docs/tutorials/concepts/builders-and-tasks.md).
*
* @return An ongoing builder.
*/
ChassisControllerBuilder &notParentedToCurrentTask();
/**
* Builds the ChassisController. Throws a std::runtime_exception if no motors were set or if no
* dimensions were set.
*
* @return A fully built ChassisController.
*/
std::shared_ptr<ChassisController> build();
/**
* Builds the OdomChassisController. Throws a std::runtime_exception if no motors were set, if no
* dimensions were set, or if no odometry information was passed.
*
* @return A fully built OdomChassisController.
*/
std::shared_ptr<OdomChassisController> buildOdometry();
private:
std::shared_ptr<Logger> logger;
struct SkidSteerMotors {
std::shared_ptr<AbstractMotor> left;
std::shared_ptr<AbstractMotor> right;
};
struct XDriveMotors {
std::shared_ptr<AbstractMotor> topLeft;
std::shared_ptr<AbstractMotor> topRight;
std::shared_ptr<AbstractMotor> bottomRight;
std::shared_ptr<AbstractMotor> bottomLeft;
};
struct HDriveMotors {
std::shared_ptr<AbstractMotor> left;
std::shared_ptr<AbstractMotor> right;
std::shared_ptr<AbstractMotor> middle;
};
enum class DriveMode { SkidSteer, XDrive, HDrive };
bool hasMotors{false}; // Used to verify motors were passed
DriveMode driveMode{DriveMode::SkidSteer};
SkidSteerMotors skidSteerMotors;
XDriveMotors xDriveMotors;
HDriveMotors hDriveMotors;
bool sensorsSetByUser{false}; // Used so motors don't overwrite sensors set manually
std::shared_ptr<ContinuousRotarySensor> leftSensor{nullptr};
std::shared_ptr<ContinuousRotarySensor> rightSensor{nullptr};
std::shared_ptr<ContinuousRotarySensor> middleSensor{nullptr};
bool hasGains{false}; // Whether gains were passed, no gains means CCI
IterativePosPIDController::Gains distanceGains;
std::unique_ptr<Filter> distanceFilter = std::make_unique<PassthroughFilter>();
IterativePosPIDController::Gains angleGains;
std::unique_ptr<Filter> angleFilter = std::make_unique<PassthroughFilter>();
IterativePosPIDController::Gains turnGains;
std::unique_ptr<Filter> turnFilter = std::make_unique<PassthroughFilter>();
TimeUtilFactory chassisControllerTimeUtilFactory = TimeUtilFactory();
TimeUtilFactory closedLoopControllerTimeUtilFactory = TimeUtilFactory();
TimeUtilFactory odometryTimeUtilFactory = TimeUtilFactory();
AbstractMotor::GearsetRatioPair gearset{AbstractMotor::gearset::invalid, 1.0};
ChassisScales driveScales{{1, 1}, imev5GreenTPR};
bool differentOdomScales{false};
ChassisScales odomScales{{1, 1}, imev5GreenTPR};
std::shared_ptr<Logger> controllerLogger = Logger::getDefaultLogger();
bool hasOdom{false}; // Whether odometry was passed
std::shared_ptr<Odometry> odometry;
StateMode stateMode;
QLength moveThreshold;
QAngle turnThreshold;
bool maxVelSetByUser{false}; // Used so motors don't overwrite maxVelocity
double maxVelocity{600};
double maxVoltage{12000};
bool isParentedToCurrentTask{true};
std::shared_ptr<ChassisControllerPID> buildCCPID();
std::shared_ptr<ChassisControllerIntegrated> buildCCI();
std::shared_ptr<DefaultOdomChassisController>
buildDOCC(std::shared_ptr<ChassisController> chassisController);
std::shared_ptr<ChassisModel> makeChassisModel();
std::shared_ptr<SkidSteerModel> makeSkidSteerModel();
std::shared_ptr<XDriveModel> makeXDriveModel();
std::shared_ptr<HDriveModel> makeHDriveModel();
};
} // namespace okapi

177
SerialTest/include/okapi/impl/control/async/asyncMotionProfileControllerBuilder.hpp Normal file
View File

@ -0,0 +1,177 @@
/*
* 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/chassisController.hpp"
#include "okapi/api/control/async/asyncLinearMotionProfileController.hpp"
#include "okapi/api/control/async/asyncMotionProfileController.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/impl/device/motor/motor.hpp"
#include "okapi/impl/device/motor/motorGroup.hpp"
#include "okapi/impl/util/timeUtilFactory.hpp"
namespace okapi {
class AsyncMotionProfileControllerBuilder {
public:
/**
* A builder that creates async motion profile controllers. Use this to build an
* AsyncMotionProfileController or an AsyncLinearMotionProfileController.
*
* @param ilogger The logger this instance will log to.
*/
explicit AsyncMotionProfileControllerBuilder(
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* Sets the output. This must be used with buildLinearMotionProfileController().
*
* @param ioutput The output.
* @param idiameter The diameter of the mechanical part the motor spins.
* @param ipair The gearset.
* @return An ongoing builder.
*/
AsyncMotionProfileControllerBuilder &withOutput(const Motor &ioutput,
const QLength &idiameter,
const AbstractMotor::GearsetRatioPair &ipair);
/**
* Sets the output. This must be used with buildLinearMotionProfileController().
*
* @param ioutput The output.
* @param idiameter The diameter of the mechanical part the motor spins.
* @param ipair The gearset.
* @return An ongoing builder.
*/
AsyncMotionProfileControllerBuilder &withOutput(const MotorGroup &ioutput,
const QLength &idiameter,
const AbstractMotor::GearsetRatioPair &ipair);
/**
* Sets the output. This must be used with buildLinearMotionProfileController().
*
* @param ioutput The output.
* @param idiameter The diameter of the mechanical part the motor spins.
* @param ipair The gearset.
* @return An ongoing builder.
*/
AsyncMotionProfileControllerBuilder &
withOutput(const std::shared_ptr<ControllerOutput<double>> &ioutput,
const QLength &idiameter,
const AbstractMotor::GearsetRatioPair &ipair);
/**
* Sets the output. This must be used with buildMotionProfileController().
*
* @param icontroller The chassis controller to use.
* @return An ongoing builder.
*/
AsyncMotionProfileControllerBuilder &withOutput(ChassisController &icontroller);
/**
* Sets the output. This must be used with buildMotionProfileController().
*
* @param icontroller The chassis controller to use.
* @return An ongoing builder.
*/
AsyncMotionProfileControllerBuilder &
withOutput(const std::shared_ptr<ChassisController> &icontroller);
/**
* Sets the output. This must be used with buildMotionProfileController().
*
* @param imodel The chassis model to use.
* @param iscales The chassis dimensions.
* @param ipair The gearset.
* @return An ongoing builder.
*/
AsyncMotionProfileControllerBuilder &withOutput(const std::shared_ptr<ChassisModel> &imodel,
const ChassisScales &iscales,
const AbstractMotor::GearsetRatioPair &ipair);
/**
* Sets the limits.
*
* @param ilimits The limits.
* @return An ongoing builder.
*/
AsyncMotionProfileControllerBuilder &withLimits(const PathfinderLimits &ilimits);
/**
* Sets the TimeUtilFactory used when building the controller. The default is the static
* TimeUtilFactory.
*
* @param itimeUtilFactory The TimeUtilFactory.
* @return An ongoing builder.
*/
AsyncMotionProfileControllerBuilder &withTimeUtilFactory(const TimeUtilFactory &itimeUtilFactory);
/**
* Sets the logger.
*
* @param ilogger The logger.
* @return An ongoing builder.
*/
AsyncMotionProfileControllerBuilder &withLogger(const std::shared_ptr<Logger> &ilogger);
/**
* Parents the internal tasks started by this builder to the current task, meaning they will be
* deleted once the current task is deleted. The `initialize` and `competition_initialize` tasks
* are never parented to. This is the default behavior.
*
* Read more about this in the [builders and tasks tutorial]
* (docs/tutorials/concepts/builders-and-tasks.md).
*
* @return An ongoing builder.
*/
AsyncMotionProfileControllerBuilder &parentedToCurrentTask();
/**
* Prevents parenting the internal tasks started by this builder to the current task, meaning they
* will not be deleted once the current task is deleted. This can cause runaway tasks, but is
* sometimes the desired behavior (e.x., if you want to use this builder once in `autonomous` and
* then again in `opcontrol`).
*
* Read more about this in the [builders and tasks tutorial]
* (docs/tutorials/concepts/builders-and-tasks.md).
*
* @return An ongoing builder.
*/
AsyncMotionProfileControllerBuilder &notParentedToCurrentTask();
/**
* Builds the AsyncLinearMotionProfileController.
*
* @return A fully built AsyncLinearMotionProfileController.
*/
std::shared_ptr<AsyncLinearMotionProfileController> buildLinearMotionProfileController();
/**
* Builds the AsyncMotionProfileController.
*
* @return A fully built AsyncMotionProfileController.
*/
std::shared_ptr<AsyncMotionProfileController> buildMotionProfileController();
private:
std::shared_ptr<Logger> logger;
bool hasLimits{false};
PathfinderLimits limits;
bool hasOutput{false};
std::shared_ptr<ControllerOutput<double>> output;
QLength diameter;
bool hasModel{false};
std::shared_ptr<ChassisModel> model;
ChassisScales scales{{1, 1}, imev5GreenTPR};
AbstractMotor::GearsetRatioPair pair{AbstractMotor::gearset::invalid};
TimeUtilFactory timeUtilFactory = TimeUtilFactory();
std::shared_ptr<Logger> controllerLogger = Logger::getDefaultLogger();
bool isParentedToCurrentTask{true};
};
} // namespace okapi

190
SerialTest/include/okapi/impl/control/async/asyncPosControllerBuilder.hpp Normal file
View File

@ -0,0 +1,190 @@
/*
* 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/asyncPosIntegratedController.hpp"
#include "okapi/api/control/async/asyncPosPidController.hpp"
#include "okapi/api/control/async/asyncPositionController.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/impl/device/motor/motor.hpp"
#include "okapi/impl/device/motor/motorGroup.hpp"
#include "okapi/impl/device/rotarysensor/adiEncoder.hpp"
#include "okapi/impl/device/rotarysensor/integratedEncoder.hpp"
#include "okapi/impl/util/timeUtilFactory.hpp"
namespace okapi {
class AsyncPosControllerBuilder {
public:
/**
* A builder that creates async position controllers. Use this to create an
* AsyncPosIntegratedController or an AsyncPosPIDController.
*
* @param ilogger The logger this instance will log to.
*/
explicit AsyncPosControllerBuilder(
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* Sets the motor.
*
* @param imotor The motor.
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &withMotor(const Motor &imotor);
/**
* Sets the motor.
*
* @param imotor The motor.
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &withMotor(const MotorGroup &imotor);
/**
* Sets the motor.
*
* @param imotor The motor.
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &withMotor(const std::shared_ptr<AbstractMotor> &imotor);
/**
* Sets the sensor. The default sensor is the motor's integrated encoder.
*
* @param isensor The sensor.
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &withSensor(const ADIEncoder &isensor);
/**
* Sets the sensor. The default sensor is the motor's integrated encoder.
*
* @param isensor The sensor.
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &withSensor(const IntegratedEncoder &isensor);
/**
* Sets the sensor. The default sensor is the motor's integrated encoder.
*
* @param isensor The sensor.
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &withSensor(const std::shared_ptr<RotarySensor> &isensor);
/**
* Sets the controller gains, causing the builder to generate an AsyncPosPIDController. This does
* not set the integrated control's gains.
*
* @param igains The gains.
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &withGains(const IterativePosPIDController::Gains &igains);
/**
* Sets the derivative filter which filters the derivative term before it is scaled by kD. The
* filter is ignored when using integrated control. The default derivative filter is a
* PassthroughFilter.
*
* @param iderivativeFilter The derivative filter.
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &withDerivativeFilter(std::unique_ptr<Filter> iderivativeFilter);
/**
* Sets the gearset. The default gearset is derived from the motor's.
*
* @param igearset The gearset.
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &withGearset(const AbstractMotor::GearsetRatioPair &igearset);
/**
* Sets the maximum velocity. The default maximum velocity is derived from the motor's gearset.
* This parameter is ignored when using an AsyncPosPIDController.
*
* @param imaxVelocity The maximum velocity.
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &withMaxVelocity(double imaxVelocity);
/**
* Sets the TimeUtilFactory used when building the controller. The default is the static
* TimeUtilFactory.
*
* @param itimeUtilFactory The TimeUtilFactory.
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &withTimeUtilFactory(const TimeUtilFactory &itimeUtilFactory);
/**
* Sets the logger.
*
* @param ilogger The logger.
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &withLogger(const std::shared_ptr<Logger> &ilogger);
/**
* Parents the internal tasks started by this builder to the current task, meaning they will be
* deleted once the current task is deleted. The `initialize` and `competition_initialize` tasks
* are never parented to. This is the default behavior.
*
* Read more about this in the [builders and tasks tutorial]
* (docs/tutorials/concepts/builders-and-tasks.md).
*
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &parentedToCurrentTask();
/**
* Prevents parenting the internal tasks started by this builder to the current task, meaning they
* will not be deleted once the current task is deleted. This can cause runaway tasks, but is
* sometimes the desired behavior (e.x., if you want to use this builder once in `autonomous` and
* then again in `opcontrol`).
*
* Read more about this in the [builders and tasks tutorial]
* (docs/tutorials/concepts/builders-and-tasks.md).
*
* @return An ongoing builder.
*/
AsyncPosControllerBuilder &notParentedToCurrentTask();
/**
* Builds the AsyncPositionController. Throws a std::runtime_exception is no motors were set.
*
* @return A fully built AsyncPositionController.
*/
std::shared_ptr<AsyncPositionController<double, double>> build();
private:
std::shared_ptr<Logger> logger;
bool hasMotors{false}; // Used to verify motors were passed
std::shared_ptr<AbstractMotor> motor;
bool sensorsSetByUser{false}; // Used so motors don't overwrite sensors set manually
std::shared_ptr<RotarySensor> sensor;
bool hasGains{false}; // Whether gains were passed, no gains means integrated control
IterativePosPIDController::Gains gains;
std::unique_ptr<Filter> derivativeFilter = std::make_unique<PassthroughFilter>();
bool gearsetSetByUser{false}; // Used so motor's don't overwrite a gearset set manually
AbstractMotor::GearsetRatioPair pair{AbstractMotor::gearset::invalid};
bool maxVelSetByUser{false}; // Used so motors don't overwrite maxVelocity
double maxVelocity{600};
TimeUtilFactory timeUtilFactory = TimeUtilFactory();
std::shared_ptr<Logger> controllerLogger = Logger::getDefaultLogger();
bool isParentedToCurrentTask{true};
std::shared_ptr<AsyncPosIntegratedController> buildAPIC();
std::shared_ptr<AsyncPosPIDController> buildAPPC();
};
} // namespace okapi

203
SerialTest/include/okapi/impl/control/async/asyncVelControllerBuilder.hpp Normal file
View File

@ -0,0 +1,203 @@
/*
* 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/asyncVelIntegratedController.hpp"
#include "okapi/api/control/async/asyncVelPidController.hpp"
#include "okapi/api/control/async/asyncVelocityController.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/impl/device/motor/motor.hpp"
#include "okapi/impl/device/motor/motorGroup.hpp"
#include "okapi/impl/device/rotarysensor/adiEncoder.hpp"
#include "okapi/impl/device/rotarysensor/integratedEncoder.hpp"
#include "okapi/impl/util/timeUtilFactory.hpp"
namespace okapi {
class AsyncVelControllerBuilder {
public:
/**
* A builder that creates async velocity controllers. Use this to create an
* AsyncVelIntegratedController or an AsyncVelPIDController.
*
* @param ilogger The logger this instance will log to.
*/
explicit AsyncVelControllerBuilder(
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* Sets the motor.
*
* @param imotor The motor.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withMotor(const Motor &imotor);
/**
* Sets the motor.
*
* @param imotor The motor.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withMotor(const MotorGroup &imotor);
/**
* Sets the motor.
*
* @param imotor The motor.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withMotor(const std::shared_ptr<AbstractMotor> &imotor);
/**
* Sets the sensor. The default sensor is the motor's integrated encoder.
*
* @param isensor The sensor.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withSensor(const ADIEncoder &isensor);
/**
* Sets the sensor. The default sensor is the motor's integrated encoder.
*
* @param isensor The sensor.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withSensor(const IntegratedEncoder &isensor);
/**
* Sets the sensor. The default sensor is the motor's integrated encoder.
*
* @param isensor The sensor.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withSensor(const std::shared_ptr<RotarySensor> &isensor);
/**
* Sets the controller gains, causing the builder to generate an AsyncVelPIDController. This does
* not set the integrated control's gains.
*
* @param igains The gains.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withGains(const IterativeVelPIDController::Gains &igains);
/**
* Sets the VelMath which calculates and filters velocity. This is ignored when using integrated
* controller. If using a PID controller (by setting the gains), this is required.
*
* @param ivelMath The VelMath.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withVelMath(std::unique_ptr<VelMath> ivelMath);
/**
* Sets the derivative filter which filters the derivative term before it is scaled by kD. The
* filter is ignored when using integrated control. The default derivative filter is a
* PassthroughFilter.
*
* @param iderivativeFilter The derivative filter.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withDerivativeFilter(std::unique_ptr<Filter> iderivativeFilter);
/**
* Sets the gearset. The default gearset is derived from the motor's.
*
* @param igearset The gearset.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withGearset(const AbstractMotor::GearsetRatioPair &igearset);
/**
* Sets the maximum velocity. The default maximum velocity is derived from the motor's gearset.
* This parameter is ignored when using an AsyncVelPIDController.
*
* @param imaxVelocity The maximum velocity.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withMaxVelocity(double imaxVelocity);
/**
* Sets the TimeUtilFactory used when building the controller. The default is the static
* TimeUtilFactory.
*
* @param itimeUtilFactory The TimeUtilFactory.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withTimeUtilFactory(const TimeUtilFactory &itimeUtilFactory);
/**
* Sets the logger.
*
* @param ilogger The logger.
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &withLogger(const std::shared_ptr<Logger> &ilogger);
/**
* Parents the internal tasks started by this builder to the current task, meaning they will be
* deleted once the current task is deleted. The `initialize` and `competition_initialize` tasks
* are never parented to. This is the default behavior.
*
* Read more about this in the [builders and tasks tutorial]
* (docs/tutorials/concepts/builders-and-tasks.md).
*
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &parentedToCurrentTask();
/**
* Prevents parenting the internal tasks started by this builder to the current task, meaning they
* will not be deleted once the current task is deleted. This can cause runaway tasks, but is
* sometimes the desired behavior (e.x., if you want to use this builder once in `autonomous` and
* then again in `opcontrol`).
*
* Read more about this in the [builders and tasks tutorial]
* (docs/tutorials/concepts/builders-and-tasks.md).
*
* @return An ongoing builder.
*/
AsyncVelControllerBuilder &notParentedToCurrentTask();
/**
* Builds the AsyncVelocityController. Throws a std::runtime_exception is no motors were set.
*
* @return A fully built AsyncVelocityController.
*/
std::shared_ptr<AsyncVelocityController<double, double>> build();
private:
std::shared_ptr<Logger> logger;
bool hasMotors{false}; // Used to verify motors were passed
std::shared_ptr<AbstractMotor> motor;
bool sensorsSetByUser{false}; // Used so motors don't overwrite sensors set manually
std::shared_ptr<RotarySensor> sensor;
bool hasGains{false}; // Whether gains were passed, no gains means integrated control
IterativeVelPIDController::Gains gains;
bool hasVelMath{false}; // Used to verify velMath was passed
std::unique_ptr<VelMath> velMath;
std::unique_ptr<Filter> derivativeFilter = std::make_unique<PassthroughFilter>();
bool gearsetSetByUser{false}; // Used so motor's don't overwrite a gearset set manually
AbstractMotor::GearsetRatioPair pair{AbstractMotor::gearset::invalid};
bool maxVelSetByUser{false}; // Used so motors don't overwrite maxVelocity
double maxVelocity{600};
TimeUtilFactory timeUtilFactory = TimeUtilFactory();
std::shared_ptr<Logger> controllerLogger = Logger::getDefaultLogger();
bool isParentedToCurrentTask{true};
std::shared_ptr<AsyncVelIntegratedController> buildAVIC();
std::shared_ptr<AsyncVelPIDController> buildAVPC();
};
} // namespace okapi

120
SerialTest/include/okapi/impl/control/iterative/iterativeControllerFactory.hpp Normal file
View File

@ -0,0 +1,120 @@
/*
* 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/iterativeMotorVelocityController.hpp"
#include "okapi/api/control/iterative/iterativePosPidController.hpp"
#include "okapi/api/control/iterative/iterativeVelPidController.hpp"
#include "okapi/api/util/mathUtil.hpp"
#include "okapi/impl/device/motor/motor.hpp"
#include "okapi/impl/device/motor/motorGroup.hpp"
#include "okapi/impl/filter/velMathFactory.hpp"
namespace okapi {
class IterativeControllerFactory {
public:
/**
* Position PID controller.
*
* @param ikP proportional gain
* @param ikI integral gain
* @param ikD derivative gain
* @param ikBias controller bias (constant offset added to the output)
* @param iderivativeFilter A filter for filtering the derivative term.
* @param ilogger The logger this instance will log to.
*/
static IterativePosPIDController
posPID(double ikP,
double ikI,
double ikD,
double ikBias = 0,
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* Velocity PD controller.
*
* @param ikP proportional gain
* @param ikD derivative gain
* @param ikF feed-forward gain
* @param ikSF a feed-forward gain to counteract static friction
* @param iderivativeFilter A filter for filtering the derivative term.
* @param ilogger The logger this instance will log to.
*/
static IterativeVelPIDController
velPID(double ikP,
double ikD,
double ikF = 0,
double ikSF = 0,
std::unique_ptr<VelMath> ivelMath = VelMathFactory::createPtr(imev5GreenTPR),
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* Velocity PD controller that automatically writes to the motor.
*
* @param imotor output motor
* @param ikP proportional gain
* @param ikD derivative gain
* @param ikF feed-forward gain
* @param ikSF a feed-forward gain to counteract static friction
* @param ivelMath The VelMath.
* @param iderivativeFilter A filter for filtering the derivative term.
* @param ilogger The logger this instance will log to.
*/
static IterativeMotorVelocityController
motorVelocity(Motor imotor,
double ikP,
double ikD,
double ikF = 0,
double ikSF = 0,
std::unique_ptr<VelMath> ivelMath = VelMathFactory::createPtr(imev5GreenTPR),
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* Velocity PD controller that automatically writes to the motor.
*
* @param imotor output motor
* @param ikP proportional gain
* @param ikD derivative gain
* @param ikF feed-forward gain
* @param ikSF a feed-forward gain to counteract static friction
* @param ivelMath The VelMath.
* @param iderivativeFilter A filter for filtering the derivative term.
* @param ilogger The logger this instance will log to.
*/
static IterativeMotorVelocityController
motorVelocity(MotorGroup imotor,
double ikP,
double ikD,
double ikF = 0,
double ikSF = 0,
std::unique_ptr<VelMath> ivelMath = VelMathFactory::createPtr(imev5GreenTPR),
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
/**
* Velocity PD controller that automatically writes to the motor.
*
* @param imotor output motor
* @param icontroller controller to use
*/
static IterativeMotorVelocityController
motorVelocity(Motor imotor,
std::shared_ptr<IterativeVelocityController<double, double>> icontroller);
/**
* Velocity PD controller that automatically writes to the motor.
*
* @param imotor output motor
* @param icontroller controller to use
*/
static IterativeMotorVelocityController
motorVelocity(MotorGroup imotor,
std::shared_ptr<IterativeVelocityController<double, double>> icontroller);
};
} // namespace okapi

25
SerialTest/include/okapi/impl/control/util/controllerRunnerFactory.hpp Normal file
View File

@ -0,0 +1,25 @@
/*
* 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/util/controllerRunner.hpp"
#include "okapi/impl/util/timeUtilFactory.hpp"
namespace okapi {
template <typename Input, typename Output> class ControllerRunnerFactory {
public:
/**
* A utility class that runs a closed-loop controller.
*
* @param ilogger The logger this instance will log to.
* @return
*/
static ControllerRunner<Input, Output>
create(const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger()) {
return ControllerRunner<Input, Output>(TimeUtilFactory::createDefault(), ilogger);
}
};
} // namespace okapi

47
SerialTest/include/okapi/impl/control/util/pidTunerFactory.hpp Normal file
View File

@ -0,0 +1,47 @@
/*
* 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/util/pidTuner.hpp"
#include <memory>
namespace okapi {
class PIDTunerFactory {
public:
static PIDTuner create(const std::shared_ptr<ControllerInput<double>> &iinput,
const std::shared_ptr<ControllerOutput<double>> &ioutput,
QTime itimeout,
std::int32_t igoal,
double ikPMin,
double ikPMax,
double ikIMin,
double ikIMax,
double ikDMin,
double ikDMax,
std::int32_t inumIterations = 5,
std::int32_t inumParticles = 16,
double ikSettle = 1,
double ikITAE = 2,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
static std::unique_ptr<PIDTuner>
createPtr(const std::shared_ptr<ControllerInput<double>> &iinput,
const std::shared_ptr<ControllerOutput<double>> &ioutput,
QTime itimeout,
std::int32_t igoal,
double ikPMin,
double ikPMax,
double ikIMin,
double ikIMax,
double ikDMin,
double ikDMax,
std::int32_t inumIterations = 5,
std::int32_t inumParticles = 16,
double ikSettle = 1,
double ikITAE = 2,
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
};
} // namespace okapi

71
SerialTest/include/okapi/impl/device/adiUltrasonic.hpp Normal file
View File

@ -0,0 +1,71 @@
/*
* 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 "api.h"
#include "okapi/api/control/controllerInput.hpp"
#include "okapi/api/filter/passthroughFilter.hpp"
#include <memory>
namespace okapi {
class ADIUltrasonic : public ControllerInput<double> {
public:
/**
* An ultrasonic sensor in the ADI (3-wire) ports.
*
* ```cpp
* auto ultra = ADIUltrasonic('A', 'B');
* auto filteredUltra = ADIUltrasonic('A', 'B', std::make_unique<MedianFilter<5>>());
* ```
*
* @param iportPing The port connected to the orange OUTPUT cable. This must be in port ``1``,
* ``3``, ``5``, or ``7`` (``A``, ``C``, ``E``, or ``G``).
* @param iportEcho The port connected to the yellow INPUT cable. This must be in the next highest
* port following iportPing.
* @param ifilter The filter to use for filtering the distance measurements.
*/
ADIUltrasonic(std::uint8_t iportPing,
std::uint8_t iportEcho,
std::unique_ptr<Filter> ifilter = std::make_unique<PassthroughFilter>());
/**
* An ultrasonic sensor in the ADI (3-wire) ports.
*
* ```cpp
* auto ultra = ADIUltrasonic({1, 'A', 'B'});
* auto filteredUltra = ADIUltrasonic({1, 'A', 'B'}, std::make_unique<MedianFilter<5>>());
* ```
*
* @param iports The ports the ultrasonic is plugged in to in the order ``{smart port, ping port,
* echo port}``. The smart port is the smart port number (``[1, 21]``). The ping port is the port
* connected to the orange OUTPUT cable. This must be in port ``1``, ``3``, ``5``, or ``7``
* (``A``, ``C``, ``E``, or ``G``). The echo port is the port connected to the yellow INPUT cable.
* This must be in the next highest port following the ping port.
* @param ifilter The filter to use for filtering the distance measurements.
*/
ADIUltrasonic(std::tuple<std::uint8_t, std::uint8_t, std::uint8_t> iports,
std::unique_ptr<Filter> ifilter = std::make_unique<PassthroughFilter>());
virtual ~ADIUltrasonic();
/**
* Returns the current filtered sensor value.
*
* @return current value
*/
virtual double get();
/**
* Get the sensor value for use in a control loop. This method might be automatically called in
* another thread by the controller. Calls get().
*/
virtual double controllerGet() override;
protected:
pros::c::ext_adi_ultrasonic_t ultra;
std::unique_ptr<Filter> filter;
};
} // namespace okapi

50
SerialTest/include/okapi/impl/device/button/adiButton.hpp Normal file
View File

@ -0,0 +1,50 @@
/*
* 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 "api.h"
#include "okapi/api/device/button/buttonBase.hpp"
namespace okapi {
class ADIButton : public ButtonBase {
public:
/**
* A button in an ADI port.
*
* ```cpp
* auto btn = ADIButton('A', false);
* auto invertedBtn = ADIButton('A', true);
* ```
*
* @param iport The ADI port number (``[1, 8]``, ``[a, h]``, ``[A, H]``).
* @param iinverted Whether the button is inverted (``true`` meaning default pressed and ``false``
* meaning default not pressed).
*/
ADIButton(std::uint8_t iport, bool iinverted = false);
/**
* A button in an ADI port.
*
* ```cpp
* auto btn = ADIButton({1, 'A'}, false);
* auto invertedBtn = ADIButton({1, 'A'}, true);
* ```
*
* @param iports The ports the button is plugged in to in the order ``{smart port, button port}``.
* The smart port is the smart port number (``[1, 21]``). The button port is the ADI port number
* (``[1, 8]``, ``[a, h]``, ``[A, H]``).
* @param iinverted Whether the button is inverted (``true`` meaning default pressed and ``false``
* meaning default not pressed).
*/
ADIButton(std::pair<std::uint8_t, std::uint8_t> iports, bool iinverted = false);
protected:
std::uint8_t smartPort;
std::uint8_t port;
virtual bool currentlyPressed() override;
};
} // namespace okapi

38
SerialTest/include/okapi/impl/device/button/controllerButton.hpp Normal file
View File

@ -0,0 +1,38 @@
/*
* 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 "api.h"
#include "okapi/api/device/button/buttonBase.hpp"
#include "okapi/impl/device/controllerUtil.hpp"
namespace okapi {
class ControllerButton : public ButtonBase {
public:
/**
* A button on a Controller.
*
* @param ibtn The button id.
* @param iinverted Whether the button is inverted (default pressed instead of default released).
*/
ControllerButton(ControllerDigital ibtn, bool iinverted = false);
/**
* A button on a Controller.
*
* @param icontroller The Controller the button is on.
* @param ibtn The button id.
* @param iinverted Whether the button is inverted (default pressed instead of default released).
*/
ControllerButton(ControllerId icontroller, ControllerDigital ibtn, bool iinverted = false);
protected:
pros::controller_id_e_t id;
pros::controller_digital_e_t btn;
virtual bool currentlyPressed() override;
};
} // namespace okapi

118
SerialTest/include/okapi/impl/device/controller.hpp Normal file
View File

@ -0,0 +1,118 @@
/*
* 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 "api.h"
#include "okapi/impl/device/button/controllerButton.hpp"
#include "okapi/impl/device/controllerUtil.hpp"
#include <array>
namespace okapi {
class Controller {
public:
Controller(ControllerId iid = ControllerId::master);
virtual ~Controller();
/**
* Returns whether the controller is connected.
*
* @return true if the controller is connected
*/
virtual bool isConnected();
/**
* Returns the current analog reading for the channel in the range [-1, 1]. Returns 0 if the
* controller is not connected.
*
* @param ichannel the channel to read
* @return the value of that channel in the range [-1, 1]
*/
virtual float getAnalog(ControllerAnalog ichannel);
/**
* Returns whether the digital button is currently pressed. Returns false if the controller is
* not connected.
*
* @param ibutton the button to check
* @return true if the button is pressed, false if the controller is not connected
*/
virtual bool getDigital(ControllerDigital ibutton);
/**
* Returns a ControllerButton for the given button on this controller.
*
* @param ibtn the button
* @return a ControllerButton on this controller
*/
virtual ControllerButton &operator[](ControllerDigital ibtn);
/**
* Sets text to the controller LCD screen.
*
* @param iline the line number in the range [0-2] at which the text will be displayed
* @param icol the column number in the range [0-14] at which the text will be displayed
* @param itext the string to display
* @return 1 if the operation was successful, PROS_ERR otherwise
*/
virtual std::int32_t setText(std::uint8_t iline, std::uint8_t icol, std::string itext);
/**
* Clears all of the lines of the controller screen. On vexOS version 1.0.0 this function will
* block for 110ms.
*
* @return 1 if the operation was successful, PROS_ERR otherwise
*/
virtual std::int32_t clear();
/**
* Clears an individual line of the controller screen.
*
* @param iline the line number to clear in the range [0, 2].
* @return 1 if the operation was successful, PROS_ERR otherwise
*/
virtual std::int32_t clearLine(std::uint8_t iline);
/**
* Rumble the controller.
*
* Controller rumble activation is currently in beta, so continuous, fast
* updates will not work well.
*
* @param irumblePattern A string consisting of the characters '.', '-', and ' ', where dots are
* short rumbles, dashes are long rumbles, and spaces are pauses. Maximum supported length is 8
* characters.
*
* @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
*/
virtual std::int32_t rumble(std::string irumblePattern);
/**
* Gets the battery capacity of the given controller.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the controller port.
*
* @return the controller's battery capacity
*/
virtual std::int32_t getBatteryCapacity();
/**
* Gets the battery level of the given controller.
*
* This function uses the following values of errno when an error state is reached:
* EACCES - Another resource is currently trying to access the controller port.
*
* @return the controller's battery level
*/
virtual std::int32_t getBatteryLevel();
protected:
ControllerId okapiId;
pros::controller_id_e_t prosId;
std::array<ControllerButton *, 12> buttonArray{};
};
} // namespace okapi

64
SerialTest/include/okapi/impl/device/controllerUtil.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 "api.h"
namespace okapi {
/**
* Which controller role this has.
*/
enum class ControllerId {
master = 0, ///< master
partner = 1 ///< partner
};
/**
* The analog sticks.
*/
enum class ControllerAnalog {
leftX = 0, ///< leftX
leftY = 1, ///< leftY
rightX = 2, ///< rightX
rightY = 3 ///< rightY
};
/**
* Various buttons.
*/
enum class ControllerDigital {
L1 = 6, ///< L1
L2 = 7, ///< L2
R1 = 8, ///< R1
R2 = 9, ///< R2
up = 10, ///< up
down = 11, ///< down
left = 12, ///< left
right = 13, ///< right
X = 14, ///< X
B = 15, ///< B
Y = 16, ///< Y
A = 17 ///< A
};
class ControllerUtil {
public:
/**
* Maps an `id` to the PROS enum equivalent.
*/
static pros::controller_id_e_t idToProsEnum(ControllerId in);
/**
* Maps an `analog` to the PROS enum equivalent.
*/
static pros::controller_analog_e_t analogToProsEnum(ControllerAnalog in);
/**
* Maps a `digital` to the PROS enum equivalent.
*/
static pros::controller_digital_e_t digitalToProsEnum(ControllerDigital in);
};
} // namespace okapi

76
SerialTest/include/okapi/impl/device/distanceSensor.hpp Normal file
View File

@ -0,0 +1,76 @@
/*
* 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 "api.h"
#include "okapi/api/control/controllerInput.hpp"
#include "okapi/api/filter/passthroughFilter.hpp"
#include <memory>
namespace okapi {
class DistanceSensor : public ControllerInput<double> {
public:
/**
* A distance sensor on a V5 port.
*
* ```cpp
* auto ds = DistanceSensor(1);
* auto filteredDistSensor = DistanceSensor(1, std::make_unique<MedianFilter<5>>());
* ```
*
* @param iport The V5 port the device uses.
* @param ifilter The filter to use for filtering the distance measurements.
*/
DistanceSensor(std::uint8_t iport,
std::unique_ptr<Filter> ifilter = std::make_unique<PassthroughFilter>());
virtual ~DistanceSensor() = default;
/**
* Get the current filtered sensor value in mm.
*
* @return The current filtered sensor value in mm.
*/
double get();
/**
* Get the sensor value for use in a control loop. This method might be automatically called in
* another thread by the controller.
*
* @return The same as [get](@ref okapi::DistanceSensor::get).
*/
double controllerGet() override;
/**
* Get the confidence in the distance reading. This value has a range of ``[0, 63]``. ``63`` means
* high confidence, lower values imply less confidence. Confidence is only available when distance
* is greater than ``200`` mm.
*
* @return The confidence value in the range ``[0, 63]``.
*/
std::int32_t getConfidence() const;
/**
* Get the current guess at relative object size. This value has a range of ``[0, 400]``. A 18" x
* 30" grey card will return a value of approximately ``75`` in typical room lighting.
*
* @return The size value in the range ``[0, 400]`` or ``PROS_ERR`` if the operation failed,
* setting errno.
*/
std::int32_t getObjectSize() const;
/**
* Get the object velocity in m/s.
*
* @return The object velocity in m/s.
*/
double getObjectVelocity() const;
protected:
std::uint8_t port;
std::unique_ptr<Filter> filter;
};
} // namespace okapi

69
SerialTest/include/okapi/impl/device/motor/adiMotor.hpp Normal file
View File

@ -0,0 +1,69 @@
/*
* 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 "api.h"
#include "okapi/api/control/controllerOutput.hpp"
#include "okapi/api/util/logging.hpp"
namespace okapi {
class ADIMotor : public ControllerOutput<double> {
public:
/**
* A motor in an ADI port.
*
* ```cpp
* auto mtr = ADIMotor('A');
* auto reversedMtr = ADIMotor('A', true);
* ```
*
* @param iport The ADI port number (``[1, 8]``, ``[a, h]``, ``[A, H]``).
* @param ireverse Whether the motor is reversed.
* @param logger The logger that initialization warnings will be logged to.
*/
ADIMotor(std::uint8_t iport,
bool ireverse = false,
const std::shared_ptr<Logger> &logger = Logger::getDefaultLogger());
/**
* A motor in an ADI port.
*
* ```cpp
* auto mtr = ADIMotor({1, 'A'}, false);
* auto reversedMtr = ADIMotor({1, 'A'}, true);
* ```
*
* @param iports The ports the motor is plugged in to in the order ``{smart port, motor port}``.
* The smart port is the smart port number (``[1, 21]``). The motor port is the ADI port number
* (``[1, 8]``, ``[a, h]``, ``[A, H]``).
* @param ireverse Whether the motor is reversed.
* @param logger The logger that initialization warnings will be logged to.
*/
ADIMotor(std::pair<std::uint8_t, std::uint8_t> iports,
bool ireverse = false,
const std::shared_ptr<Logger> &logger = Logger::getDefaultLogger());
/**
* Set the voltage to the motor.
*
* @param ivoltage voltage in the range [-127, 127].
*/
virtual void moveVoltage(std::int8_t ivoltage) const;
/**
* 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::uint8_t smartPort;
std::uint8_t port;
std::int8_t reversed;
};
} // namespace okapi

Some files were not shown because too many files have changed in this diff Show More