Initial commit - test serial
This commit is contained in:
Cole A. Deck
134
SerialTest/include/okapi/api.hpp
Normal file
134
SerialTest/include/okapi/api.hpp
Normal 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"
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
165
SerialTest/include/okapi/api/chassis/model/chassisModel.hpp
Normal 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
247
SerialTest/include/okapi/api/chassis/model/hDriveModel.hpp
Normal 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
|
||||
@@ -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
198
SerialTest/include/okapi/api/chassis/model/skidSteerModel.hpp
Normal 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
|
||||
@@ -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
|
||||
@@ -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
273
SerialTest/include/okapi/api/chassis/model/xDriveModel.hpp
Normal 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
|
||||
@@ -0,0 +1,24 @@
|
||||
/*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "okapi/api/control/closedLoopController.hpp"
|
||||
|
||||
namespace okapi {
|
||||
/**
|
||||
* Closed-loop controller that steps on its own in another thread and automatically writes to the
|
||||
* output.
|
||||
*/
|
||||
template <typename Input, typename Output>
|
||||
class AsyncController : public ClosedLoopController<Input, Output> {
|
||||
public:
|
||||
/**
|
||||
* Blocks the current task until the controller has settled. Determining what settling means is
|
||||
* implementation-dependent.
|
||||
*/
|
||||
virtual void waitUntilSettled() = 0;
|
||||
};
|
||||
} // namespace okapi
|
||||
@@ -0,0 +1,291 @@
|
||||
/*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "okapi/api/control/async/asyncPositionController.hpp"
|
||||
#include "okapi/api/control/util/pathfinderUtil.hpp"
|
||||
#include "okapi/api/device/motor/abstractMotor.hpp"
|
||||
#include "okapi/api/units/QAngularSpeed.hpp"
|
||||
#include "okapi/api/units/QSpeed.hpp"
|
||||
#include "okapi/api/util/logging.hpp"
|
||||
#include "okapi/api/util/timeUtil.hpp"
|
||||
#include <atomic>
|
||||
#include <map>
|
||||
|
||||
#include "squiggles.hpp"
|
||||
|
||||
namespace okapi {
|
||||
class AsyncLinearMotionProfileController : public AsyncPositionController<std::string, double> {
|
||||
public:
|
||||
/**
|
||||
* An Async Controller which generates and follows 1D motion profiles.
|
||||
*
|
||||
* @param itimeUtil The TimeUtil.
|
||||
* @param ilimits The default limits.
|
||||
* @param ioutput The output to write velocity targets to.
|
||||
* @param idiameter The effective diameter for whatever the motor spins.
|
||||
* @param ipair The gearset.
|
||||
* @param ilogger The logger this instance will log to.
|
||||
*/
|
||||
AsyncLinearMotionProfileController(
|
||||
const TimeUtil &itimeUtil,
|
||||
const PathfinderLimits &ilimits,
|
||||
const std::shared_ptr<ControllerOutput<double>> &ioutput,
|
||||
const QLength &idiameter,
|
||||
const AbstractMotor::GearsetRatioPair &ipair,
|
||||
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
|
||||
|
||||
AsyncLinearMotionProfileController(AsyncLinearMotionProfileController &&other) = delete;
|
||||
|
||||
AsyncLinearMotionProfileController &
|
||||
operator=(AsyncLinearMotionProfileController &&other) = delete;
|
||||
|
||||
~AsyncLinearMotionProfileController() override;
|
||||
|
||||
/**
|
||||
* Generates a path which intersects the given waypoints and saves it internally with a key of
|
||||
* pathId. Call `executePath()` with the same `pathId` to run it.
|
||||
*
|
||||
* If the waypoints form a path which is impossible to achieve, an instance of
|
||||
* `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
|
||||
* are no waypoints, no path is generated.
|
||||
*
|
||||
* @param iwaypoints The waypoints to hit on the path.
|
||||
* @param ipathId A unique identifier to save the path with.
|
||||
*/
|
||||
void generatePath(std::initializer_list<QLength> iwaypoints, const std::string &ipathId);
|
||||
|
||||
/**
|
||||
* Generates a path which intersects the given waypoints and saves it internally with a key of
|
||||
* pathId. Call `executePath()` with the same pathId to run it.
|
||||
*
|
||||
* If the waypoints form a path which is impossible to achieve, an instance of
|
||||
* `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
|
||||
* are no waypoints, no path is generated.
|
||||
*
|
||||
* @param iwaypoints The waypoints to hit on the path.
|
||||
* @param ipathId A unique identifier to save the path with.
|
||||
* @param ilimits The limits to use for this path only.
|
||||
*/
|
||||
void generatePath(std::initializer_list<QLength> iwaypoints,
|
||||
const std::string &ipathId,
|
||||
const PathfinderLimits &ilimits);
|
||||
|
||||
/**
|
||||
* Removes a path and frees the memory it used. This function returns `true` if the path was
|
||||
* either deleted or didn't exist in the first place. It returns `false` if the path could not be
|
||||
* removed because it is running.
|
||||
*
|
||||
* @param ipathId A unique identifier for the path, previously passed to generatePath()
|
||||
* @return `true` if the path no longer exists
|
||||
*/
|
||||
bool removePath(const std::string &ipathId);
|
||||
|
||||
/**
|
||||
* Gets the identifiers of all paths saved in this `AsyncMotionProfileController`.
|
||||
*
|
||||
* @return The identifiers of all paths
|
||||
*/
|
||||
std::vector<std::string> getPaths();
|
||||
|
||||
/**
|
||||
* Executes a path with the given ID. If there is no path matching the ID, the method will
|
||||
* return. Any targets set while a path is being followed will be ignored.
|
||||
*
|
||||
* @param ipathId A unique identifier for the path, previously passed to `generatePath()`.
|
||||
*/
|
||||
void setTarget(std::string ipathId) override;
|
||||
|
||||
/**
|
||||
* Executes a path with the given ID. If there is no path matching the ID, the method will
|
||||
* return. Any targets set while a path is being followed will be ignored.
|
||||
*
|
||||
* @param ipathId A unique identifier for the path, previously passed to `generatePath()`.
|
||||
* @param ibackwards Whether to follow the profile backwards.
|
||||
*/
|
||||
void setTarget(std::string ipathId, bool ibackwards);
|
||||
|
||||
/**
|
||||
* Writes the value of the controller output. This method might be automatically called in another
|
||||
* thread by the controller.
|
||||
*
|
||||
* This just calls `setTarget()`.
|
||||
*/
|
||||
void controllerSet(std::string ivalue) override;
|
||||
|
||||
/**
|
||||
* Gets the last set target, or the default target if none was set.
|
||||
*
|
||||
* @return the last target
|
||||
*/
|
||||
std::string getTarget() override;
|
||||
|
||||
/**
|
||||
* Gets the last set target, or the default target if none was set.
|
||||
*
|
||||
* @return the last target
|
||||
*/
|
||||
virtual std::string getTarget() const;
|
||||
|
||||
/**
|
||||
* This is overridden to return the current path.
|
||||
*
|
||||
* @return The most recent value of the process variable.
|
||||
*/
|
||||
std::string getProcessValue() const override;
|
||||
|
||||
/**
|
||||
* Blocks the current task until the controller has settled. This controller is settled when
|
||||
* it has finished following a path. If no path is being followed, it is settled.
|
||||
*/
|
||||
void waitUntilSettled() override;
|
||||
|
||||
/**
|
||||
* Generates a new path from the position (typically the current position) to the target and
|
||||
* blocks until the controller has settled. Does not save the path which was generated.
|
||||
*
|
||||
* @param iposition The starting position.
|
||||
* @param itarget The target position.
|
||||
* @param ibackwards Whether to follow the profile backwards.
|
||||
*/
|
||||
void moveTo(const QLength &iposition, const QLength &itarget, bool ibackwards = false);
|
||||
|
||||
/**
|
||||
* Generates a new path from the position (typically the current position) to the target and
|
||||
* blocks until the controller has settled. Does not save the path which was generated.
|
||||
*
|
||||
* @param iposition The starting position.
|
||||
* @param itarget The target position.
|
||||
* @param ilimits The limits to use for this path only.
|
||||
* @param ibackwards Whether to follow the profile backwards.
|
||||
*/
|
||||
void moveTo(const QLength &iposition,
|
||||
const QLength &itarget,
|
||||
const PathfinderLimits &ilimits,
|
||||
bool ibackwards = false);
|
||||
|
||||
/**
|
||||
* Returns the last error of the controller. Does not update when disabled. Returns zero if there
|
||||
* is no path currently being followed.
|
||||
*
|
||||
* @return the last error
|
||||
*/
|
||||
double getError() const override;
|
||||
|
||||
/**
|
||||
* Returns whether the controller has settled at the target. Determining what settling means is
|
||||
* implementation-dependent.
|
||||
*
|
||||
* If the controller is disabled, this method must return `true`.
|
||||
*
|
||||
* @return whether the controller is settled
|
||||
*/
|
||||
bool isSettled() override;
|
||||
|
||||
/**
|
||||
* Resets the controller's internal state so it is similar to when it was first initialized, while
|
||||
* keeping any user-configured information. This implementation also stops movement.
|
||||
*/
|
||||
void reset() override;
|
||||
|
||||
/**
|
||||
* Changes whether the controller is off or on. Turning the controller on after it was off will
|
||||
* NOT cause the controller to move to its last set target.
|
||||
*/
|
||||
void flipDisable() override;
|
||||
|
||||
/**
|
||||
* Sets whether the controller is off or on. Turning the controller on after it was off will
|
||||
* NOT cause the controller to move to its last set target, unless it was reset in that time.
|
||||
*
|
||||
* @param iisDisabled whether the controller is disabled
|
||||
*/
|
||||
void flipDisable(bool iisDisabled) override;
|
||||
|
||||
/**
|
||||
* Returns whether the controller is currently disabled.
|
||||
*
|
||||
* @return whether the controller is currently disabled
|
||||
*/
|
||||
bool isDisabled() const override;
|
||||
|
||||
/**
|
||||
* This implementation does nothing because the API always requires the starting position to be
|
||||
* specified.
|
||||
*/
|
||||
void tarePosition() override;
|
||||
|
||||
/**
|
||||
* This implementation does nothing because the maximum velocity is configured using
|
||||
* PathfinderLimits elsewhere.
|
||||
*
|
||||
* @param imaxVelocity Ignored.
|
||||
*/
|
||||
void setMaxVelocity(std::int32_t imaxVelocity) override;
|
||||
|
||||
/**
|
||||
* Starts the internal thread. This should not be called by normal users. This method is called
|
||||
* by the AsyncControllerFactory when making a new instance of this class.
|
||||
*/
|
||||
void startThread();
|
||||
|
||||
/**
|
||||
* Returns the underlying thread handle.
|
||||
*
|
||||
* @return The underlying thread handle.
|
||||
*/
|
||||
CrossplatformThread *getThread() const;
|
||||
|
||||
/**
|
||||
* Attempts to remove a path without stopping execution, then if that fails, disables the
|
||||
* controller and removes the path.
|
||||
*
|
||||
* @param ipathId The path ID that will be removed
|
||||
*/
|
||||
void forceRemovePath(const std::string &ipathId);
|
||||
|
||||
protected:
|
||||
std::shared_ptr<Logger> logger;
|
||||
std::map<std::string, std::vector<squiggles::ProfilePoint>> paths{};
|
||||
PathfinderLimits limits;
|
||||
std::shared_ptr<ControllerOutput<double>> output;
|
||||
QLength diameter;
|
||||
AbstractMotor::GearsetRatioPair pair;
|
||||
double currentProfilePosition{0};
|
||||
TimeUtil timeUtil;
|
||||
|
||||
// This must be locked when accessing the current path
|
||||
CrossplatformMutex currentPathMutex;
|
||||
|
||||
std::string currentPath{""};
|
||||
std::atomic_bool isRunning{false};
|
||||
std::atomic_int direction{1};
|
||||
std::atomic_bool disabled{false};
|
||||
std::atomic_bool dtorCalled{false};
|
||||
CrossplatformThread *task{nullptr};
|
||||
|
||||
static void trampoline(void *context);
|
||||
void loop();
|
||||
|
||||
/**
|
||||
* Follow the supplied path. Must follow the disabled lifecycle.
|
||||
*/
|
||||
virtual void executeSinglePath(const std::vector<squiggles::ProfilePoint> &path,
|
||||
std::unique_ptr<AbstractRate> rate);
|
||||
|
||||
/**
|
||||
* Converts linear "chassis" speed to rotational motor speed.
|
||||
*
|
||||
* @param linear "chassis" frame speed
|
||||
* @return motor frame speed
|
||||
*/
|
||||
QAngularSpeed convertLinearToRotational(QSpeed linear) const;
|
||||
|
||||
std::string getPathErrorMessage(const std::vector<PathfinderPoint> &points,
|
||||
const std::string &ipathId,
|
||||
int length);
|
||||
};
|
||||
} // namespace okapi
|
||||
@@ -0,0 +1,326 @@
|
||||
/*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "okapi/api/chassis/controller/chassisScales.hpp"
|
||||
#include "okapi/api/chassis/model/skidSteerModel.hpp"
|
||||
#include "okapi/api/control/async/asyncPositionController.hpp"
|
||||
#include "okapi/api/control/util/pathfinderUtil.hpp"
|
||||
#include "okapi/api/units/QAngularSpeed.hpp"
|
||||
#include "okapi/api/units/QSpeed.hpp"
|
||||
#include "okapi/api/util/logging.hpp"
|
||||
#include "okapi/api/util/timeUtil.hpp"
|
||||
#include <atomic>
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
|
||||
#include "squiggles.hpp"
|
||||
|
||||
namespace okapi {
|
||||
class AsyncMotionProfileController : public AsyncPositionController<std::string, PathfinderPoint> {
|
||||
public:
|
||||
/**
|
||||
* An Async Controller which generates and follows 2D motion profiles. Throws a
|
||||
* `std::invalid_argument` exception if the gear ratio is zero.
|
||||
*
|
||||
* @param itimeUtil The TimeUtil.
|
||||
* @param ilimits The default limits.
|
||||
* @param imodel The chassis model to control.
|
||||
* @param iscales The chassis dimensions.
|
||||
* @param ipair The gearset.
|
||||
* @param ilogger The logger this instance will log to.
|
||||
*/
|
||||
AsyncMotionProfileController(const TimeUtil &itimeUtil,
|
||||
const PathfinderLimits &ilimits,
|
||||
const std::shared_ptr<ChassisModel> &imodel,
|
||||
const ChassisScales &iscales,
|
||||
const AbstractMotor::GearsetRatioPair &ipair,
|
||||
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
|
||||
|
||||
AsyncMotionProfileController(AsyncMotionProfileController &&other) = delete;
|
||||
|
||||
AsyncMotionProfileController &operator=(AsyncMotionProfileController &&other) = delete;
|
||||
|
||||
~AsyncMotionProfileController() override;
|
||||
|
||||
/**
|
||||
* Generates a path which intersects the given waypoints and saves it internally with a key of
|
||||
* pathId. Call `executePath()` with the same pathId to run it.
|
||||
*
|
||||
* If the waypoints form a path which is impossible to achieve, an instance of
|
||||
* `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
|
||||
* are no waypoints, no path is generated.
|
||||
*
|
||||
* @param iwaypoints The waypoints to hit on the path.
|
||||
* @param ipathId A unique identifier to save the path with.
|
||||
*/
|
||||
void generatePath(std::initializer_list<PathfinderPoint> iwaypoints, const std::string &ipathId);
|
||||
|
||||
/**
|
||||
* Generates a path which intersects the given waypoints and saves it internally with a key of
|
||||
* pathId. Call `executePath()` with the same pathId to run it.
|
||||
*
|
||||
* If the waypoints form a path which is impossible to achieve, an instance of
|
||||
* `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
|
||||
* are no waypoints, no path is generated.
|
||||
*
|
||||
* NOTE: The waypoints are expected to be in the
|
||||
* okapi::State::FRAME_TRANSFORMATION format where +x is forward, +y is right,
|
||||
* and 0 theta is measured from the +x axis to the +y axis.
|
||||
*
|
||||
* @param iwaypoints The waypoints to hit on the path.
|
||||
* @param ipathId A unique identifier to save the path with.
|
||||
* @param ilimits The limits to use for this path only.
|
||||
*/
|
||||
void generatePath(std::initializer_list<PathfinderPoint> iwaypoints,
|
||||
const std::string &ipathId,
|
||||
const PathfinderLimits &ilimits);
|
||||
|
||||
/**
|
||||
* Removes a path and frees the memory it used. This function returns true if the path was either
|
||||
* deleted or didn't exist in the first place. It returns false if the path could not be removed
|
||||
* because it is running.
|
||||
*
|
||||
* @param ipathId A unique identifier for the path, previously passed to `generatePath()`
|
||||
* @return True if the path no longer exists
|
||||
*/
|
||||
bool removePath(const std::string &ipathId);
|
||||
|
||||
/**
|
||||
* Gets the identifiers of all paths saved in this `AsyncMotionProfileController`.
|
||||
*
|
||||
* @return The identifiers of all paths
|
||||
*/
|
||||
std::vector<std::string> getPaths();
|
||||
|
||||
/**
|
||||
* Executes a path with the given ID. If there is no path matching the ID, the method will
|
||||
* return. Any targets set while a path is being followed will be ignored.
|
||||
*
|
||||
* @param ipathId A unique identifier for the path, previously passed to `generatePath()`.
|
||||
*/
|
||||
void setTarget(std::string ipathId) override;
|
||||
|
||||
/**
|
||||
* Executes a path with the given ID. If there is no path matching the ID, the method will
|
||||
* return. Any targets set while a path is being followed will be ignored.
|
||||
*
|
||||
* @param ipathId A unique identifier for the path, previously passed to `generatePath()`.
|
||||
* @param ibackwards Whether to follow the profile backwards.
|
||||
* @param imirrored Whether to follow the profile mirrored.
|
||||
*/
|
||||
void setTarget(std::string ipathId, bool ibackwards, bool imirrored = false);
|
||||
|
||||
/**
|
||||
* Writes the value of the controller output. This method might be automatically called in another
|
||||
* thread by the controller. This just calls `setTarget()`.
|
||||
*/
|
||||
void controllerSet(std::string ivalue) override;
|
||||
|
||||
/**
|
||||
* Gets the last set target, or the default target if none was set.
|
||||
*
|
||||
* @return the last target
|
||||
*/
|
||||
std::string getTarget() override;
|
||||
|
||||
/**
|
||||
* This is overridden to return the current path.
|
||||
*
|
||||
* @return The most recent value of the process variable.
|
||||
*/
|
||||
std::string getProcessValue() const override;
|
||||
|
||||
/**
|
||||
* Blocks the current task until the controller has settled. This controller is settled when
|
||||
* it has finished following a path. If no path is being followed, it is settled.
|
||||
*/
|
||||
void waitUntilSettled() override;
|
||||
|
||||
/**
|
||||
* Generates a new path from the position (typically the current position) to the target and
|
||||
* blocks until the controller has settled. Does not save the path which was generated.
|
||||
*
|
||||
* @param iwaypoints The waypoints to hit on the path.
|
||||
* @param ibackwards Whether to follow the profile backwards.
|
||||
* @param imirrored Whether to follow the profile mirrored.
|
||||
*/
|
||||
void moveTo(std::initializer_list<PathfinderPoint> iwaypoints,
|
||||
bool ibackwards = false,
|
||||
bool imirrored = false);
|
||||
|
||||
/**
|
||||
* Generates a new path from the position (typically the current position) to the target and
|
||||
* blocks until the controller has settled. Does not save the path which was generated.
|
||||
*
|
||||
* @param iwaypoints The waypoints to hit on the path.
|
||||
* @param ilimits The limits to use for this path only.
|
||||
* @param ibackwards Whether to follow the profile backwards.
|
||||
* @param imirrored Whether to follow the profile mirrored.
|
||||
*/
|
||||
void moveTo(std::initializer_list<PathfinderPoint> iwaypoints,
|
||||
const PathfinderLimits &ilimits,
|
||||
bool ibackwards = false,
|
||||
bool imirrored = false);
|
||||
|
||||
/**
|
||||
* Returns the last error of the controller. Does not update when disabled. This implementation
|
||||
* always returns zero since the robot is assumed to perfectly follow the path. Subclasses can
|
||||
* override this to be more accurate using odometry information.
|
||||
*
|
||||
* @return the last error
|
||||
*/
|
||||
PathfinderPoint getError() const override;
|
||||
|
||||
/**
|
||||
* Returns whether the controller has settled at the target. Determining what settling means is
|
||||
* implementation-dependent.
|
||||
*
|
||||
* If the controller is disabled, this method must return true.
|
||||
*
|
||||
* @return whether the controller is settled
|
||||
*/
|
||||
bool isSettled() override;
|
||||
|
||||
/**
|
||||
* Resets the controller so it can start from 0 again properly. Keeps configuration from
|
||||
* before. This implementation also stops movement.
|
||||
*/
|
||||
void reset() override;
|
||||
|
||||
/**
|
||||
* Changes whether the controller is off or on. Turning the controller on after it was off will
|
||||
* NOT cause the controller to move to its last set target.
|
||||
*/
|
||||
void flipDisable() override;
|
||||
|
||||
/**
|
||||
* Sets whether the controller is off or on. Turning the controller on after it was off will
|
||||
* NOT cause the controller to move to its last set target, unless it was reset in that time.
|
||||
*
|
||||
* @param iisDisabled whether the controller is disabled
|
||||
*/
|
||||
void flipDisable(bool iisDisabled) override;
|
||||
|
||||
/**
|
||||
* Returns whether the controller is currently disabled.
|
||||
*
|
||||
* @return whether the controller is currently disabled
|
||||
*/
|
||||
bool isDisabled() const override;
|
||||
|
||||
/**
|
||||
* This implementation does nothing because the API always requires the starting position to be
|
||||
* specified.
|
||||
*/
|
||||
void tarePosition() override;
|
||||
|
||||
/**
|
||||
* This implementation does nothing because the maximum velocity is configured using
|
||||
* PathfinderLimits elsewhere.
|
||||
*
|
||||
* @param imaxVelocity Ignored.
|
||||
*/
|
||||
void setMaxVelocity(std::int32_t imaxVelocity) override;
|
||||
|
||||
/**
|
||||
* Starts the internal thread. This should not be called by normal users. This method is called
|
||||
* by the `AsyncMotionProfileControllerBuilder` when making a new instance of this class.
|
||||
*/
|
||||
void startThread();
|
||||
|
||||
/**
|
||||
* @return The underlying thread handle.
|
||||
*/
|
||||
CrossplatformThread *getThread() const;
|
||||
|
||||
/**
|
||||
* Saves a generated path to a file. Paths are stored as `<ipathId>.csv`. An SD card
|
||||
* must be inserted into the brain and the directory must exist. `idirectory` can be prefixed with
|
||||
* `/usd/`, but it this is not required.
|
||||
*
|
||||
* @param idirectory The directory to store the path file in
|
||||
* @param ipathId The path ID of the generated path
|
||||
*/
|
||||
void storePath(const std::string &idirectory, const std::string &ipathId);
|
||||
|
||||
/**
|
||||
* Loads a path from a directory on the SD card containing a path CSV file. `/usd/` is
|
||||
* automatically prepended to `idirectory` if it is not specified.
|
||||
*
|
||||
* @param idirectory The directory that the path files are stored in
|
||||
* @param ipathId The path ID that the paths are stored under (and will be loaded into)
|
||||
*/
|
||||
void loadPath(const std::string &idirectory, const std::string &ipathId);
|
||||
|
||||
/**
|
||||
* Attempts to remove a path without stopping execution. If that fails, disables the controller
|
||||
* and removes the path.
|
||||
*
|
||||
* @param ipathId The path ID that will be removed
|
||||
*/
|
||||
void forceRemovePath(const std::string &ipathId);
|
||||
|
||||
protected:
|
||||
std::shared_ptr<Logger> logger;
|
||||
std::map<std::string, std::vector<squiggles::ProfilePoint>> paths{};
|
||||
PathfinderLimits limits;
|
||||
std::shared_ptr<ChassisModel> model;
|
||||
ChassisScales scales;
|
||||
AbstractMotor::GearsetRatioPair pair;
|
||||
TimeUtil timeUtil;
|
||||
|
||||
// This must be locked when accessing the current path
|
||||
CrossplatformMutex currentPathMutex;
|
||||
|
||||
std::string currentPath{""};
|
||||
std::atomic_bool isRunning{false};
|
||||
std::atomic_int direction{1};
|
||||
std::atomic_bool mirrored{false};
|
||||
std::atomic_bool disabled{false};
|
||||
std::atomic_bool dtorCalled{false};
|
||||
CrossplatformThread *task{nullptr};
|
||||
|
||||
static void trampoline(void *context);
|
||||
void loop();
|
||||
|
||||
/**
|
||||
* Follow the supplied path. Must follow the disabled lifecycle.
|
||||
*/
|
||||
virtual void executeSinglePath(const std::vector<squiggles::ProfilePoint> &path,
|
||||
std::unique_ptr<AbstractRate> rate);
|
||||
|
||||
/**
|
||||
* Converts linear chassis speed to rotational motor speed.
|
||||
*
|
||||
* @param linear chassis frame speed
|
||||
* @return motor frame speed
|
||||
*/
|
||||
QAngularSpeed convertLinearToRotational(QSpeed linear) const;
|
||||
|
||||
std::string getPathErrorMessage(const std::vector<PathfinderPoint> &points,
|
||||
const std::string &ipathId,
|
||||
int length);
|
||||
|
||||
/**
|
||||
* Joins and escapes a directory and file name
|
||||
*
|
||||
* @param directory The directory path, separated by forward slashes (/) and with or without a
|
||||
* trailing slash
|
||||
* @param filename The file name in the directory
|
||||
* @return the fully qualified and legal path name
|
||||
*/
|
||||
static std::string makeFilePath(const std::string &directory, const std::string &filename);
|
||||
|
||||
void internalStorePath(std::ostream &file, const std::string &ipathId);
|
||||
void internalLoadPath(std::istream &file, const std::string &ipathId);
|
||||
void internalLoadPathfinderPath(std::istream &leftFile,
|
||||
std::istream &rightFile,
|
||||
const std::string &ipathId);
|
||||
|
||||
static constexpr double DT = 0.01;
|
||||
};
|
||||
} // namespace okapi
|
||||
@@ -0,0 +1,145 @@
|
||||
/*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "okapi/api/control/async/asyncPositionController.hpp"
|
||||
#include "okapi/api/device/motor/abstractMotor.hpp"
|
||||
#include "okapi/api/util/logging.hpp"
|
||||
#include "okapi/api/util/timeUtil.hpp"
|
||||
|
||||
namespace okapi {
|
||||
/**
|
||||
* Closed-loop controller that uses the V5 motor's onboard control to move. Input units are whatever
|
||||
* units the motor is in.
|
||||
*/
|
||||
class AsyncPosIntegratedController : public AsyncPositionController<double, double> {
|
||||
public:
|
||||
/**
|
||||
* Closed-loop controller that uses the V5 motor's onboard control to move. Input units are
|
||||
* whatever units the motor is in. Throws a std::invalid_argument exception if the gear ratio is
|
||||
* zero.
|
||||
*
|
||||
* @param imotor The motor to control.
|
||||
* @param ipair The gearset.
|
||||
* @param imaxVelocity The maximum velocity after gearing.
|
||||
* @param itimeUtil The TimeUtil.
|
||||
* @param ilogger The logger this instance will log to.
|
||||
*/
|
||||
AsyncPosIntegratedController(const std::shared_ptr<AbstractMotor> &imotor,
|
||||
const AbstractMotor::GearsetRatioPair &ipair,
|
||||
std::int32_t imaxVelocity,
|
||||
const TimeUtil &itimeUtil,
|
||||
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
|
||||
|
||||
/**
|
||||
* Sets the target for the controller.
|
||||
*/
|
||||
void setTarget(double itarget) override;
|
||||
|
||||
/**
|
||||
* Gets the last set target, or the default target if none was set.
|
||||
*
|
||||
* @return the last target
|
||||
*/
|
||||
double getTarget() override;
|
||||
|
||||
/**
|
||||
* @return The most recent value of the process variable.
|
||||
*/
|
||||
double getProcessValue() const override;
|
||||
|
||||
/**
|
||||
* Returns the last error of the controller. Does not update when disabled.
|
||||
*/
|
||||
double getError() const override;
|
||||
|
||||
/**
|
||||
* Returns whether the controller has settled at the target. Determining what settling means is
|
||||
* implementation-dependent.
|
||||
*
|
||||
* If the controller is disabled, this method must return true.
|
||||
*
|
||||
* @return whether the controller is settled
|
||||
*/
|
||||
bool isSettled() override;
|
||||
|
||||
/**
|
||||
* Resets the controller's internal state so it is similar to when it was first initialized, while
|
||||
* keeping any user-configured information.
|
||||
*/
|
||||
void reset() override;
|
||||
|
||||
/**
|
||||
* Changes whether the controller is off or on. Turning the controller on after it was off will
|
||||
* cause the controller to move to its last set target, unless it was reset in that time.
|
||||
*/
|
||||
void flipDisable() override;
|
||||
|
||||
/**
|
||||
* Sets whether the controller is off or on. Turning the controller on after it was off will
|
||||
* cause the controller to move to its last set target, unless it was reset in that time.
|
||||
*
|
||||
* @param iisDisabled whether the controller is disabled
|
||||
*/
|
||||
void flipDisable(bool iisDisabled) override;
|
||||
|
||||
/**
|
||||
* Returns whether the controller is currently disabled.
|
||||
*
|
||||
* @return whether the controller is currently disabled
|
||||
*/
|
||||
bool isDisabled() const override;
|
||||
|
||||
/**
|
||||
* Blocks the current task until the controller has settled. Determining what settling means is
|
||||
* implementation-dependent.
|
||||
*/
|
||||
void waitUntilSettled() override;
|
||||
|
||||
/**
|
||||
* Writes the value of the controller output. This method might be automatically called in another
|
||||
* thread by the controller. The range of input values is expected to be [-1, 1].
|
||||
*
|
||||
* @param ivalue the controller's output in the range [-1, 1]
|
||||
*/
|
||||
void controllerSet(double ivalue) override;
|
||||
|
||||
/**
|
||||
* Sets the "absolute" zero position of the controller to its current position.
|
||||
*/
|
||||
void tarePosition() override;
|
||||
|
||||
/**
|
||||
* Sets a new maximum velocity in motor RPM [0-600].
|
||||
*
|
||||
* @param imaxVelocity The new maximum velocity in motor RPM [0-600].
|
||||
*/
|
||||
void setMaxVelocity(std::int32_t imaxVelocity) override;
|
||||
|
||||
/**
|
||||
* Stops the motor mid-movement. Does not change the last set target.
|
||||
*/
|
||||
virtual void stop();
|
||||
|
||||
protected:
|
||||
std::shared_ptr<Logger> logger;
|
||||
TimeUtil timeUtil;
|
||||
std::shared_ptr<AbstractMotor> motor;
|
||||
AbstractMotor::GearsetRatioPair pair;
|
||||
std::int32_t maxVelocity;
|
||||
double lastTarget{0};
|
||||
double offset{0};
|
||||
bool controllerIsDisabled{false};
|
||||
bool hasFirstTarget{false};
|
||||
std::unique_ptr<SettledUtil> settledUtil;
|
||||
|
||||
/**
|
||||
* Resumes moving after the controller is reset. Should not cause movement if the controller is
|
||||
* turned off, reset, and turned back on.
|
||||
*/
|
||||
virtual void resumeMovement();
|
||||
};
|
||||
} // namespace okapi
|
||||
@@ -0,0 +1,100 @@
|
||||
/*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "okapi/api/control/async/asyncPositionController.hpp"
|
||||
#include "okapi/api/control/async/asyncWrapper.hpp"
|
||||
#include "okapi/api/control/controllerOutput.hpp"
|
||||
#include "okapi/api/control/iterative/iterativePosPidController.hpp"
|
||||
#include "okapi/api/control/offsettableControllerInput.hpp"
|
||||
#include "okapi/api/util/timeUtil.hpp"
|
||||
#include <memory>
|
||||
|
||||
namespace okapi {
|
||||
class AsyncPosPIDController : public AsyncWrapper<double, double>,
|
||||
public AsyncPositionController<double, double> {
|
||||
public:
|
||||
/**
|
||||
* An async position PID controller.
|
||||
*
|
||||
* @param iinput The controller input. Will be turned into an OffsettableControllerInput.
|
||||
* @param ioutput The controller output.
|
||||
* @param itimeUtil The TimeUtil.
|
||||
* @param ikP The proportional gain.
|
||||
* @param ikI The integral gain.
|
||||
* @param ikD The derivative gain.
|
||||
* @param ikBias The controller bias.
|
||||
* @param iratio Any external gear ratio.
|
||||
* @param iderivativeFilter The derivative filter.
|
||||
*/
|
||||
AsyncPosPIDController(
|
||||
const std::shared_ptr<ControllerInput<double>> &iinput,
|
||||
const std::shared_ptr<ControllerOutput<double>> &ioutput,
|
||||
const TimeUtil &itimeUtil,
|
||||
double ikP,
|
||||
double ikI,
|
||||
double ikD,
|
||||
double ikBias = 0,
|
||||
double iratio = 1,
|
||||
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
|
||||
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
|
||||
|
||||
/**
|
||||
* An async position PID controller.
|
||||
*
|
||||
* @param iinput The controller input.
|
||||
* @param ioutput The controller output.
|
||||
* @param itimeUtil The TimeUtil.
|
||||
* @param ikP The proportional gain.
|
||||
* @param ikI The integral gain.
|
||||
* @param ikD The derivative gain.
|
||||
* @param ikBias The controller bias.
|
||||
* @param iratio Any external gear ratio.
|
||||
* @param iderivativeFilter The derivative filter.
|
||||
*/
|
||||
AsyncPosPIDController(
|
||||
const std::shared_ptr<OffsetableControllerInput> &iinput,
|
||||
const std::shared_ptr<ControllerOutput<double>> &ioutput,
|
||||
const TimeUtil &itimeUtil,
|
||||
double ikP,
|
||||
double ikI,
|
||||
double ikD,
|
||||
double ikBias = 0,
|
||||
double iratio = 1,
|
||||
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
|
||||
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
|
||||
|
||||
/**
|
||||
* Sets the "absolute" zero position of the controller to its current position.
|
||||
*/
|
||||
void tarePosition() override;
|
||||
|
||||
/**
|
||||
* This implementation does not respect the maximum velocity.
|
||||
*
|
||||
* @param imaxVelocity Ignored.
|
||||
*/
|
||||
void setMaxVelocity(std::int32_t imaxVelocity) override;
|
||||
|
||||
/**
|
||||
* Set controller gains.
|
||||
*
|
||||
* @param igains The new gains.
|
||||
*/
|
||||
void setGains(const IterativePosPIDController::Gains &igains);
|
||||
|
||||
/**
|
||||
* Gets the current gains.
|
||||
*
|
||||
* @return The current gains.
|
||||
*/
|
||||
IterativePosPIDController::Gains getGains() const;
|
||||
|
||||
protected:
|
||||
std::shared_ptr<OffsetableControllerInput> offsettableInput;
|
||||
std::shared_ptr<IterativePosPIDController> internalController;
|
||||
};
|
||||
} // namespace okapi
|
||||
@@ -0,0 +1,28 @@
|
||||
/*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "okapi/api/control/async/asyncController.hpp"
|
||||
#include <memory>
|
||||
|
||||
namespace okapi {
|
||||
template <typename Input, typename Output>
|
||||
class AsyncPositionController : virtual public AsyncController<Input, Output> {
|
||||
public:
|
||||
/**
|
||||
* Sets the "absolute" zero position of the controller to its current position.
|
||||
*/
|
||||
virtual void tarePosition() = 0;
|
||||
|
||||
/**
|
||||
* Sets a new maximum velocity (typically motor RPM [0-600]). The interpretation of the units
|
||||
* of this velocity and whether it will be respected is implementation-dependent.
|
||||
*
|
||||
* @param imaxVelocity The new maximum velocity.
|
||||
*/
|
||||
virtual void setMaxVelocity(std::int32_t imaxVelocity) = 0;
|
||||
};
|
||||
} // namespace okapi
|
||||
@@ -0,0 +1,124 @@
|
||||
/*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "okapi/api/control/async/asyncVelocityController.hpp"
|
||||
#include "okapi/api/device/motor/abstractMotor.hpp"
|
||||
#include "okapi/api/util/logging.hpp"
|
||||
#include "okapi/api/util/timeUtil.hpp"
|
||||
#include <memory>
|
||||
|
||||
namespace okapi {
|
||||
/**
|
||||
* Closed-loop controller that uses the V5 motor's onboard control to move. Input units are whatever
|
||||
* units the motor is in.
|
||||
*/
|
||||
class AsyncVelIntegratedController : public AsyncVelocityController<double, double> {
|
||||
public:
|
||||
/**
|
||||
* Closed-loop controller that uses the V5 motor's onboard control to move. Input units are
|
||||
* whatever units the motor is in. Throws a std::invalid_argument exception if the gear ratio is
|
||||
* zero.
|
||||
*
|
||||
* @param imotor The motor to control.
|
||||
* @param ipair The gearset.
|
||||
* @param imaxVelocity The maximum velocity after gearing.
|
||||
* @param itimeUtil The TimeUtil.
|
||||
* @param ilogger The logger this instance will log to.
|
||||
*/
|
||||
AsyncVelIntegratedController(const std::shared_ptr<AbstractMotor> &imotor,
|
||||
const AbstractMotor::GearsetRatioPair &ipair,
|
||||
std::int32_t imaxVelocity,
|
||||
const TimeUtil &itimeUtil,
|
||||
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
|
||||
|
||||
/**
|
||||
* Sets the target for the controller.
|
||||
*/
|
||||
void setTarget(double itarget) override;
|
||||
|
||||
/**
|
||||
* Gets the last set target, or the default target if none was set.
|
||||
*
|
||||
* @return the last target
|
||||
*/
|
||||
double getTarget() override;
|
||||
|
||||
/**
|
||||
* @return The most recent value of the process variable.
|
||||
*/
|
||||
double getProcessValue() const override;
|
||||
|
||||
/**
|
||||
* Returns the last error of the controller. Does not update when disabled.
|
||||
*/
|
||||
double getError() const override;
|
||||
|
||||
/**
|
||||
* Returns whether the controller has settled at the target. Determining what settling means is
|
||||
* implementation-dependent.
|
||||
*
|
||||
* If the controller is disabled, this method must return true.
|
||||
*
|
||||
* @return whether the controller is settled
|
||||
*/
|
||||
bool isSettled() override;
|
||||
|
||||
/**
|
||||
* Resets the controller's internal state so it is similar to when it was first initialized, while
|
||||
* keeping any user-configured information.
|
||||
*/
|
||||
void reset() override;
|
||||
|
||||
/**
|
||||
* Changes whether the controller is off or on. Turning the controller on after it was off will
|
||||
* cause the controller to move to its last set target, unless it was reset in that time.
|
||||
*/
|
||||
void flipDisable() override;
|
||||
|
||||
/**
|
||||
* Sets whether the controller is off or on. Turning the controller on after it was off will
|
||||
* cause the controller to move to its last set target, unless it was reset in that time.
|
||||
*
|
||||
* @param iisDisabled whether the controller is disabled
|
||||
*/
|
||||
void flipDisable(bool iisDisabled) override;
|
||||
|
||||
/**
|
||||
* Returns whether the controller is currently disabled.
|
||||
*
|
||||
* @return whether the controller is currently disabled
|
||||
*/
|
||||
bool isDisabled() const override;
|
||||
|
||||
/**
|
||||
* Blocks the current task until the controller has settled. Determining what settling means is
|
||||
* implementation-dependent.
|
||||
*/
|
||||
void waitUntilSettled() override;
|
||||
|
||||
/**
|
||||
* Writes the value of the controller output. This method might be automatically called in another
|
||||
* thread by the controller. The range of input values is expected to be [-1, 1].
|
||||
*
|
||||
* @param ivalue the controller's output in the range [-1, 1]
|
||||
*/
|
||||
void controllerSet(double ivalue) override;
|
||||
|
||||
protected:
|
||||
std::shared_ptr<Logger> logger;
|
||||
TimeUtil timeUtil;
|
||||
std::shared_ptr<AbstractMotor> motor;
|
||||
AbstractMotor::GearsetRatioPair pair;
|
||||
std::int32_t maxVelocity;
|
||||
double lastTarget = 0;
|
||||
bool controllerIsDisabled = false;
|
||||
bool hasFirstTarget = false;
|
||||
std::unique_ptr<SettledUtil> settledUtil;
|
||||
|
||||
virtual void resumeMovement();
|
||||
};
|
||||
} // namespace okapi
|
||||
@@ -0,0 +1,64 @@
|
||||
/*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "okapi/api/control/async/asyncVelocityController.hpp"
|
||||
#include "okapi/api/control/async/asyncWrapper.hpp"
|
||||
#include "okapi/api/control/controllerInput.hpp"
|
||||
#include "okapi/api/control/controllerOutput.hpp"
|
||||
#include "okapi/api/control/iterative/iterativeVelPidController.hpp"
|
||||
#include "okapi/api/util/timeUtil.hpp"
|
||||
#include <memory>
|
||||
|
||||
namespace okapi {
|
||||
class AsyncVelPIDController : public AsyncWrapper<double, double>,
|
||||
public AsyncVelocityController<double, double> {
|
||||
public:
|
||||
/**
|
||||
* An async velocity PID controller.
|
||||
*
|
||||
* @param iinput The controller input.
|
||||
* @param ioutput The controller output.
|
||||
* @param itimeUtil The TimeUtil.
|
||||
* @param ikP The proportional gain.
|
||||
* @param ikD The derivative gain.
|
||||
* @param ikF The feed-forward gain.
|
||||
* @param ikSF A feed-forward gain to counteract static friction.
|
||||
* @param ivelMath The VelMath used for calculating velocity.
|
||||
* @param iratio Any external gear ratio.
|
||||
* @param iderivativeFilter The derivative filter.
|
||||
*/
|
||||
AsyncVelPIDController(
|
||||
const std::shared_ptr<ControllerInput<double>> &iinput,
|
||||
const std::shared_ptr<ControllerOutput<double>> &ioutput,
|
||||
const TimeUtil &itimeUtil,
|
||||
double ikP,
|
||||
double ikD,
|
||||
double ikF,
|
||||
double ikSF,
|
||||
std::unique_ptr<VelMath> ivelMath,
|
||||
double iratio = 1,
|
||||
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
|
||||
const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
|
||||
|
||||
/**
|
||||
* Set controller gains.
|
||||
*
|
||||
* @param igains The new gains.
|
||||
*/
|
||||
void setGains(const IterativeVelPIDController::Gains &igains);
|
||||
|
||||
/**
|
||||
* Gets the current gains.
|
||||
*
|
||||
* @return The current gains.
|
||||
*/
|
||||
IterativeVelPIDController::Gains getGains() const;
|
||||
|
||||
protected:
|
||||
std::shared_ptr<IterativeVelPIDController> internalController;
|
||||
};
|
||||
} // namespace okapi
|
||||
@@ -0,0 +1,14 @@
|
||||
/*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "okapi/api/control/async/asyncController.hpp"
|
||||
#include <memory>
|
||||
|
||||
namespace okapi {
|
||||
template <typename Input, typename Output>
|
||||
class AsyncVelocityController : virtual public AsyncController<Input, Output> {};
|
||||
} // namespace okapi
|
||||
287
SerialTest/include/okapi/api/control/async/asyncWrapper.hpp
Normal file
287
SerialTest/include/okapi/api/control/async/asyncWrapper.hpp
Normal file
@@ -0,0 +1,287 @@
|
||||
/*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "okapi/api/control/async/asyncController.hpp"
|
||||
#include "okapi/api/control/controllerInput.hpp"
|
||||
#include "okapi/api/control/iterative/iterativeController.hpp"
|
||||
#include "okapi/api/control/util/settledUtil.hpp"
|
||||
#include "okapi/api/coreProsAPI.hpp"
|
||||
#include "okapi/api/util/abstractRate.hpp"
|
||||
#include "okapi/api/util/logging.hpp"
|
||||
#include "okapi/api/util/mathUtil.hpp"
|
||||
#include "okapi/api/util/supplier.hpp"
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
|
||||
namespace okapi {
|
||||
template <typename Input, typename Output>
|
||||
class AsyncWrapper : virtual public AsyncController<Input, Output> {
|
||||
public:
|
||||
/**
|
||||
* A wrapper class that transforms an `IterativeController` into an `AsyncController` by running
|
||||
* it in another task. The input controller will act like an `AsyncController`.
|
||||
*
|
||||
* @param iinput controller input, passed to the `IterativeController`
|
||||
* @param ioutput controller output, written to from the `IterativeController`
|
||||
* @param icontroller the controller to use
|
||||
* @param irateSupplier used for rates used in the main loop and in `waitUntilSettled`
|
||||
* @param iratio Any external gear ratio.
|
||||
* @param ilogger The logger this instance will log to.
|
||||
*/
|
||||
AsyncWrapper(const std::shared_ptr<ControllerInput<Input>> &iinput,
|
||||
const std::shared_ptr<ControllerOutput<Output>> &ioutput,
|
||||
const std::shared_ptr<IterativeController<Input, Output>> &icontroller,
|
||||
const Supplier<std::unique_ptr<AbstractRate>> &irateSupplier,
|
||||
const double iratio = 1,
|
||||
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger())
|
||||
: logger(std::move(ilogger)),
|
||||
rateSupplier(irateSupplier),
|
||||
input(iinput),
|
||||
output(ioutput),
|
||||
controller(icontroller),
|
||||
ratio(iratio) {
|
||||
}
|
||||
|
||||
AsyncWrapper(AsyncWrapper<Input, Output> &&other) = delete;
|
||||
|
||||
AsyncWrapper<Input, Output> &operator=(AsyncWrapper<Input, Output> &&other) = delete;
|
||||
|
||||
~AsyncWrapper() override {
|
||||
dtorCalled.store(true, std::memory_order_release);
|
||||
delete task;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the target for the controller.
|
||||
*/
|
||||
void setTarget(const Input itarget) override {
|
||||
LOG_INFO("AsyncWrapper: Set target to " + std::to_string(itarget));
|
||||
hasFirstTarget = true;
|
||||
controller->setTarget(itarget * ratio);
|
||||
lastTarget = itarget;
|
||||
}
|
||||
|
||||
/**
|
||||
* Writes the value of the controller output. This method might be automatically called in another
|
||||
* thread by the controller.
|
||||
*
|
||||
* @param ivalue the controller's output
|
||||
*/
|
||||
void controllerSet(const Input ivalue) override {
|
||||
controller->controllerSet(ivalue);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the last set target, or the default target if none was set.
|
||||
*
|
||||
* @return the last target
|
||||
*/
|
||||
Input getTarget() override {
|
||||
return controller->getTarget();
|
||||
}
|
||||
|
||||
/**
|
||||
* @return The most recent value of the process variable.
|
||||
*/
|
||||
Input getProcessValue() const override {
|
||||
return controller->getProcessValue();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the last calculated output of the controller.
|
||||
*/
|
||||
Output getOutput() const {
|
||||
return controller->getOutput();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the last error of the controller. Does not update when disabled.
|
||||
*/
|
||||
Output getError() const override {
|
||||
return controller->getError();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns whether the controller has settled at the target. Determining what settling means is
|
||||
* implementation-dependent.
|
||||
*
|
||||
* If the controller is disabled, this method must return true.
|
||||
*
|
||||
* @return whether the controller is settled
|
||||
*/
|
||||
bool isSettled() override {
|
||||
return isDisabled() || controller->isSettled();
|
||||
}
|
||||
|
||||
/**
|
||||
* Set time between loops.
|
||||
*
|
||||
* @param isampleTime time between loops
|
||||
*/
|
||||
void setSampleTime(const QTime &isampleTime) {
|
||||
controller->setSampleTime(isampleTime);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set controller output bounds.
|
||||
*
|
||||
* @param imax max output
|
||||
* @param imin min output
|
||||
*/
|
||||
void setOutputLimits(const Output imax, const Output imin) {
|
||||
controller->setOutputLimits(imax, imin);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the (soft) limits for the target range that controllerSet() scales into. The target
|
||||
* computed by controllerSet() is scaled into the range [-itargetMin, itargetMax].
|
||||
*
|
||||
* @param itargetMax The new max target for controllerSet().
|
||||
* @param itargetMin The new min target for controllerSet().
|
||||
*/
|
||||
void setControllerSetTargetLimits(double itargetMax, double itargetMin) {
|
||||
controller->setControllerSetTargetLimits(itargetMax, itargetMin);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the upper output bound.
|
||||
*
|
||||
* @return the upper output bound
|
||||
*/
|
||||
Output getMaxOutput() {
|
||||
return controller->getMaxOutput();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the lower output bound.
|
||||
*
|
||||
* @return the lower output bound
|
||||
*/
|
||||
Output getMinOutput() {
|
||||
return controller->getMinOutput();
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the controller's internal state so it is similar to when it was first initialized, while
|
||||
* keeping any user-configured information.
|
||||
*/
|
||||
void reset() override {
|
||||
LOG_INFO_S("AsyncWrapper: Reset");
|
||||
controller->reset();
|
||||
hasFirstTarget = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Changes whether the controller is off or on. Turning the controller on after it was off will
|
||||
* cause the controller to move to its last set target, unless it was reset in that time.
|
||||
*/
|
||||
void flipDisable() override {
|
||||
LOG_INFO("AsyncWrapper: flipDisable " + std::to_string(!controller->isDisabled()));
|
||||
controller->flipDisable();
|
||||
resumeMovement();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets whether the controller is off or on. Turning the controller on after it was off will
|
||||
* cause the controller to move to its last set target, unless it was reset in that time.
|
||||
*
|
||||
* @param iisDisabled whether the controller is disabled
|
||||
*/
|
||||
void flipDisable(const bool iisDisabled) override {
|
||||
LOG_INFO("AsyncWrapper: flipDisable " + std::to_string(iisDisabled));
|
||||
controller->flipDisable(iisDisabled);
|
||||
resumeMovement();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns whether the controller is currently disabled.
|
||||
*
|
||||
* @return whether the controller is currently disabled
|
||||
*/
|
||||
bool isDisabled() const override {
|
||||
return controller->isDisabled();
|
||||
}
|
||||
|
||||
/**
|
||||
* Blocks the current task until the controller has settled. Determining what settling means is
|
||||
* implementation-dependent.
|
||||
*/
|
||||
void waitUntilSettled() override {
|
||||
LOG_INFO_S("AsyncWrapper: Waiting to settle");
|
||||
|
||||
auto rate = rateSupplier.get();
|
||||
while (!isSettled()) {
|
||||
rate->delayUntil(motorUpdateRate);
|
||||
}
|
||||
|
||||
LOG_INFO_S("AsyncWrapper: Done waiting to settle");
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the internal thread. This should not be called by normal users. This method is called
|
||||
* by the AsyncControllerFactory when making a new instance of this class.
|
||||
*/
|
||||
void startThread() {
|
||||
if (!task) {
|
||||
task = new CrossplatformThread(trampoline, this, "AsyncWrapper");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the underlying thread handle.
|
||||
*
|
||||
* @return The underlying thread handle.
|
||||
*/
|
||||
CrossplatformThread *getThread() const {
|
||||
return task;
|
||||
}
|
||||
|
||||
protected:
|
||||
std::shared_ptr<Logger> logger;
|
||||
Supplier<std::unique_ptr<AbstractRate>> rateSupplier;
|
||||
std::shared_ptr<ControllerInput<Input>> input;
|
||||
std::shared_ptr<ControllerOutput<Output>> output;
|
||||
std::shared_ptr<IterativeController<Input, Output>> controller;
|
||||
bool hasFirstTarget{false};
|
||||
Input lastTarget;
|
||||
double ratio;
|
||||
std::atomic_bool dtorCalled{false};
|
||||
CrossplatformThread *task{nullptr};
|
||||
|
||||
static void trampoline(void *context) {
|
||||
if (context) {
|
||||
static_cast<AsyncWrapper *>(context)->loop();
|
||||
}
|
||||
}
|
||||
|
||||
void loop() {
|
||||
auto rate = rateSupplier.get();
|
||||
while (!dtorCalled.load(std::memory_order_acquire) && !task->notifyTake(0)) {
|
||||
if (!isDisabled()) {
|
||||
output->controllerSet(controller->step(input->controllerGet()));
|
||||
}
|
||||
|
||||
rate->delayUntil(controller->getSampleTime());
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Resumes moving after the controller is reset. Should not cause movement if the controller is
|
||||
* turned off, reset, and turned back on.
|
||||
*/
|
||||
virtual void resumeMovement() {
|
||||
if (isDisabled()) {
|
||||
// This will grab the output *when disabled*
|
||||
output->controllerSet(controller->getOutput());
|
||||
} else {
|
||||
if (hasFirstTarget) {
|
||||
setTarget(lastTarget);
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace okapi
|
||||
@@ -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
19
SerialTest/include/okapi/api/control/controllerInput.hpp
Normal 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
19
SerialTest/include/okapi/api/control/controllerOutput.hpp
Normal 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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
131
SerialTest/include/okapi/api/control/util/controllerRunner.hpp
Normal 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
156
SerialTest/include/okapi/api/control/util/flywheelSimulator.hpp
Normal 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
23
SerialTest/include/okapi/api/control/util/pathfinderUtil.hpp
Normal 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
80
SerialTest/include/okapi/api/control/util/pidTuner.hpp
Normal 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
51
SerialTest/include/okapi/api/control/util/settledUtil.hpp
Normal 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
131
SerialTest/include/okapi/api/coreProsAPI.hpp
Normal 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;
|
||||
};
|
||||
@@ -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
52
SerialTest/include/okapi/api/device/button/buttonBase.hpp
Normal 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
537
SerialTest/include/okapi/api/device/motor/abstractMotor.hpp
Normal 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
|
||||
@@ -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
|
||||
@@ -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
59
SerialTest/include/okapi/api/filter/averageFilter.hpp
Normal 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
50
SerialTest/include/okapi/api/filter/composableFilter.hpp
Normal 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
52
SerialTest/include/okapi/api/filter/demaFilter.hpp
Normal 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
71
SerialTest/include/okapi/api/filter/ekfFilter.hpp
Normal 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
47
SerialTest/include/okapi/api/filter/emaFilter.hpp
Normal 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
28
SerialTest/include/okapi/api/filter/filter.hpp
Normal 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
|
||||
@@ -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
94
SerialTest/include/okapi/api/filter/medianFilter.hpp
Normal 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
36
SerialTest/include/okapi/api/filter/passthroughFilter.hpp
Normal 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
74
SerialTest/include/okapi/api/filter/velMath.hpp
Normal 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
95
SerialTest/include/okapi/api/odometry/odomMath.hpp
Normal 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
57
SerialTest/include/okapi/api/odometry/odomState.hpp
Normal 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
61
SerialTest/include/okapi/api/odometry/odometry.hpp
Normal 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
30
SerialTest/include/okapi/api/odometry/point.hpp
Normal 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
17
SerialTest/include/okapi/api/odometry/stateMode.hpp
Normal 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
|
||||
@@ -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
93
SerialTest/include/okapi/api/odometry/twoEncoderOdometry.hpp
Normal 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
36
SerialTest/include/okapi/api/units/QAcceleration.hpp
Normal 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
35
SerialTest/include/okapi/api/units/QAngle.hpp
Normal 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
16
SerialTest/include/okapi/api/units/QAngularAcceleration.hpp
Normal 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
16
SerialTest/include/okapi/api/units/QAngularJerk.hpp
Normal 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
39
SerialTest/include/okapi/api/units/QAngularSpeed.hpp
Normal 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
26
SerialTest/include/okapi/api/units/QArea.hpp
Normal 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
43
SerialTest/include/okapi/api/units/QForce.hpp
Normal 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
27
SerialTest/include/okapi/api/units/QFrequency.hpp
Normal 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
18
SerialTest/include/okapi/api/units/QJerk.hpp
Normal 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
84
SerialTest/include/okapi/api/units/QLength.hpp
Normal 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
62
SerialTest/include/okapi/api/units/QMass.hpp
Normal 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
44
SerialTest/include/okapi/api/units/QPressure.hpp
Normal 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
43
SerialTest/include/okapi/api/units/QSpeed.hpp
Normal 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
55
SerialTest/include/okapi/api/units/QTime.hpp
Normal 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
43
SerialTest/include/okapi/api/units/QTorque.hpp
Normal 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
28
SerialTest/include/okapi/api/units/QVolume.hpp
Normal 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
419
SerialTest/include/okapi/api/units/RQuantity.hpp
Normal 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
46
SerialTest/include/okapi/api/units/RQuantityName.hpp
Normal 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
41
SerialTest/include/okapi/api/util/abstractRate.hpp
Normal 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
125
SerialTest/include/okapi/api/util/abstractTimer.hpp
Normal 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
192
SerialTest/include/okapi/api/util/logging.hpp
Normal 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
255
SerialTest/include/okapi/api/util/mathUtil.hpp
Normal 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
34
SerialTest/include/okapi/api/util/supplier.hpp
Normal 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
41
SerialTest/include/okapi/api/util/timeUtil.hpp
Normal 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
|
||||
@@ -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 ¬ParentedToCurrentTask();
|
||||
|
||||
/**
|
||||
* 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
|
||||
@@ -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 ¬ParentedToCurrentTask();
|
||||
|
||||
/**
|
||||
* 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
|
||||
@@ -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 ¬ParentedToCurrentTask();
|
||||
|
||||
/**
|
||||
* 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
|
||||
@@ -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 ¬ParentedToCurrentTask();
|
||||
|
||||
/**
|
||||
* 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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
71
SerialTest/include/okapi/impl/device/adiUltrasonic.hpp
Normal 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
50
SerialTest/include/okapi/impl/device/button/adiButton.hpp
Normal 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
|
||||
@@ -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
118
SerialTest/include/okapi/impl/device/controller.hpp
Normal 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
64
SerialTest/include/okapi/impl/device/controllerUtil.hpp
Normal 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
76
SerialTest/include/okapi/impl/device/distanceSensor.hpp
Normal 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
69
SerialTest/include/okapi/impl/device/motor/adiMotor.hpp
Normal 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
Reference in New Issue
Block a user