diff --git a/README.md b/README.md index c56985f5..5c18c0f1 100644 --- a/README.md +++ b/README.md @@ -2,4 +2,8 @@ ![example workflow](https://github.com/ad101-lab/pronounce-this/actions/workflows/c-cpp.yml/badge.svg) -This is our programming repository for 2021-2022 VEX Competition Tipping Point. +This is our programming repository for 2022-2023 VEX Competition Spin up. + +## Libraries used + +pros-grafana-lib - [Github link](https://github.com/BWHS-Robotics/pros-grafana-cli) diff --git a/firmware/libpros.a b/firmware/libpros.a index 672ff7a4..286ce009 100644 Binary files a/firmware/libpros.a and b/firmware/libpros.a differ diff --git a/firmware/okapilib.a b/firmware/okapilib.a index 3b5174da..f09c6211 100644 Binary files a/firmware/okapilib.a and b/firmware/okapilib.a differ diff --git a/firmware/squiggles.mk b/firmware/squiggles.mk new file mode 100644 index 00000000..f970674a --- /dev/null +++ b/firmware/squiggles.mk @@ -0,0 +1 @@ +INCLUDE+=-iquote"$(ROOT)/include/okapi/squiggles" diff --git a/firmware/v5-common.ld b/firmware/v5-common.ld index f1b64f01..e3ea12e1 100644 --- a/firmware/v5-common.ld +++ b/firmware/v5-common.ld @@ -21,7 +21,7 @@ SECTIONS *(.gnu.linkonce.t.*) *(.plt) *(.gnu_warning) - *(.gcc_execpt_table) + *(.gcc_except_table) *(.glue_7) *(.glue_7t) *(.vfp11_veneer) diff --git a/include/api.h b/include/api.h index 241b7cb7..63a658fa 100644 --- a/include/api.h +++ b/include/api.h @@ -8,7 +8,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * All rights reserved. * * This Source Code Form is subject to the terms of the Mozilla Public @@ -40,9 +40,9 @@ #endif /* __cplusplus */ #define PROS_VERSION_MAJOR 3 -#define PROS_VERSION_MINOR 5 -#define PROS_VERSION_PATCH 4 -#define PROS_VERSION_STRING "3.5.4" +#define PROS_VERSION_MINOR 6 +#define PROS_VERSION_PATCH 0 +#define PROS_VERSION_STRING "3.6.0" #define PROS_ERR (INT32_MAX) #define PROS_ERR_F (INFINITY) @@ -53,6 +53,7 @@ #include "pros/ext_adi.h" #include "pros/gps.h" #include "pros/imu.h" +#include "pros/link.h" #include "pros/llemu.h" #include "pros/misc.h" #include "pros/motors.h" @@ -75,6 +76,7 @@ #include "pros/rtos.hpp" #include "pros/screen.hpp" #include "pros/vision.hpp" +#include "pros/link.hpp" #endif #endif // _PROS_API_H_ diff --git a/include/display/lv_misc/lv_color.h b/include/display/lv_misc/lv_color.h index 4a3e3351..e2da8133 100644 --- a/include/display/lv_misc/lv_color.h +++ b/include/display/lv_misc/lv_color.h @@ -378,7 +378,11 @@ static inline uint8_t lv_color_brightness(lv_color_t color) # define LV_COLOR_MAKE(r8, g8, b8) ((lv_color_t){{g8 >> 5, r8 >> 3, b8 >> 3, (g8 >> 2) & 0x7}}) # endif #elif LV_COLOR_DEPTH == 32 -#define LV_COLOR_MAKE(r8, g8, b8) ((lv_color_t){{b8, g8, r8, 0xff}}) /*Fix 0xff alpha*/ +#ifdef __cplusplus +# define LV_COLOR_MAKE(r8, g8, b8) lv_color_t{b8, g8, r8, 0xff} +#else +# define LV_COLOR_MAKE(r8, g8, b8) ((lv_color_t){{b8, g8, r8, 0xff}}) /*Fix 0xff alpha*/ +#endif #endif #else #if LV_COLOR_DEPTH == 1 diff --git a/include/feedbackControllers/feedbackController.hpp b/include/feedbackControllers/feedbackController.hpp new file mode 100644 index 00000000..3029720e --- /dev/null +++ b/include/feedbackControllers/feedbackController.hpp @@ -0,0 +1,48 @@ + + +namespace Pronounce { + class FeedbackController { + private: + protected: + double target = 0.0; + double position = 0.0; + double power; + double maxPower = 1.0; + public: + FeedbackController(/* args */) {} + + virtual double update(double input) { return 0; } + + virtual void reset() {} + + void setTarget(double target) { + this->target = target; + } + + double getTarget() { + return target; + } + + void setPosition(double target) { + this->target = target; + } + + double getPosition() { + return position; + } + + double getPower() { + return power; + } + + void setMaxPower(double maxPower) { + this->maxPower = maxPower; + } + + double getMaxPower() { + return maxPower; + } + + ~FeedbackController() {} + }; +} // namespace Pronounce diff --git a/include/feedbackControllers/flywheelPID.hpp b/include/feedbackControllers/flywheelPID.hpp new file mode 100644 index 00000000..0aed377b --- /dev/null +++ b/include/feedbackControllers/flywheelPID.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include "feedbackController.hpp" +#include "pid.hpp" + +namespace Pronounce { + class FlywheelPID : public PID { + private: + double feedforwardMultiplier; + public: + FlywheelPID() {} + + double update(double input) { + return calculatePidValues(input) + input * feedforwardMultiplier; + } + + ~FlywheelPID() {} + }; +} // namespace Pronounce diff --git a/include/feedbackControllers/pid.hpp b/include/feedbackControllers/pid.hpp index 689364c1..f5e61cad 100644 --- a/include/feedbackControllers/pid.hpp +++ b/include/feedbackControllers/pid.hpp @@ -2,130 +2,134 @@ #include #include "utils/utils.hpp" +#include "feedbackController.hpp" namespace Pronounce { - + /** * @brief PID class - * + * * @author ad101-lab - * + * * Hopefully adding FF later - * + * */ - class PID { - private: - double kP; - double kI; - double kD; - - double target = 0.0; - double position; + class PID : public FeedbackController { + private: + double kP; + double kI; + double kD; - double error; - double totalError = 0.0; - double prevError = 0.0; - double derivitive; + double error; + double totalError = 0.0; + double prevError = 0.0; + double derivitive; - double integralBound = 30.0; - double maxIntegral = 0.3; + double integralBound = 30.0; + double maxIntegral = 0.3; - double power; + double power; bool turnPid = false; - public: - PID(); - PID(double kP, double kI, double kD, double target = 0, double position = 0, bool turnPid = false); - - double update(); - - void operator=(PID pid) { - this->kP = pid.getKP(); - this->kI = pid.getKI(); - this->kD = pid.getKD(); - this->integralBound = pid.getIntegralBound(); - this->maxIntegral = pid.getMaxIntegral(); - } - - void operator=(PID* pid) { - this->kP = pid->getKP(); - this->kI = pid->getKI(); - this->kD = pid->getKD(); - this->integralBound = pid->getIntegralBound(); - this->maxIntegral = pid->getMaxIntegral(); - } - - double getDerivitive() { - return derivitive; - } - - double getError() { - return error; - } - - double getPower() { - return power; - } - - void setPower(double power) { - this->power = power; - } - - double getPosition() { - return this->position; - } - - void setPosition(double position) { - this->position = position; - } - - double getTarget() { - return target; - } - - void setTarget(double target) { - this->target = target; - } - - double getKP() { - return kP; - } - - void setKP(double kP) { - this->kP = kP; - } - - double getKI() { - return kI; - } - - void setKI(double kI) { - this->kI = kI; - } - - double getKD() { - return kD; - } - - void setKD(double kD) { - this->kD = kD; - } - - double getIntegralBound() { - return this->integralBound; - } - - void setIntegralBound(double integralBound) { - this->integralBound = integralBound; - } - - double getMaxIntegral() { - return this->maxIntegral; - } - - void setMaxIntegral(double maxIntegral) { - this->maxIntegral = maxIntegral; - } + + protected: + double calculatePidValues(double input) { + position = input; + + if (turnPid) { + this->error = angleDifference(target, position); + } + else { + this->error = target - position; + } + + this->derivitive = error - prevError; + + if (abs(error) < integralBound) { + totalError += error; + } + else { + totalError = 0; + } + + totalError = abs(totalError) > maxIntegral ? signnum_c(totalError) * maxIntegral : totalError; + + this->power = error * kP + derivitive * kD + totalError * kI; + + prevError = error; + + return this->power; + } + + public: + PID(); + PID(double kP, double kI, double kD, double target = 0, double position = 0, bool turnPid = false); + + double update(double input); + + void operator=(PID pid) { + this->kP = pid.getKP(); + this->kI = pid.getKI(); + this->kD = pid.getKD(); + this->integralBound = pid.getIntegralBound(); + this->maxIntegral = pid.getMaxIntegral(); + } + + void operator=(PID* pid) { + this->kP = pid->getKP(); + this->kI = pid->getKI(); + this->kD = pid->getKD(); + this->integralBound = pid->getIntegralBound(); + this->maxIntegral = pid->getMaxIntegral(); + } + + double getDerivitive() { + return derivitive; + } + + double getError() { + return error; + } + + double getKP() { + return kP; + } + + void setKP(double kP) { + this->kP = kP; + } + + double getKI() { + return kI; + } + + void setKI(double kI) { + this->kI = kI; + } + + double getKD() { + return kD; + } + + void setKD(double kD) { + this->kD = kD; + } + + double getIntegralBound() { + return this->integralBound; + } + + void setIntegralBound(double integralBound) { + this->integralBound = integralBound; + } + + double getMaxIntegral() { + return this->maxIntegral; + } + + void setMaxIntegral(double maxIntegral) { + this->maxIntegral = maxIntegral; + } bool getTurnPid() { return turnPid; @@ -141,8 +145,8 @@ namespace Pronounce { this->derivitive = 0; } - ~PID(); - }; + ~PID(); + }; } // namespace Pronounce diff --git a/include/okapi/api/control/async/asyncLinearMotionProfileController.hpp b/include/okapi/api/control/async/asyncLinearMotionProfileController.hpp index 284e10ea..fd5e391a 100644 --- a/include/okapi/api/control/async/asyncLinearMotionProfileController.hpp +++ b/include/okapi/api/control/async/asyncLinearMotionProfileController.hpp @@ -15,9 +15,7 @@ #include #include -extern "C" { -#include "okapi/pathfinder/include/pathfinder.h" -} +#include "squiggles.hpp" namespace okapi { class AsyncLinearMotionProfileController : public AsyncPositionController { @@ -250,16 +248,8 @@ class AsyncLinearMotionProfileController : public AsyncPositionController; - using SegmentPtr = std::unique_ptr; - - struct TrajectoryPair { - SegmentPtr segment; - int length; - }; - std::shared_ptr logger; - std::map paths{}; + std::map> paths{}; PathfinderLimits limits; std::shared_ptr> output; QLength diameter; @@ -283,7 +273,7 @@ class AsyncLinearMotionProfileController : public AsyncPositionController rate); + virtual void executeSinglePath(const std::vector &path, std::unique_ptr rate); /** * Converts linear "chassis" speed to rotational motor speed. @@ -294,14 +284,6 @@ class AsyncLinearMotionProfileController : public AsyncPositionController &points, const std::string &ipathId, int length); - - /** - * Reads the length of the path in a thread-safe manner. - * - * @param path The path to read from. - * @return The length of the path. - */ - int getPathLength(const TrajectoryPair &path); + getPathErrorMessage(const std::vector &points, const std::string &ipathId, int length); }; } // namespace okapi diff --git a/include/okapi/api/control/async/asyncMotionProfileController.hpp b/include/okapi/api/control/async/asyncMotionProfileController.hpp index 6f42823d..29888703 100644 --- a/include/okapi/api/control/async/asyncMotionProfileController.hpp +++ b/include/okapi/api/control/async/asyncMotionProfileController.hpp @@ -14,11 +14,10 @@ #include "okapi/api/util/logging.hpp" #include "okapi/api/util/timeUtil.hpp" #include +#include #include -extern "C" { -#include "okapi/pathfinder/include/pathfinder.h" -} +#include "squiggles.hpp" namespace okapi { class AsyncMotionProfileController : public AsyncPositionController { @@ -67,6 +66,10 @@ class AsyncMotionProfileController : public AsyncPositionController..csv`. An SD card + * Saves a generated path to a file. Paths are stored as `.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 files in + * @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 path CSV files. `/usd/` is + * 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 @@ -262,17 +265,8 @@ class AsyncMotionProfileController : public AsyncPositionController; - using SegmentPtr = std::unique_ptr; - - struct TrajectoryPair { - SegmentPtr left; - SegmentPtr right; - int length; - }; - std::shared_ptr logger; - std::map paths{}; + std::map> paths{}; PathfinderLimits limits; std::shared_ptr model; ChassisScales scales; @@ -296,7 +290,7 @@ class AsyncMotionProfileController : public AsyncPositionController rate); + virtual void executeSinglePath(const std::vector &path, std::unique_ptr rate); /** * Converts linear chassis speed to rotational motor speed. @@ -307,7 +301,7 @@ class AsyncMotionProfileController : public AsyncPositionController &points, const std::string &ipathId, int length); + getPathErrorMessage(const std::vector &points, const std::string &ipathId, int length); /** * Joins and escapes a directory and file name @@ -319,15 +313,10 @@ class AsyncMotionProfileController : public AsyncPositionController(x) * inch; } +constexpr QLength operator"" _tile(long double x) { + return static_cast(x) * tile; +} constexpr QLength operator"" _mm(unsigned long long int x) { return static_cast(x) * millimeter; } @@ -73,5 +77,8 @@ constexpr QLength operator"" _ft(unsigned long long int x) { constexpr QLength operator"" _in(unsigned long long int x) { return static_cast(x) * inch; } +constexpr QLength operator"" _tile(unsigned long long int x) { + return static_cast(x) * tile; +} } // namespace literals } // namespace okapi diff --git a/include/okapi/api/util/logging.hpp b/include/okapi/api/util/logging.hpp index b850b7cd..b3a6c303 100644 --- a/include/okapi/api/util/logging.hpp +++ b/include/okapi/api/util/logging.hpp @@ -78,7 +78,7 @@ class Logger { "%ld (%s) DEBUG: %s\n", static_cast(timer->millis().convert(millisecond)), CrossplatformThread::getName().c_str(), - ilazyMessage.c_str()); + ilazyMessage().c_str()); } } diff --git a/include/okapi/impl/device/motor/motorGroup.hpp b/include/okapi/impl/device/motor/motorGroup.hpp index 35469622..6187c4c6 100644 --- a/include/okapi/impl/device/motor/motorGroup.hpp +++ b/include/okapi/impl/device/motor/motorGroup.hpp @@ -372,6 +372,13 @@ class MotorGroup : public AbstractMotor { */ void controllerSet(double ivalue) override; + /** + * Gets the number of motors in the motor group. + * + * @return size_t + */ + size_t getSize(); + /** * Get the encoder associated with the first motor in this group. * diff --git a/include/okapi/pathfinder/include/pathfinder.h b/include/okapi/pathfinder/include/pathfinder.h deleted file mode 100644 index 8db9223f..00000000 --- a/include/okapi/pathfinder/include/pathfinder.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef PATHFINDER_H_DEF -#define PATHFINDER_H_DEF - -#include "okapi/pathfinder/include/pathfinder/mathutil.h" -#include "okapi/pathfinder/include/pathfinder/structs.h" - -#include "okapi/pathfinder/include/pathfinder/fit.h" -#include "okapi/pathfinder/include/pathfinder/spline.h" -#include "okapi/pathfinder/include/pathfinder/trajectory.h" - -#include "okapi/pathfinder/include/pathfinder/modifiers/tank.h" -#include "okapi/pathfinder/include/pathfinder/modifiers/swerve.h" - -#include "okapi/pathfinder/include/pathfinder/followers/encoder.h" -#include "okapi/pathfinder/include/pathfinder/followers/distance.h" - -#include "okapi/pathfinder/include/pathfinder/io.h" - -#endif \ No newline at end of file diff --git a/include/okapi/pathfinder/include/pathfinder/fit.h b/include/okapi/pathfinder/include/pathfinder/fit.h deleted file mode 100644 index 39eea48f..00000000 --- a/include/okapi/pathfinder/include/pathfinder/fit.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef PATHFINDER_FIT_H_DEF -#define PATHFINDER_FIT_H_DEF - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "okapi/pathfinder/include/pathfinder/lib.h" -#include "okapi/pathfinder/include/pathfinder/structs.h" - -CAPI void pf_fit_hermite_pre(Waypoint a, Waypoint b, Spline *s); -CAPI void pf_fit_hermite_cubic(Waypoint a, Waypoint b, Spline *s); -CAPI void pf_fit_hermite_quintic(Waypoint a, Waypoint b, Spline *s); - -#define FIT_HERMITE_CUBIC &pf_fit_hermite_cubic -#define FIT_HERMITE_QUINTIC &pf_fit_hermite_quintic - -#ifdef __cplusplus -} -#endif - -#endif \ No newline at end of file diff --git a/include/okapi/pathfinder/include/pathfinder/followers/distance.h b/include/okapi/pathfinder/include/pathfinder/followers/distance.h deleted file mode 100644 index 21c6e6e6..00000000 --- a/include/okapi/pathfinder/include/pathfinder/followers/distance.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef PATHFINDER_FOL_DISTANCE_H_DEF -#define PATHFINDER_FOL_DISTANCE_H_DEF - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "okapi/pathfinder/include/pathfinder/lib.h" -#include "okapi/pathfinder/include/pathfinder/structs.h" - -CAPI typedef struct { - double kp, ki, kd, kv, ka; -} FollowerConfig; - -CAPI typedef struct { - double last_error, heading, output; - int segment, finished; -} DistanceFollower; - -CAPI double pathfinder_follow_distance(FollowerConfig c, DistanceFollower *follower, Segment *trajectory, int trajectory_length, double distance); - -CAPI double pathfinder_follow_distance2(FollowerConfig c, DistanceFollower *follower, Segment segment, int trajectory_length, double distance); - -#ifdef __cplusplus -} -#endif - -#endif \ No newline at end of file diff --git a/include/okapi/pathfinder/include/pathfinder/followers/encoder.h b/include/okapi/pathfinder/include/pathfinder/followers/encoder.h deleted file mode 100644 index c1d9cfd9..00000000 --- a/include/okapi/pathfinder/include/pathfinder/followers/encoder.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef PATHFINDER_FOL_ENCODER_H_DEF -#define PATHFINDER_FOL_ENCODER_H_DEF - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "okapi/pathfinder/include/pathfinder/structs.h" - -typedef struct { - int initial_position, ticks_per_revolution; - double wheel_circumference; - double kp, ki, kd, kv, ka; -} EncoderConfig; - -typedef struct { - double last_error, heading, output; - int segment, finished; -} EncoderFollower; - - -double pathfinder_follow_encoder(EncoderConfig c, EncoderFollower *follower, Segment *trajectory, int trajectory_length, int encoder_tick); - -double pathfinder_follow_encoder2(EncoderConfig c, EncoderFollower *follower, Segment segment, int trajectory_length, int encoder_tick); - -#ifdef __cplusplus -} -#endif - -#endif \ No newline at end of file diff --git a/include/okapi/pathfinder/include/pathfinder/io.h b/include/okapi/pathfinder/include/pathfinder/io.h deleted file mode 100644 index 210bb269..00000000 --- a/include/okapi/pathfinder/include/pathfinder/io.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef PATHFINDER_IO_H_DEF -#define PATHFINDER_IO_H_DEF - -#include -#include -#include - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "okapi/pathfinder/include/pathfinder/structs.h" -#include "okapi/pathfinder/include/pathfinder/lib.h" - -#define CSV_LEADING_STRING "dt,x,y,position,velocity,acceleration,jerk,heading\n" - -CAPI void intToBytes(int n, char *bytes); -CAPI int bytesToInt(char *bytes); -CAPI void longToBytes(unsigned long long n, char *bytes); -CAPI unsigned long long bytesToLong(char *bytes); -CAPI double longToDouble(unsigned long long l); -CAPI unsigned long long doubleToLong(double d); -CAPI void doubleToBytes(double n, char *bytes); -CAPI double bytesToDouble(char *bytes); - -CAPI void pathfinder_serialize(FILE *fp, Segment *trajectory, int trajectory_length); -CAPI int pathfinder_deserialize(FILE *fp, Segment *target); - -CAPI void pathfinder_serialize_csv(FILE *fp, Segment *trajectory, int trajectory_length); -CAPI int pathfinder_deserialize_csv(FILE *fp, Segment *target); - -#ifdef __cplusplus -} -#endif - -#endif \ No newline at end of file diff --git a/include/okapi/pathfinder/include/pathfinder/lib.h b/include/okapi/pathfinder/include/pathfinder/lib.h deleted file mode 100644 index 0ba47afe..00000000 --- a/include/okapi/pathfinder/include/pathfinder/lib.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef PATHFINDER_LIB_H_DEF -#define PATHFINDER_LIB_H_DEF - -#if defined(WIN32) || defined(_WIN32) || defined(__WIN32) && !defined(__CYGWIN__) - #define CAPI __declspec(dllexport) -#else - #define CAPI -#endif - -#endif \ No newline at end of file diff --git a/include/okapi/pathfinder/include/pathfinder/mathutil.h b/include/okapi/pathfinder/include/pathfinder/mathutil.h deleted file mode 100644 index db3ebcbc..00000000 --- a/include/okapi/pathfinder/include/pathfinder/mathutil.h +++ /dev/null @@ -1,29 +0,0 @@ -#include - -#ifndef PATHFINDER_MATH_UTIL_H_DEF -#define PATHFINDER_MATH_UTIL_H_DEF - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "okapi/pathfinder/include/pathfinder/lib.h" - -#define PI 3.14159265358979323846 -#define TAU PI*2 - -#define MIN(a,b) (((a)<(b))?(a):(b)) -#define MAX(a,b) (((a)>(b))?(a):(b)) - -CAPI double bound_radians(double angle); - -CAPI double r2d(double angleInRads); - -CAPI double d2r(double angleInDegrees); - -#ifdef __cplusplus -} -#endif - -#endif \ No newline at end of file diff --git a/include/okapi/pathfinder/include/pathfinder/modifiers/swerve.h b/include/okapi/pathfinder/include/pathfinder/modifiers/swerve.h deleted file mode 100644 index 60be7500..00000000 --- a/include/okapi/pathfinder/include/pathfinder/modifiers/swerve.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef PATHFINDER_MOD_SWERVE_H_DEF -#define PATHFINDER_MOD_SWERVE_H_DEF - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "okapi/pathfinder/include/pathfinder/lib.h" -#include "okapi/pathfinder/include/pathfinder/structs.h" - -CAPI typedef enum { - SWERVE_DEFAULT -} SWERVE_MODE; - -CAPI void pathfinder_modify_swerve(Segment *original, int length, Segment *front_left, Segment *front_right, - Segment *back_left, Segment *back_right, double wheelbase_width, double wheelbase_depth, SWERVE_MODE mode); - -#ifdef __cplusplus -} -#endif - -#endif \ No newline at end of file diff --git a/include/okapi/pathfinder/include/pathfinder/modifiers/tank.h b/include/okapi/pathfinder/include/pathfinder/modifiers/tank.h deleted file mode 100644 index f72db638..00000000 --- a/include/okapi/pathfinder/include/pathfinder/modifiers/tank.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef PATHFINDER_MOD_TANK_H_DEF -#define PATHFINDER_MOD_TANK_H_DEF - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "okapi/pathfinder/include/pathfinder/lib.h" -#include "okapi/pathfinder/include/pathfinder/structs.h" - -CAPI void pathfinder_modify_tank(Segment *original, int length, Segment *left, Segment *right, double wheelbase_width); - -#ifdef __cplusplus -} -#endif - -#endif \ No newline at end of file diff --git a/include/okapi/pathfinder/include/pathfinder/spline.h b/include/okapi/pathfinder/include/pathfinder/spline.h deleted file mode 100644 index 8c68ae12..00000000 --- a/include/okapi/pathfinder/include/pathfinder/spline.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef PATHFINDER_SPLINE_H_DEF -#define PATHFINDER_SPLINE_H_DEF - -#include "okapi/pathfinder/include/pathfinder/lib.h" -#include "okapi/pathfinder/include/pathfinder/structs.h" - -#define PATHFINDER_SAMPLES_FAST (int)1000 -#define PATHFINDER_SAMPLES_LOW (int)PATHFINDER_SAMPLES_FAST*10 -#define PATHFINDER_SAMPLES_HIGH (int)PATHFINDER_SAMPLES_LOW*10 - -CAPI Coord pf_spline_coords(Spline s, double percentage); -CAPI double pf_spline_deriv(Spline s, double percentage); -CAPI double pf_spline_deriv_2(double a, double b, double c, double d, double e, double k, double p); -CAPI double pf_spline_angle(Spline s, double percentage); - -CAPI double pf_spline_distance(Spline *s, int sample_count); -CAPI double pf_spline_progress_for_distance(Spline s, double distance, int sample_count); - -#endif \ No newline at end of file diff --git a/include/okapi/pathfinder/include/pathfinder/structs.h b/include/okapi/pathfinder/include/pathfinder/structs.h deleted file mode 100644 index 5c424d39..00000000 --- a/include/okapi/pathfinder/include/pathfinder/structs.h +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef PATHFINDER_STRUCT_H_DEF -#define PATHFINDER_STRUCT_H_DEF - -#include "okapi/pathfinder/include/pathfinder/lib.h" - -CAPI typedef struct { - double x, y, angle; -} Waypoint; - -CAPI typedef struct { - double a, b, c, d, e; - double x_offset, y_offset, angle_offset, knot_distance, arc_length; -} Spline; - -CAPI typedef struct { - double x, y; -} Coord; - -CAPI typedef struct { - double dt, x, y, position, velocity, acceleration, jerk, heading; -} Segment; - -CAPI typedef struct { - double dt, max_v, max_a, max_j, src_v, src_theta, dest_pos, dest_v, dest_theta; - int sample_count; -} TrajectoryConfig; - -CAPI typedef struct { - int filter1, filter2, length; - double dt, u, v, impulse; -} TrajectoryInfo; - -CAPI typedef struct { - Spline *saptr; - double *laptr; - double totalLength; - int length; - int path_length; - TrajectoryInfo info; - TrajectoryConfig config; -} TrajectoryCandidate; - -#endif \ No newline at end of file diff --git a/include/okapi/pathfinder/include/pathfinder/trajectory.h b/include/okapi/pathfinder/include/pathfinder/trajectory.h deleted file mode 100644 index fb9a410f..00000000 --- a/include/okapi/pathfinder/include/pathfinder/trajectory.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef PATHFINDER_TRAJECTORY_H_DEF -#define PATHFINDER_TRAJECTORY_H_DEF - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "okapi/pathfinder/include/pathfinder/lib.h" -#include "okapi/pathfinder/include/pathfinder/structs.h" - -CAPI int pathfinder_prepare(Waypoint *path, int path_length, void (*fit)(Waypoint,Waypoint,Spline*), int sample_count, double dt, - double max_velocity, double max_acceleration, double max_jerk, TrajectoryCandidate *cand); -CAPI int pathfinder_generate(TrajectoryCandidate *c, Segment *segments); - -CAPI void pf_trajectory_copy(Segment *src, Segment *dest, int length); - -CAPI TrajectoryInfo pf_trajectory_prepare(TrajectoryConfig c); -CAPI int pf_trajectory_create(TrajectoryInfo info, TrajectoryConfig c, Segment *seg); -CAPI int pf_trajectory_fromSecondOrderFilter(int filter_1_l, int filter_2_l, - double dt, double u, double v, double impulse, int len, Segment *t); - -#ifdef __cplusplus -} -#endif - -#endif \ No newline at end of file diff --git a/include/okapi/squiggles/constraints.hpp b/include/okapi/squiggles/constraints.hpp new file mode 100644 index 00000000..f59089be --- /dev/null +++ b/include/okapi/squiggles/constraints.hpp @@ -0,0 +1,65 @@ +/** + * Copyright 2020 Jonathan Bayless + * + * Use of this source code is governed by an MIT-style license that can be found + * in the LICENSE file or at https://opensource.org/licenses/MIT. + */ +#ifndef _SQUIGGLES_CONSTRAINTS_HPP_ +#define _SQUIGGLES_CONSTRAINTS_HPP_ + +#include +#include + +namespace squiggles { +struct Constraints { + /** + * Defines the motion constraints for a path. + * + * @param imax_vel The maximum allowable velocity for the robot in meters per + * second. + * @param imax_accel The maximum allowable acceleration for the robot in + * meters per second per second. + * @param imax_jerk The maximum allowable jerk for the robot in meters per + * second per second per second (m/s^3). + * @param imax_curvature The maximum allowable change in heading in radians + * per second. This is not set to the numeric limits by + * default as that will allow for wild paths. + * @param imin_accel The minimum allowable acceleration for the robot in + * meters per second per second. + */ + Constraints(double imax_vel, + double imax_accel = std::numeric_limits::max(), + double imax_jerk = std::numeric_limits::max(), + double imax_curvature = 1000, + double imin_accel = std::nan("")) + : max_vel(imax_vel), + max_accel(imax_accel), + max_jerk(imax_jerk), + max_curvature(imax_curvature) { + if (imax_accel == std::numeric_limits::max()) { + min_accel = std::numeric_limits::lowest(); + } else { + min_accel = std::isnan(imin_accel) ? -imax_accel : imin_accel; + } + } + + /** + * Serializes the Constraints data for debugging. + * + * @return The Constraints data. + */ + std::string to_string() const { + return "Constraints: {max_vel: " + std::to_string(max_vel) + + ", max_accel: " + std::to_string(max_accel) + + ", max_jerk: " + std::to_string(max_jerk) + + ", min_accel: " + std::to_string(min_accel) + "}"; + } + + double max_vel; + double max_accel; + double max_jerk; + double min_accel; + double max_curvature; +}; +} // namespace squiggles +#endif diff --git a/include/okapi/squiggles/geometry/controlvector.hpp b/include/okapi/squiggles/geometry/controlvector.hpp new file mode 100644 index 00000000..c279bd49 --- /dev/null +++ b/include/okapi/squiggles/geometry/controlvector.hpp @@ -0,0 +1,62 @@ +/** + * Copyright 2020 Jonathan Bayless + * + * Use of this source code is governed by an MIT-style license that can be found + * in the LICENSE file or at https://opensource.org/licenses/MIT. + */ +#ifndef _GEOMETRY_CONTROL_VECTOR_HPP_ +#define _GEOMETRY_CONTROL_VECTOR_HPP_ + +#include +#include + +#include "pose.hpp" + +namespace squiggles { +class ControlVector { + public: + /** + * A vector used to specify a state along a hermite spline. + * + * @param ipose The 2D position and heading. + * @param ivel The velocity component of the vector. + * @param iaccel The acceleration component of the vector. + * @param ijerk The jerk component of the vector. + */ + ControlVector(Pose ipose, + double ivel = std::nan(""), + double iaccel = 0.0, + double ijerk = 0.0) + : pose(ipose), vel(ivel), accel(iaccel), jerk(ijerk) {} + + ControlVector() = default; + + /** + * Serializes the Control Vector data for debugging. + * + * @return The Control Vector data. + */ + std::string to_string() const { + return "ControlVector: {" + pose.to_string() + + ", v: " + std::to_string(vel) + ", a: " + std::to_string(accel) + + ", j: " + std::to_string(jerk) + "}"; + } + + std::string to_csv() const { + return pose.to_csv() + "," + std::to_string(vel) + "," + + std::to_string(accel) + "," + std::to_string(jerk); + } + + bool operator==(const ControlVector& other) const { + return pose == other.pose && nearly_equal(vel, other.vel) && + nearly_equal(accel, other.accel) && nearly_equal(jerk, other.jerk); + } + + Pose pose; + double vel; + double accel; + double jerk; +}; +} // namespace squiggles + +#endif \ No newline at end of file diff --git a/include/okapi/squiggles/geometry/pose.hpp b/include/okapi/squiggles/geometry/pose.hpp new file mode 100644 index 00000000..b3b76907 --- /dev/null +++ b/include/okapi/squiggles/geometry/pose.hpp @@ -0,0 +1,67 @@ +/** + * Copyright 2020 Jonathan Bayless + * + * Use of this source code is governed by an MIT-style license that can be found + * in the LICENSE file or at https://opensource.org/licenses/MIT. + */ +#ifndef _GEOMETRY_POSE_HPP_ +#define _GEOMETRY_POSE_HPP_ + +#include +#include + +#include "math/utils.hpp" + +namespace squiggles { +class Pose { + public: + /** + * Specifies a point and heading in 2D space. + * + * @param ix The x position of the point in meters. + * @param iy The y position of the point in meters. + * @param iyaw The heading at the point in radians. + */ + Pose(double ix, double iy, double iyaw) : x(ix), y(iy), yaw(iyaw) {} + + Pose() = default; + + /** + * Calculates the Euclidean distance between this pose and another. + * + * @param other The point from which the distance will be calculated. + * + * @return The distance between this pose and Other. + */ + double dist(const Pose& other) const { + return std::sqrt((x - other.x) * (x - other.x) + + (y - other.y) * (y - other.y)); + } + + /** + * Serializes the Pose data for debugging. + * + * @return The Pose data. + */ + std::string to_string() const { + return "Pose: {x: " + std::to_string(x) + ", y: " + std::to_string(y) + + ", yaw: " + std::to_string(yaw) + "}"; + } + + std::string to_csv() const { + return std::to_string(x) + "," + std::to_string(y) + "," + + std::to_string(yaw); + } + + bool operator==(const Pose& other) const { + return nearly_equal(x, other.x) && nearly_equal(y, other.y) && + nearly_equal(yaw, other.yaw); + } + + double x; + double y; + double yaw; +}; +} // namespace squiggles + +#endif \ No newline at end of file diff --git a/include/okapi/squiggles/geometry/profilepoint.hpp b/include/okapi/squiggles/geometry/profilepoint.hpp new file mode 100644 index 00000000..e6eed094 --- /dev/null +++ b/include/okapi/squiggles/geometry/profilepoint.hpp @@ -0,0 +1,100 @@ +/** + * Copyright 2020 Jonathan Bayless + * + * Use of this source code is governed by an MIT-style license that can be found + * in the LICENSE file or at https://opensource.org/licenses/MIT. + */ +#ifndef _GEOMETRY_PROFILE_POINT_HPP_ +#define _GEOMETRY_PROFILE_POINT_HPP_ + +#include +#include +#include + +#include "controlvector.hpp" +#include "math/utils.hpp" + +namespace squiggles { +struct ProfilePoint { + /** + * Defines a state along a motion profiled path. + * + * @param ivector The pose and associated dynamics at this state in the path. + * @param iwheel_velocities The component of the robot's velocity provided by + * each wheel in meters per second. + * @param icurvature The degree to which the curve deviates from a straight + * line at this point in 1 / meters. + * @param itime The timestamp for this state relative to the start of the + * path in seconds. + */ + ProfilePoint(ControlVector ivector, + std::vector iwheel_velocities, + double icurvature, + double itime) + : vector(ivector), + wheel_velocities(iwheel_velocities), + curvature(icurvature), + time(itime) {} + + ProfilePoint() = default; + + /** + * Serializes the Profile Point data for debugging. + * + * @return The Profile Point data. + */ + std::string to_string() const { + std::string wheels = "{"; + for (auto& w : wheel_velocities) { + wheels += std::to_string(w); + wheels += ", "; + } + wheels += "}"; + return "ProfilePoint: {" + vector.to_string() + ", wheels: " + wheels + + ", k: " + std::to_string(curvature) + + ", t: " + std::to_string(time) + "}"; + } + + std::string to_csv() const { + std::string wheels = ""; + for (auto& w : wheel_velocities) { + wheels += ","; + wheels += std::to_string(w); + } + return vector.to_csv() + "," + std::to_string(curvature) + "," + + std::to_string(time) + wheels; + } + + bool operator==(const ProfilePoint& other) const { + for (std::size_t i = 0; i < wheel_velocities.size(); ++i) { + if (!nearly_equal(wheel_velocities[i], other.wheel_velocities[i])) { + return false; + } + } + return vector == other.vector && nearly_equal(curvature, other.curvature) && + nearly_equal(time, other.time); + } + + friend std::ostream& operator<<(std::ostream& os, const ProfilePoint& p) { + return os << "ProfilePoint(ControlVector(Pose(" + + std::to_string(p.vector.pose.x) + "," + + std::to_string(p.vector.pose.y) + "," + + std::to_string(p.vector.pose.yaw) + ")," + + std::to_string(p.vector.vel) + "," + + std::to_string(p.vector.accel) + "," + + std::to_string(p.vector.jerk) + "),{" + + std::to_string(p.wheel_velocities[0]) + "," + + std::to_string(p.wheel_velocities[1]) + "}," + + std::to_string(p.curvature) + "," + std::to_string(p.time) + + "),"; + // return os << p.to_string(); + } + + ControlVector vector; + std::vector wheel_velocities; + double curvature; + double time; +}; +} // namespace squiggles + +#endif \ No newline at end of file diff --git a/include/okapi/squiggles/io.hpp b/include/okapi/squiggles/io.hpp new file mode 100644 index 00000000..c22ed970 --- /dev/null +++ b/include/okapi/squiggles/io.hpp @@ -0,0 +1,56 @@ +/** + * Copyright 2020 Jonathan Bayless + * + * Use of this source code is governed by an MIT-style license that can be found + * in the LICENSE file or at https://opensource.org/licenses/MIT. + */ +#ifndef _SQUIGGLES_IO_HPP_ +#define _SQUIGGLES_IO_HPP_ + +#include +#include + +#include "geometry/profilepoint.hpp" + +namespace squiggles { +/** + * Writes the path data to a CSV file. + * + * @param out The output stream to write the CSV data to. This is usually a + * file. + * @param path The path to serialized + * + * @return 0 if the path was serialized succesfully or -1 if an error occurred. + */ +int serialize_path(std::ostream& out, std::vector path); + +/** + * Converts CSV data into a path. + * + * @param in The input stream containing the CSV data. This is usually a file. + * + * @return The path specified by the CSV data or std::nullopt if de-serializing + * the path was unsuccessful. + */ +std::optional> deserialize_path(std::istream& in); + +/** + * Converts CSV data from the Pathfinder library's format to a Squiggles path. + * + * NOTE: this code translates data from Jaci Brunning's Pathfinder library. + * The source for that library can be found at: + * https://github.com/JaciBrunning/Pathfinder/ + * + * @param left The input stream containing the left wheels' CSV data. This is + * usually a file. + * @param right The input stream containing the right wheels' CSV data. This is + * usually a file. + * + * @return The path specified by the CSV data or std::nullopt if de-serializing + * the path was unsuccessful. + */ +std::optional> +deserialize_pathfinder_path(std::istream& left, std::istream& right); +} // namespace squiggles + +#endif diff --git a/include/okapi/squiggles/math/quinticpolynomial.hpp b/include/okapi/squiggles/math/quinticpolynomial.hpp new file mode 100644 index 00000000..f12580fc --- /dev/null +++ b/include/okapi/squiggles/math/quinticpolynomial.hpp @@ -0,0 +1,65 @@ +/** + * Copyright 2020 Jonathan Bayless + * + * Use of this source code is governed by an MIT-style license that can be found + * in the LICENSE file or at https://opensource.org/licenses/MIT. + */ +#ifndef _MATH_QUINTIC_POLYNOMIAL_HPP_ +#define _MATH_QUINTIC_POLYNOMIAL_HPP_ + +#include + +namespace squiggles { +class QuinticPolynomial { + public: + /** + * Defines the polynomial function for a spline in one dimension. + * + * @param s_p The starting position of the curve in meters. + * @param s_v The starting velocity of the curve in meters per second. + * @param s_a The starting acceleration of the curve in meters per second per + * second. + * @param g_p The goal or ending position of the curve in meters. + * @param g_v The goal or ending velocity of the curve in meters per second. + * @param g_a The goal or ending acceleration of the curve in meters per + * second per second. + * @param t The desired duration for the curve in seconds. + */ + QuinticPolynomial(double s_p, + double s_v, + double s_a, + double g_p, + double g_v, + double g_a, + double t); + + /** + * Calculates the values of the polynomial and its derivatives at the given + * time stamp. + */ + double calc_point(double t); + double calc_first_derivative(double t); + double calc_second_derivative(double t); + double calc_third_derivative(double t); + + /** + * Serializes the Quintic Polynomial data for debugging. + * + * @return The Quintic Polynomial data. + */ + std::string to_string() const { + return "QuinticPolynomial: {0: " + std::to_string(a0) + + " 1: " + std::to_string(a1) + " 2: " + std::to_string(a2) + + " 3: " + std::to_string(a3) + " 4: " + std::to_string(a4) + + " 5: " + std::to_string(a5) + "}"; + } + + protected: + /** + * The coefficients for each term of the polynomial. + */ + double a0, a1, a2, a3, a4, a5; +}; +} // namespace squiggles + +#endif \ No newline at end of file diff --git a/include/okapi/squiggles/math/utils.hpp b/include/okapi/squiggles/math/utils.hpp new file mode 100644 index 00000000..cca3da0c --- /dev/null +++ b/include/okapi/squiggles/math/utils.hpp @@ -0,0 +1,61 @@ +/** + * Copyright 2020 Jonathan Bayless + * + * Use of this source code is governed by an MIT-style license that can be found + * in the LICENSE file or at https://opensource.org/licenses/MIT. + */ +#ifndef _MATH_UTILS_HPP_ +#define _MATH_UTILS_HPP_ + +#include +#include + +namespace squiggles { +/** + * Returns the sign value of the given value. + * + * @return 1 if the value is positive, -1 if the value is negative, and 0 if + * the value is 0. + */ +template inline int sgn(T v) { + return (v > T(0)) - (v < T(0)); +} + +inline bool +nearly_equal(const double& a, const double& b, double epsilon = 1e-5) { + return std::fabs(a - b) < epsilon; +} +} // namespace squiggles + +namespace std { +// Copied from https://github.com/emsr/cxx_linear +template +constexpr std::enable_if_t< + std::is_floating_point_v<_Float> && + __cplusplus <= 201703L, // Only defines this function if C++ standard < 20 + _Float> +lerp(_Float __a, _Float __b, _Float __t) { + if (std::isnan(__a) || std::isnan(__b) || std::isnan(__t)) + return std::numeric_limits<_Float>::quiet_NaN(); + else if ((__a <= _Float{0} && __b >= _Float{0}) || + (__a >= _Float{0} && __b <= _Float{0})) + // ab <= 0 but product could overflow. +#ifndef FMA + return __t * __b + (_Float{1} - __t) * __a; +#else + return std::fma(__t, __b, (_Float{1} - __t) * __a); +#endif + else if (__t == _Float{1}) + return __b; + else { // monotonic near t == 1. +#ifndef FMA + const auto __x = __a + __t * (__b - __a); +#else + const auto __x = std::fma(__t, __b - __a, __a); +#endif + return (__t > _Float{1}) == (__b > __a) ? std::max(__b, __x) + : std::min(__b, __x); + } +} +} // namespace std +#endif \ No newline at end of file diff --git a/include/okapi/squiggles/physicalmodel/passthroughmodel.hpp b/include/okapi/squiggles/physicalmodel/passthroughmodel.hpp new file mode 100644 index 00000000..1db2a17a --- /dev/null +++ b/include/okapi/squiggles/physicalmodel/passthroughmodel.hpp @@ -0,0 +1,38 @@ +/** + * Copyright 2020 Jonathan Bayless + * + * Use of this source code is governed by an MIT-style license that can be found + * in the LICENSE file or at https://opensource.org/licenses/MIT. + */ +#ifndef _PHYSICAL_MODEL_PASSTHROUGH_MODEL_HPP_ +#define _PHYSICAL_MODEL_PASSTHROUGH_MODEL_HPP_ + +#include "physicalmodel/physicalmodel.hpp" + +namespace squiggles { +class PassthroughModel : public PhysicalModel { + public: + /** + * Defines a Physical Model that imposes no constraints of its own. + */ + PassthroughModel() = default; + + Constraints constraints([[maybe_unused]] const Pose pose, + [[maybe_unused]] double curvature, + double vel) override { + return Constraints(vel); + }; + + std::vector + linear_to_wheel_vels([[maybe_unused]] double lin_vel, + [[maybe_unused]] double curvature) override { + return std::vector{}; + } + + std::string to_string() const override { + return "PassthroughModel {}"; + } +}; +} // namespace squiggles + +#endif diff --git a/include/okapi/squiggles/physicalmodel/physicalmodel.hpp b/include/okapi/squiggles/physicalmodel/physicalmodel.hpp new file mode 100644 index 00000000..5c22a4ca --- /dev/null +++ b/include/okapi/squiggles/physicalmodel/physicalmodel.hpp @@ -0,0 +1,43 @@ +/** + * Copyright 2020 Jonathan Bayless + * + * Use of this source code is governed by an MIT-style license that can be found + * in the LICENSE file or at https://opensource.org/licenses/MIT. + */ +#ifndef _PHYSICAL_MODEL_PHYSICAL_MODEL_HPP_ +#define _PHYSICAL_MODEL_PHYSICAL_MODEL_HPP_ + +#include "constraints.hpp" +#include "geometry/pose.hpp" + +namespace squiggles { +class PhysicalModel { + public: + /** + * Calculate a set of stricter constraints for the path at the given state + * than the general constraints based on the robot's kinematics. + * + * @param pose The 2D pose for this state in the path. + * @param curvature The change in heading at this state in the path in 1 / + * meters. + * @param vel The linear velocity at this state in the path in meters per + * second. + */ + virtual Constraints + constraints(const Pose pose, double curvature, double vel) = 0; + + /** + * Converts a linear velocity and desired curvature into the component for + * each wheel of the robot. + * + * @param linear The linear velocity for the robot in meters per second. + * @param curvature The change in heading for the robot in 1 / meters. + */ + virtual std::vector linear_to_wheel_vels(double linear, + double curvature) = 0; + + virtual std::string to_string() const = 0; +}; +} // namespace squiggles + +#endif diff --git a/include/okapi/squiggles/physicalmodel/tankmodel.hpp b/include/okapi/squiggles/physicalmodel/tankmodel.hpp new file mode 100644 index 00000000..9f0709b2 --- /dev/null +++ b/include/okapi/squiggles/physicalmodel/tankmodel.hpp @@ -0,0 +1,45 @@ +/** + * Copyright 2020 Jonathan Bayless + * + * Use of this source code is governed by an MIT-style license that can be found + * in the LICENSE file or at https://opensource.org/licenses/MIT. + */ +#ifndef _PHYSICAL_MODEL_TANK_MODEL_HPP_ +#define _PHYSICAL_MODEL_TANK_MODEL_HPP_ + +#include +#include + +#include "physicalmodel/physicalmodel.hpp" + +namespace squiggles { +class TankModel : public PhysicalModel { + public: + /** + * Defines a model of a tank drive or differential drive robot. + * + * @param itrack_width The distance between the the wheels on each side of the + * robot in meters. + * @param ilinear_constraints The maximum values for the robot's movement. + */ + TankModel(double itrack_width, Constraints ilinear_constraints); + + Constraints + constraints(const Pose pose, double curvature, double vel) override; + + std::vector linear_to_wheel_vels(double lin_vel, + double curvature) override; + + std::string to_string() const override; + + private: + double vel_constraint(const Pose pose, double curvature, double vel); + std::tuple + accel_constraint(const Pose pose, double curvature, double vel) const; + + double track_width; + Constraints linear_constraints; +}; +} // namespace squiggles + +#endif \ No newline at end of file diff --git a/include/okapi/squiggles/spline.hpp b/include/okapi/squiggles/spline.hpp new file mode 100644 index 00000000..4ee5991f --- /dev/null +++ b/include/okapi/squiggles/spline.hpp @@ -0,0 +1,310 @@ +/** + * Copyright 2020 Jonathan Bayless + * + * Use of this source code is governed by an MIT-style license that can be found + * in the LICENSE file or at https://opensource.org/licenses/MIT. + */ +#ifndef _SQUIGGLES_SPLINE_HPP_ +#define _SQUIGGLES_SPLINE_HPP_ + +#include +#include +#include + +#include "constraints.hpp" +#include "geometry/controlvector.hpp" +#include "geometry/profilepoint.hpp" +#include "math/quinticpolynomial.hpp" +#include "physicalmodel/passthroughmodel.hpp" +#include "physicalmodel/physicalmodel.hpp" + +namespace squiggles { +class SplineGenerator { + public: + /** + * Generates curves that match the given motion constraints. + * + * @param iconstraints The maximum allowable values for the robot's motion. + * @param imodel The robot's physical characteristics and constraints + * @param idt The difference in time in seconds between each state for the + * generated paths. + */ + SplineGenerator(Constraints iconstraints, + std::shared_ptr imodel = + std::make_shared(), + double idt = 0.1); + + /** + * Creates a motion profiled path between the given waypoints. + * + * @param iwaypoints The list of poses that the robot should reach along the + * path. + * @param fast If true, the path optimization process will stop as soon as the + * constraints are met. If false, the optimizer will find the + * smoothest possible path between the points. + * + * @return A series of robot states defining a path between the poses. + */ + std::vector generate(std::vector iwaypoints, + bool fast = false); + std::vector generate(std::initializer_list iwaypoints, + bool fast = false); + + /** + * Creates a motion profiled path between the given waypoints. + * + * @param iwaypoints The list of vectors that the robot should reach along the + * path. + * + * @return A series of robot states defining a path between the vectors. + */ + std::vector generate(std::vector iwaypoints); + std::vector + generate(std::initializer_list iwaypoints); + + protected: + /** + * The maximum allowable values for the robot's motion. + */ + Constraints constraints; + + /** + * Defines the physical structure of the robot and translates the linear + * kinematics to wheel velocities. + */ + std::shared_ptr model; + + /** + * The time difference between each value in the generated path. + */ + double dt; + + /** + * The minimum and maximum durations for a path to take. A larger range allows + * for longer possible paths at the expense of a longer path generation time. + */ + const int T_MIN = 2; + const int T_MAX = 15; + const int MAX_GRAD_DESCENT_ITERATIONS = 10; + + /** + * This is factor is used to create a "dummy velocity" in the initial path + * generation step one or both of the preferred start or end velocities is + * zero. The velocity will be replaced with the preferred start/end velocity + * in parameterization but a nonzero velocity is needed for the spline + * calculation. + * + * This was 1.2 in the WPILib example but that large of a value seems to + * create wild paths, 0.12 worked better in testing with VEX-sized paths. + */ + public: + const double K_DEFAULT_VEL = 1.0; + + /** + * The output of the initial, "naive" generation step. We discard the + * derivative values to replace them with values from a motion profile. + */ + + struct GeneratedPoint { + GeneratedPoint(Pose ipose, double icurvature = 0.0) + : pose(ipose), curvature(icurvature) {} + + std::string to_string() const { + return "GeneratedPoint: {" + pose.to_string() + + ", curvature: " + std::to_string(curvature) + "}"; + } + + Pose pose; + double curvature; + }; + + /** + * An intermediate value used in the "naive" generation step. Contains the + * final GeneratedPoint value that will be returned as well as the spline's + * derivative values to perform the initial check against the constraints. + */ + struct GeneratedVector { + GeneratedVector(GeneratedPoint ipoint, + double ivel, + double iaccel, + double ijerk) + : point(ipoint), vel(ivel), accel(iaccel), jerk(ijerk) {} + + GeneratedPoint point; + double vel; + double accel; + double jerk; + + std::string to_string() const { + return "GeneratedVector: {" + point.to_string() + + ", vel: " + std::to_string(vel) + + ", accel: " + std::to_string(accel) + + ", jerk: " + std::to_string(jerk) + "}"; + } + }; + + std::vector gen_single_raw_path(ControlVector start, + ControlVector end, + int duration, + double start_vel, + double end_vel); + /** + * Runs a Gradient Descent algorithm to minimize the linear acceleration, + * linear jerk, and curvature for the generated path. + * + * This is used when there is not a start/end velocity specified for a given + * path. + */ + std::vector + gradient_descent(ControlVector& start, ControlVector& end, bool fast); + + /** + * An intermediate value used in the parameterization step. Adds the + * constrained values from the motion profile to the output from the "naive" + * generation step. + */ + struct ConstrainedState { + ConstrainedState(Pose ipose, + double icurvature, + double idistance, + double imax_vel, + double imin_accel, + double imax_accel) + : pose(ipose), + curvature(icurvature), + distance(idistance), + max_vel(imax_vel), + min_accel(imin_accel), + max_accel(imax_accel) {} + + ConstrainedState() = default; + + Pose pose = Pose(); + double curvature = 0; + double distance = 0; + double max_vel = 0; + double min_accel = 0; + double max_accel = 0; + + std::string to_string() const { + return "ConstrainedState: {x: " + std::to_string(pose.x) + + ", y: " + std::to_string(pose.y) + + ", yaw: " + std::to_string(pose.yaw) + + ", k: " + std::to_string(curvature) + + ", dist: " + std::to_string(distance) + + ", v: " + std::to_string(max_vel) + + ", min_a: " + std::to_string(min_accel) + + ", max_a: " + std::to_string(max_accel) + "}"; + } + }; + + /** + * The actual function called by the "generate" functions. + * + * @param start An iterator pointing to the first ControlVector in the path + * @param end An iterator pointting to the last ControlVector in the path + * + * @return The points from each path concatenated together + */ + template + std::vector _generate(Iter start, Iter end, bool fast); + + public: + /** + * Performs the "naive" generation step. + * + * This step calculates the spline polynomials that fit within the + * SplineGenerator's acceleration and jerk constraints and returns the points + * that form the curve. + */ + std::vector + gen_raw_path(ControlVector& start, ControlVector& end, bool fast); + + /** + * Imposes a linear motion profile on the raw path. + * + * This step creates the velocity and acceleration values to command the robot + * at each point along the curve. + */ + std::vector + parameterize(const ControlVector start, + const ControlVector end, + const std::vector& raw_path, + const double preferred_start_vel, + const double preferred_end_vel, + const double start_time); + + /** + * Finds the new timestamps for each point along the curve based on the motion + * profile. + */ + std::vector + integrate_constrained_states(std::vector constrainedStates); + + /** + * Finds the ProfilePoint on the profiled curve for the given timestamp. + * + * This with interpolate between points on the curve if a point with an exact + * matching timestamp is not found. + */ + ProfilePoint get_point_at_time(const ControlVector start, + const ControlVector end, + std::vector points, + double t); + + /** + * Linearly interpolates between points along the profiled curve. + */ + ProfilePoint lerp_point(QuinticPolynomial x_qp, + QuinticPolynomial y_qp, + ProfilePoint start, + ProfilePoint end, + double i); + + /** + * Returns the spline curve for the given control vectors and path duration. + */ + QuinticPolynomial get_x_spline(const ControlVector start, + const ControlVector end, + const double duration); + QuinticPolynomial get_y_spline(const ControlVector start, + const ControlVector end, + const double duration); + + /** + * Applies the general constraints and model constraints to the given state. + */ + void enforce_accel_lims(ConstrainedState* state); + + /** + * Imposes the motion profile constraints on a segment of the path from the + * perspective of iterating forwards through the path. + */ + void forward_pass(ConstrainedState* predecessor, ConstrainedState* successor); + + /** + * Imposes the motion profile constraints on a segment of the path from the + * perspective of iterating backwards through the path. + */ + void backward_pass(ConstrainedState* predecessor, + ConstrainedState* successor); + + /** + * Calculates the final velocity for a path segment. + */ + double vf(double vi, double a, double ds); + + /** + * Calculates the initial acceleration needed to match the segments' + * velocities. + */ + double ai(double vf, double vi, double s); + + /** + * Values that are closer to each other than this value are considered equal. + */ + static constexpr double K_EPSILON = 1e-5; +}; +} // namespace squiggles + +#endif diff --git a/include/okapi/squiggles/squiggles.hpp b/include/okapi/squiggles/squiggles.hpp new file mode 100644 index 00000000..86c1bfa9 --- /dev/null +++ b/include/okapi/squiggles/squiggles.hpp @@ -0,0 +1,22 @@ +/** + * Copyright 2020 Jonathan Bayless + * + * Use of this source code is governed by an MIT-style license that can be found + * in the LICENSE file or at https://opensource.org/licenses/MIT. + */ +#ifndef _ROBOT_SQUIGGLES_H_ +#define _ROBOT_SQUIGGLES_H_ + +#include "geometry/controlvector.hpp" +#include "geometry/pose.hpp" +#include "geometry/profilepoint.hpp" + +#include "physicalmodel/passthroughmodel.hpp" +#include "physicalmodel/physicalmodel.hpp" +#include "physicalmodel/tankmodel.hpp" + +#include "constraints.hpp" +#include "io.hpp" +#include "spline.hpp" + +#endif \ No newline at end of file diff --git a/include/pros/adi.h b/include/pros/adi.h index e0d2ae7d..abe7544d 100644 --- a/include/pros/adi.h +++ b/include/pros/adi.h @@ -8,7 +8,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 @@ -69,6 +69,14 @@ typedef enum adi_port_config_e { E_ADI_ERR = PROS_ERR } adi_port_config_e_t; +/** + * Represents the potentiometer version type. + */ +typedef enum adi_potentiometer_type_e { + E_ADI_POT_EDR = 0, + E_ADI_POT_V2 +} adi_potentiometer_type_e_t; + #ifdef PROS_USE_SIMPLE_NAMES #ifdef __cplusplus #define ADI_ANALOG_IN pros::E_ADI_ANALOG_IN @@ -689,6 +697,66 @@ int32_t adi_gyro_reset(adi_gyro_t gyro); */ int32_t adi_gyro_shutdown(adi_gyro_t gyro); +/** + * Reference type for an initialized potentiometer. + * + * This merely contains the port number for the potentiometer, unlike its use as an + * object to store gyro data in PROS 2. + */ +typedef int32_t adi_potentiometer_t; + +/** + * Initializes a potentiometer on the given port of the original potentiometer. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of ADI Ports + * EADDRINUSE - The port is not configured as a potentiometer + * + * \param port + * The ADI port to initialize as a gyro (from 1-8, 'a'-'h', 'A'-'H') + * + * \return An adi_potentiometer_t object containing the given port, or PROS_ERR if the + * initialization failed. + */ +adi_potentiometer_t adi_potentiometer_init(uint8_t port); + +/** + * Initializes a potentiometer on the given port. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of ADI Ports + * EADDRINUSE - The port is not configured as a potentiometer + * + * \param port + * The ADI port to initialize as a gyro (from 1-8, 'a'-'h', 'A'-'H') + * \param potentiometer_type + * An adi_potentiometer_type_e_t enum value specifying the potentiometer version type + * + * \return An adi_potentiometer_t object containing the given port, or PROS_ERR if the + * initialization failed. + */ +adi_potentiometer_t adi_potentiometer_type_init(uint8_t port, adi_potentiometer_type_e_t potentiometer_type); + +/** + * Gets the current potentiometer angle in tenths of a degree. + * + * The original potentiometer rotates 250 degrees thus returning an angle between 0-250 degrees. + * Potentiometer V2 rotates 330 degrees thus returning an angle between 0-330 degrees. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of ADI Ports + * EADDRINUSE - The port is not configured as a potentiometer + * + * \param potentiometer + * The adi_potentiometer_t object for which the angle will be returned + * + * \return The potentiometer angle in degrees. + */ +double adi_potentiometer_get_angle(adi_potentiometer_t potentiometer); + #ifdef __cplusplus } // namespace c } // namespace pros diff --git a/include/pros/adi.hpp b/include/pros/adi.hpp index 60e350bf..d91ff7e7 100644 --- a/include/pros/adi.hpp +++ b/include/pros/adi.hpp @@ -8,7 +8,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 @@ -40,7 +40,7 @@ class ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param adi_port * The ADI port number (from 1-8, 'a'-'h', 'A'-'H') to configure @@ -55,7 +55,7 @@ class ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param port_pair * The pair of the smart port number (from 1-22) and the ADI port number @@ -109,7 +109,7 @@ class ADIPort { std::uint8_t _adi_port; }; -class ADIAnalogIn : private ADIPort { +class ADIAnalogIn : protected ADIPort { public: /** * Configures an ADI port to act as an Analog Input. @@ -117,7 +117,7 @@ class ADIAnalogIn : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param adi_port * The ADI port number (from 1-8, 'a'-'h', 'A'-'H') to configure @@ -133,7 +133,7 @@ class ADIAnalogIn : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param port_pair * The pair of the smart port number (from 1-22) and the @@ -224,7 +224,7 @@ class ADIAnalogIn : private ADIPort { using ADIPort::get_value; }; -using ADIPotentiometer = ADIAnalogIn; +// using ADIPotentiometer = ADIAnalogIn; using ADILineSensor = ADIAnalogIn; using ADILightSensor = ADIAnalogIn; using ADIAccelerometer = ADIAnalogIn; @@ -237,7 +237,7 @@ class ADIAnalogOut : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param adi_port * The ADI port number (from 1-8, 'a'-'h', 'A'-'H') to configure @@ -250,7 +250,7 @@ class ADIAnalogOut : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param port_pair * The pair of the smart port number (from 1-22) and the @@ -286,7 +286,7 @@ class ADIDigitalOut : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param adi_port * The ADI port number (from 1-8, 'a'-'h', 'A'-'H') to configure @@ -301,7 +301,7 @@ class ADIDigitalOut : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param port_pair * The pair of the smart port number (from 1-22) and the @@ -337,7 +337,7 @@ class ADIDigitalIn : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param adi_port * The ADI port number (from 1-8, 'a'-'h', 'A'-'H') to configure @@ -350,7 +350,7 @@ class ADIDigitalIn : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param port_pair * The pair of the smart port number (from 1-22) and the @@ -400,7 +400,7 @@ class ADIMotor : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param adi_port * The ADI port number (from 1-8, 'a'-'h', 'A'-'H') to configure @@ -413,7 +413,7 @@ class ADIMotor : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param port_pair * The pair of the smart port number (from 1-22) and the @@ -469,7 +469,7 @@ class ADIEncoder : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param adi_port_top * The "top" wire from the encoder sensor with the removable cover side up @@ -486,7 +486,7 @@ class ADIEncoder : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param port_tuple * The tuple of the smart port number, the "top" wire from the encoder @@ -535,7 +535,7 @@ class ADIUltrasonic : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param port_ping * The port connected to the orange OUTPUT cable. This should be in port @@ -552,12 +552,12 @@ class ADIUltrasonic : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param port_tuple * The tuple of the smart port number, the port connected to the orange - * OUTPUT cable (1, 3, 5, 7 or 'A', 'C', 'E', 'G'), and the port - * connected to the yellow INPUT cable (the next) highest port + * OUTPUT cable (1, 3, 5, 7 or 'A', 'C', 'E', 'G'), and the port + * connected to the yellow INPUT cable (the next) highest port * following port_ping). */ ADIUltrasonic(ext_adi_port_tuple_t port_tuple); @@ -595,7 +595,7 @@ class ADIGyro : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param adi_port * The ADI port to initialize as a gyro (from 1-8, 'a'-'h', 'A'-'H') @@ -619,7 +619,7 @@ class ADIGyro : private ADIPort { * This function uses the following values of errno when an error state is * reached: * ENXIO - Either the ADI port value or the smart port value is not within its - * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). * * \param port_pair * The pair of the smart port number (from 1-22) and the @@ -658,6 +658,108 @@ class ADIGyro : private ADIPort { */ std::int32_t reset() const; }; + +class ADIPotentiometer : public ADIAnalogIn { + public: + /** + * Configures an ADI port to act as a Potentiometer. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - Either the ADI port value or the smart port value is not within its + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * + * \param adi_port + * The ADI port number (from 1-8, 'a'-'h', 'A'-'H') to configure + * \param potentiometer_type + * An adi_potentiometer_type_e_t enum value specifying the potentiometer version type + */ + ADIPotentiometer(std::uint8_t adi_port, adi_potentiometer_type_e_t potentiometer_type = E_ADI_POT_EDR); + + /** + * Configures an ADI port on an adi_expander to act as a Potentiometer. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - Either the ADI port value or the smart port value is not within its + * valid range (ADI port: 1-8, 'a'-'h', or 'A'-'H'; smart port: 1-21). + * + * \param adi_port + * The pair of the smart port number (from 1-22) and the + * ADI port number (from 1-8, 'a'-'h', 'A'-'H') to configure + * \param potentiometer_type + * An adi_potentiometer_type_e_t enum value specifying the potentiometer version type + */ + ADIPotentiometer(ext_adi_port_pair_t port_pair, adi_potentiometer_type_e_t potentiometer_type = E_ADI_POT_EDR); + + /** + * Gets the current potentiometer angle in tenths of a degree. + * + * The original potentiometer rotates 250 degrees thus returning an angle between 0-250 degrees. + * Potentiometer V2 rotates 330 degrees thus returning an angle between 0-330 degrees. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of ADI Ports + * EADDRINUSE - The port is not configured as a potentiometer + + * \return The potentiometer angle in degrees. + */ + double get_angle() const; + + /** + * Gets the 12-bit value of the specified port. + * + * The value returned is undefined if the analog pin has been switched to a + * different mode. + * + * This function uses the following values of errno when an error state is + * reached: + * ENODEV - The port is not configured as a potentiometer + * + * \return The analog sensor value, where a value of 0 reflects an input + * voltage of nearly 0 V and a value of 4095 reflects an input voltage of + * nearly 5 V + */ + using ADIAnalogIn::get_value; + + /** + * Calibrates the potentiometer on the specified port and returns the new + * calibration value. + * + * This method assumes that the potentiometer value is not actively changing at + * this time and computes an average from approximately 500 samples, 1 ms + * apart, for a 0.5 s period of calibration. The average value thus calculated + * is returned and stored for later calls to the + * pros::ADIPotentiometer::get_value_calibrated() function. This function + * will return the difference between this value and the current sensor value + * when called. + * + * Do not use this function when the potentiometer value might be unstable (rotating the potentiometer) + * + * This function uses the following values of errno when an error state is + * reached: + * ENODEV - The port is not configured as a potentiometer + * + * \return The average potentiometer value computed by this function + */ + using ADIAnalogIn::calibrate; + + /** + * Gets the 12 bit calibrated value of a potentiometer port. + * + * The pros::ADIPotentiometer::calibrate() function must be run first. + * + * This function uses the following values of errno when an error state is + * reached: + * ENODEV - The port is not configured as a potentiometer + * + * \return The difference of the potentiometer value from its calibrated default from + * -4095 to 4095 + */ + using ADIAnalogIn::get_value_calibrated; +}; + } // namespace pros #endif // _PROS_ADI_HPP_ diff --git a/include/pros/api_legacy.h b/include/pros/api_legacy.h index 748072c7..068f7e8a 100644 --- a/include/pros/api_legacy.h +++ b/include/pros/api_legacy.h @@ -10,7 +10,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * All rights reserved. * * This Source Code Form is subject to the terms of the Mozilla Public diff --git a/include/pros/apix.h b/include/pros/apix.h index 14c8a313..8e7d3068 100644 --- a/include/pros/apix.h +++ b/include/pros/apix.h @@ -12,7 +12,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * All rights reserved. * * This Source Code Form is subject to the terms of the Mozilla Public @@ -377,7 +377,8 @@ typedef enum v5_device_e { E_DEVICE_ADI = 12, E_DEVICE_OPTICAL = 16, E_DEVICE_GPS = 20, - E_DEVICE_GENERIC = 129, + E_DEVICE_SERIAL = 129, + E_DEVICE_GENERIC __attribute__((deprecated("use E_DEVICE_SERIAL instead"))) = E_DEVICE_SERIAL, E_DEVICE_UNDEFINED = 255 } v5_device_e_t; diff --git a/include/pros/distance.h b/include/pros/distance.h index fae64a84..b7feda19 100644 --- a/include/pros/distance.h +++ b/include/pros/distance.h @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 diff --git a/include/pros/ext_adi.h b/include/pros/ext_adi.h index 11351fd0..d4282761 100644 --- a/include/pros/ext_adi.h +++ b/include/pros/ext_adi.h @@ -8,7 +8,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 @@ -630,6 +630,50 @@ int32_t ext_adi_gyro_reset(ext_adi_gyro_t gyro); */ int32_t ext_adi_gyro_shutdown(ext_adi_gyro_t gyro); +/** + * Reference type for an initialized potentiometer. + * + * This merely contains the port number for the potentiometer, unlike its use as an + * object to store gyro data in PROS 2. + */ +typedef int32_t ext_adi_potentiometer_t; + +/** + * Initializes a potentiometer on the given port. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of ADI Ports + * EADDRINUSE - The port is not configured as a potentiometer + * + * \param port + * The ADI port to initialize as a gyro (from 1-8, 'a'-'h', 'A'-'H') + * \param potentiometer_type + * An adi_potentiometer_type_e_t enum value specifying the potentiometer version type + * + * \return An adi_potentiometer_t object containing the given port, or PROS_ERR if the + * initialization failed. + */ +ext_adi_potentiometer_t ext_adi_potentiometer_init(uint8_t smart_port, uint8_t adi_port, adi_potentiometer_type_e_t potentiometer_type); + +/** + * Gets the current potentiometer angle in tenths of a degree. + * + * The original potentiometer rotates 250 degrees thus returning an angle between 0-250 degrees. + * Potentiometer V2 rotates 333 degrees thus returning an angle between 0-333 degrees. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of ADI Ports + * EADDRINUSE - The port is not configured as a potentiometer + * + * \param potentiometer + * The adi_potentiometer_t object for which the angle will be returned + * + * \return The potentiometer angle in degrees. + */ +double ext_adi_potentiometer_get_angle(ext_adi_potentiometer_t potentiometer); + #ifdef __cplusplus } // namespace c } // namespace pros diff --git a/include/pros/gps.h b/include/pros/gps.h index 0a83edea..1af417bc 100644 --- a/include/pros/gps.h +++ b/include/pros/gps.h @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 diff --git a/include/pros/gps.hpp b/include/pros/gps.hpp index 5c4656c4..0d8b731c 100644 --- a/include/pros/gps.hpp +++ b/include/pros/gps.hpp @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 diff --git a/include/pros/imu.h b/include/pros/imu.h index a772769e..17f716a9 100644 --- a/include/pros/imu.h +++ b/include/pros/imu.h @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 diff --git a/include/pros/imu.hpp b/include/pros/imu.hpp index 7bee256f..dcf21f99 100644 --- a/include/pros/imu.hpp +++ b/include/pros/imu.hpp @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 diff --git a/include/pros/link.h b/include/pros/link.h new file mode 100644 index 00000000..26067b87 --- /dev/null +++ b/include/pros/link.h @@ -0,0 +1,257 @@ +/** + * \file pros/link.h + * + * Contains prototypes for functions related to the robot to robot communications. + * + * Visit https://pros.cs.purdue.edu/v5/api/c/link.html to learn + * more. + * + * This file should not be modified by users, since it gets replaced whenever + * a kernel upgrade occurs. + * + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. + * + * 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/. + */ + +#ifndef _PROS_LINK_H_ +#define _PROS_LINK_H_ + +#include +#include + +#ifdef __cplusplus +extern "C" { +namespace pros { +#endif + +typedef enum link_type_e { + E_LINK_RECIEVER = 0, + E_LINK_TRANSMITTER +} link_type_e_t; + +#define LINK_BUFFER_SIZE 512 + +#ifdef __cplusplus +namespace c { +#endif + +/** + * Initializes a link on a radio port, with an indicated type. There might be a + * 1 to 2 second delay from when this function is called to when the link is initializes. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * + * \param port + * The port of the radio for the intended link. + * \param link_id + * Unique link ID in the form of a string, needs to be different from other links in + * the area. + * \param type + * Indicates whether the radio link on the brain is a transmitter or reciever, + * with the transmitter having double the transmitting bandwidth as the recieving + * end (1040 bytes/s vs 520 bytes/s). + * + * \return PROS_ERR if initialization fails, 1 if the initialization succeeds. + */ +uint32_t link_init(uint8_t port, const char* link_id, link_type_e_t type); + +/** + * Initializes a link on a radio port, with an indicated type and the ability for + * vexlink to override the controller radio. There might be a 1 to 2 second delay + * from when this function is called to when the link is initializes. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * + * \param port + * The port of the radio for the intended link. + * \param link_id + * Unique link ID in the form of a string, needs to be different from other links in + * the area. + * \param type + * Indicates whether the radio link on the brain is a transmitter or reciever, + * with the transmitter having double the transmitting bandwidth as the recieving + * end (1040 bytes/s vs 520 bytes/s). + * + * \return PROS_ERR if initialization fails, 1 if the initialization succeeds. + */ +uint32_t link_init_override(uint8_t port, const char* link_id, link_type_e_t type); + +/** + * Checks if a radio link on a port is active or not. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * + * \param port + * The port of the radio for the intended link. + * + * \return If a radio is connected to a port and it's connected to a link. + */ +bool link_connected(uint8_t port); + +/** + * Returns the bytes of data available to be read + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * + * \param port + * The port of the radio for the intended link. + * + * \return PROS_ERR if port is not a link/radio, else the bytes available to be + * read by the user. + */ +uint32_t link_raw_receivable_size(uint8_t port); + +/** + * Returns the bytes of data available in transmission buffer. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * + * \param port + * The port of the radio for the intended link. + * + * \return PROS_ERR if port is not a link/radio, + */ +uint32_t link_raw_transmittable_size(uint8_t port); + +/** + * Send raw serial data through vexlink. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * EBUSY - The transmitter buffer is still busy with a previous transmission, and there is no + * room in the FIFO buffer (queue) to transmit the data. + * EINVAL - The data given is NULL + * + * \param port + * The port of the radio for the intended link. + * \param data + * Buffer with data to send + * \param data_size + * Bytes of data to be read to the destination buffer + * + * \return PROS_ERR if port is not a link, and the successfully transmitted + * data size if it succeeded. + */ +uint32_t link_transmit_raw(uint8_t port, void* data, uint16_t data_size); + +/** + * Receive raw serial data through vexlink. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * EINVAL - The destination given is NULL, or the size given is larger than the FIFO buffer + * or destination buffer. + * + * \param port + * The port of the radio for the intended link. + * \param dest + * Destination buffer to read data to + * \param data_size + * Bytes of data to be read to the destination buffer + * + * \return PROS_ERR if port is not a link, and the successfully received + * data size if it succeeded. + */ +uint32_t link_receive_raw(uint8_t port, void* dest, uint16_t data_size); + +/** + * Send packeted message through vexlink, with a checksum and start byte. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * EBUSY - The transmitter buffer is still busy with a previous transmission, and there is no + * room in the FIFO buffer (queue) to transmit the data. + * EINVAL - The data given is NULL + * + * \param port + * The port of the radio for the intended link. + * \param data + * Buffer with data to send + * \param data_size + * Bytes of data to be read to the destination buffer + * + * \return PROS_ERR if port is not a link, and the successfully transmitted + * data size if it succeeded. + */ +uint32_t link_transmit(uint8_t port, void* data, uint16_t data_size); + +/** + * Receive packeted message through vexlink, with a checksum and start byte. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * EINVAL - The destination given is NULL, or the size given is larger than the FIFO buffer + * or destination buffer. + * EBADMSG - Protocol error related to start byte, data size, or checksum. + * + * \param port + * The port of the radio for the intended link. + * \param dest + * Destination buffer to read data to + * \param data_size + * Bytes of data to be read to the destination buffer + * + * \return PROS_ERR if port is not a link or protocol error, and the successfully + * transmitted data size if it succeeded. + */ +uint32_t link_receive(uint8_t port, void* dest, uint16_t data_size); + +/** + * Clear the receive buffer of the link, and discarding the data. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * + * \param port + * The port of the radio for the intended link. + * + * \return PROS_ERR if port is not a link, and the successfully received + * data size if it succeeded. + */ +uint32_t link_clear_receive_buf(uint8_t port); + +#ifdef __cplusplus +} +} +} +#endif + +#endif diff --git a/include/pros/link.hpp b/include/pros/link.hpp new file mode 100644 index 00000000..d44c6bd5 --- /dev/null +++ b/include/pros/link.hpp @@ -0,0 +1,200 @@ +/** + * \file pros/link.hpp + * + * Contains prototypes for functions related to robot to robot communications. + * + * Visit https://pros.cs.purdue.edu/v5/tutorials/topical/link.html to learn + * more. + * + * This file should not be modified by users, since it gets replaced whenever + * a kernel upgrade occurs. + * + * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * + * 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/. + */ +#ifndef _PROS_LINK_HPP_ +#define _PROS_LINK_HPP_ + +#include +#include + +#include "pros/link.h" + +namespace pros { +class Link { + private: + std::uint8_t _port; + + public: + /** + * Initializes a link on a radio port, with an indicated type. There might be a + * 1 to 2 second delay from when this function is called to when the link is initializes. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * + * \param port + * The port of the radio for the intended link. + * \param link_id + * Unique link ID in the form of a string, needs to be different from other links in + * the area. + * \param type + * Indicates whether the radio link on the brain is a transmitter or reciever, + * with the transmitter having double the transmitting bandwidth as the recieving + * end (1040 bytes/s vs 520 bytes/s). + * \param ov + * Indicates if the radio on the given port needs vexlink to override the controller radio + * + * \return PROS_ERR if initialization fails, 1 if the initialization succeeds. + */ + Link(const std::uint8_t port, const std::string link_id, link_type_e_t type, bool ov = false); + + /** + * Checks if a radio link on a port is active or not. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * + * \return If a radio is connected to a port and it's connected to a link. + */ + bool connected(); + + /** + * Returns the bytes of data number of without protocol available to be read + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * + * \return PROS_ERR if port is not a link/radio, else the bytes available to be + * read by the user. + */ + std::uint32_t raw_receivable_size(); + + /** + * Returns the bytes of data available in transmission buffer. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * + * \return PROS_ERR if port is not a link/radio, + */ + std::uint32_t raw_transmittable_size(); + + /** + * Send raw serial data through vexlink. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * EBUSY - The transmitter buffer is still busy with a previous transmission, and there is no + * room in the FIFO buffer (queue) to transmit the data. + * EINVAL - The data given is NULL + * + * \param data + * Buffer with data to send + * \param data_size + * Buffer with data to send + * + * \return PROS_ERR if port is not a link, and the successfully transmitted + * data size if it succeeded. + */ + std::uint32_t transmit_raw(void* data, std::uint16_t data_size); + + /** + * Receive raw serial data through vexlink. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * EINVAL - The destination given is NULL, or the size given is larger than the FIFO buffer + * or destination buffer. + * + * \param dest + * Destination buffer to read data to + * \param data_size + * Bytes of data to be read to the destination buffer + * + * \return PROS_ERR if port is not a link, and the successfully received + * data size if it succeeded. + */ + std::uint32_t receive_raw(void* dest, std::uint16_t data_size); + + /** + * Send packeted message through vexlink, with a checksum and start byte. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * EBUSY - The transmitter buffer is still busy with a previous transmission, and there is no + * room in the FIFO buffer (queue) to transmit the data. + * EINVAL - The data given is NULL + * + * \param data + * Buffer with data to send + * \param data_size + * Bytes of data to be read to the destination buffer + * + * \return PROS_ERR if port is not a link, and the successfully transmitted + * data size if it succeeded. + */ + std::uint32_t transmit(void* data, std::uint16_t data_size); + + /** + * Receive packeted message through vexlink, with a checksum and start byte. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + * EINVAL - The destination given is NULL, or the size given is larger than the FIFO buffer + * or destination buffer. + * EBADMSG - Protocol error related to start byte, data size, or checksum. + + * \param dest + * Destination buffer to read data to + * \param data_size + * Bytes of data to be read to the destination buffer + * + * \return PROS_ERR if port is not a link, and the successfully received + * data size if it succeeded. + */ + std::uint32_t receive(void* dest, std::uint16_t data_size); + + /** + * Clear the receive buffer of the link, and discarding the data. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a radio. + * ENXIO - The sensor is still calibrating, or no link is connected via the radio. + + * \return PROS_ERR if port is not a link, 1 if the operation succeeded. + */ + std::uint32_t clear_receive_buf(); +}; +} // namespace pros + +#endif diff --git a/include/pros/llemu.h b/include/pros/llemu.h index 7b4d64d3..4cb792b3 100644 --- a/include/pros/llemu.h +++ b/include/pros/llemu.h @@ -13,7 +13,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 @@ -225,6 +225,28 @@ bool lcd_register_btn2_cb(lcd_btn_cb_fn_t cb); */ uint8_t lcd_read_buttons(void); +/** + * Changes the color of the LCD background to a provided color expressed in + * type lv_color_t. + * + * \param color + * A color of type lv_color_t + * + * \return void + */ +void lcd_set_background_color(lv_color_t color); + +/** + * Changes the text color of the LCD to a provided color expressed in + * type lv_color_t. + * + * \param color + * A color of type lv_color_t + * + * \return void + */ +void lcd_set_text_color(lv_color_t color); + #ifdef __cplusplus } // namespace c } // namespace pros diff --git a/include/pros/llemu.hpp b/include/pros/llemu.hpp index aa96e9d6..8818eddb 100644 --- a/include/pros/llemu.hpp +++ b/include/pros/llemu.hpp @@ -13,7 +13,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 @@ -152,7 +152,7 @@ using lcd_btn_cb_fn_t = void (*)(void); * user-provided callback function will be invoked. * * \param cb - * A callback function of type lcd_btn_cb_fn_t(void (*cb)(void)) + * A callback function of type lcd_btn_cb_fn_t(void (*cb)(void)) */ void register_btn0_cb(lcd_btn_cb_fn_t cb); @@ -163,7 +163,7 @@ void register_btn0_cb(lcd_btn_cb_fn_t cb); * user-provided callback function will be invoked. * * \param cb - * A callback function of type lcd_btn_cb_fn_t(void (*cb)(void)) + * A callback function of type lcd_btn_cb_fn_t(void (*cb)(void)) */ void register_btn1_cb(lcd_btn_cb_fn_t cb); @@ -174,7 +174,7 @@ void register_btn1_cb(lcd_btn_cb_fn_t cb); * user-provided callback function will be invoked. * * \param cb - * A callback function of type lcd_btn_cb_fn_t(void (*cb)(void)) + * A callback function of type lcd_btn_cb_fn_t(void (*cb)(void)) */ void register_btn2_cb(lcd_btn_cb_fn_t cb); @@ -193,6 +193,69 @@ void register_btn2_cb(lcd_btn_cb_fn_t cb); * \return The buttons pressed as a bit mask */ std::uint8_t read_buttons(void); + +/** + * Changes the color of the LCD background to a provided color expressed in + * type lv_color_t. + * + * \param color + * A color of type lv_color_t + * + * \return void + */ +void set_background_color(lv_color_t color); + +/** + * Changes the color of the LCD background to a provided color expressed in RGB + * form, with three values of type uint8_t. + * + * \param r + * A value of type uint8_t, with a range of 0 to 255, representing the + * red value of a color + * + * \param g + * A value of type uint8_t, with a range of 0 to 255, representing the + * green value of a color + * + * \param b + * A value of type uint8_t, with a range of 0 to 255, representing the + * blue value of a color + * + * \return void + */ +void set_background_color(std::uint8_t r, std::uint8_t g, std::uint8_t b); + +/** + * Changes the text color of the LCD to a provided color expressed in + * type lv_color_t. + * + * \param color + * A color of type lv_color_t + * + * \return void + */ +void set_text_color(lv_color_t color); + +/** + * Changes the text color of the LCD to a provided color expressed in RGB + * form, with three values of type uint8_t. + * + * \param r + * A value of type uint8_t, with a range of 0 to 255, representing the + * red value of a color + * + * \param g + * A value of type uint8_t, with a range of 0 to 255, representing the + * green value of a color + * + * \param b + * A value of type uint8_t, with a range of 0 to 255, representing the + * blue value of a color + * + * \return void + */ +void set_text_color(std::uint8_t r, std::uint8_t g, std::uint8_t b); + } // namespace lcd } // namespace pros diff --git a/include/pros/misc.h b/include/pros/misc.h index b629ec04..c1a2cd44 100644 --- a/include/pros/misc.h +++ b/include/pros/misc.h @@ -10,7 +10,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * All rights reservered. * * This Source Code Form is subject to the terms of the Mozilla Public diff --git a/include/pros/misc.hpp b/include/pros/misc.hpp index efc3ed57..2415c2f1 100644 --- a/include/pros/misc.hpp +++ b/include/pros/misc.hpp @@ -10,7 +10,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * All rights reservered. * * This Source Code Form is subject to the terms of the Mozilla Public diff --git a/include/pros/motors.h b/include/pros/motors.h index 62a79662..d2246f5b 100644 --- a/include/pros/motors.h +++ b/include/pros/motors.h @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 @@ -56,6 +56,27 @@ namespace c { */ int32_t motor_move(uint8_t port, int32_t voltage); +/** + * Stops the motor using the currently configured brake mode. + * + * This function sets motor velocity to zero, which will cause it to act + * according to the set brake mode. If brake mode is set to MOTOR_BRAKE_HOLD, + * this function may behave differently than calling motor_move_absolute(port, 0) + * or motor_move_relative(port, 0). + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a motor + * + * \param port + * The V5 port number from 1-21 + * + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + */ +int32_t motor_brake(uint8_t port); + /** * Sets the target absolute position for the motor to move to. * diff --git a/include/pros/motors.hpp b/include/pros/motors.hpp index 6d743cfe..0da91883 100644 --- a/include/pros/motors.hpp +++ b/include/pros/motors.hpp @@ -185,6 +185,23 @@ class Motor { */ virtual std::int32_t move_voltage(const std::int32_t voltage) const; + /** + * Stops the motor using the currently configured brake mode. + * + * This function sets motor velocity to zero, which will cause it to act + * according to the set brake mode. If brake mode is set to MOTOR_BRAKE_HOLD, + * this function may behave differently than calling move_absolute(0) + * or move_relative(0). + * + * This function uses the following values of errno when an error state is + * reached: + * ENODEV - The port cannot be configured as a motor + * + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + */ + virtual std::int32_t brake(void) const; + /** * Changes the output velocity for a profiled movement (motor_move_absolute() * or motor_move_relative()). This will have no effect if the motor is not diff --git a/include/pros/optical.h b/include/pros/optical.h index b2867aa3..5fd12549 100644 --- a/include/pros/optical.h +++ b/include/pros/optical.h @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 diff --git a/include/pros/optical.hpp b/include/pros/optical.hpp index 381c52d2..783520d4 100644 --- a/include/pros/optical.hpp +++ b/include/pros/optical.hpp @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 diff --git a/include/pros/rotation.h b/include/pros/rotation.h index 021a8417..197936ac 100644 --- a/include/pros/rotation.h +++ b/include/pros/rotation.h @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 @@ -180,6 +180,24 @@ int32_t rotation_set_reversed(uint8_t port, bool value); */ int32_t rotation_reverse(uint8_t port); +/** + * Initialize the Rotation Sensor with a reverse flag + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Rotation Sensor + * + * \param port + * The V5 Rotation Sensor port number from 1-21 + * \param reverse_flag + * Determines if the Rotation Sensor is reversed or not. + * + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + */ +int32_t rotation_init_reverse(uint8_t port, bool reverse_flag); + /** * Get the Rotation Sensor's reversed flag * diff --git a/include/pros/rotation.hpp b/include/pros/rotation.hpp index fa9de552..c53ab7b1 100644 --- a/include/pros/rotation.hpp +++ b/include/pros/rotation.hpp @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 @@ -29,6 +29,8 @@ class Rotation { public: Rotation(const std::uint8_t port) : _port(port){}; + Rotation(const std::uint8_t port, const bool reverse_flag); + /** * Reset the Rotation Sensor * diff --git a/include/pros/rtos.h b/include/pros/rtos.h index 7b29526f..3afdb24a 100644 --- a/include/pros/rtos.h +++ b/include/pros/rtos.h @@ -10,7 +10,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * All rights reserved. * * This Source Code Form is subject to the terms of the Mozilla Public @@ -307,6 +307,21 @@ task_t task_get_current(); */ uint32_t task_notify(task_t task); +/** + * + * Utilizes task notifications to wait until specified task is complete and deleted, + * then continues to execute the program. Analogous to std::thread::join in C++. + * + * See https://pros.cs.purdue.edu/v5/tutorials/topical/notifications.html for + * details. + * + * \param task + * The task to wait on. + * + * \return void + */ +void task_join(task_t task); + /** * Sends a notification to a task, optionally performing some action. Will also * retrieve the value of the notification in the target task before modifying diff --git a/include/pros/rtos.hpp b/include/pros/rtos.hpp index 37000fa4..94ed5f42 100644 --- a/include/pros/rtos.hpp +++ b/include/pros/rtos.hpp @@ -10,7 +10,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * All rights reserved. * * This Source Code Form is subject to the terms of the Mozilla Public @@ -23,6 +23,7 @@ #include "pros/rtos.h" #undef delay +#include #include #include #include @@ -57,8 +58,8 @@ class Task { * debugging. The name may be up to 32 characters long. * */ - Task(task_fn_t function, void* parameters = NULL, std::uint32_t prio = TASK_PRIORITY_DEFAULT, - std::uint16_t stack_depth = TASK_STACK_DEPTH_DEFAULT, const char* name = ""); + explicit Task(task_fn_t function, void* parameters = nullptr, std::uint32_t prio = TASK_PRIORITY_DEFAULT, + std::uint16_t stack_depth = TASK_STACK_DEPTH_DEFAULT, const char* name = ""); /** * Creates a new task and add it to the list of tasks that are ready to run. @@ -103,14 +104,14 @@ class Task { */ template static task_t create(F&& function, std::uint32_t prio = TASK_PRIORITY_DEFAULT, - std::uint16_t stack_depth = TASK_STACK_DEPTH_DEFAULT, const char* name = "") { + std::uint16_t stack_depth = TASK_STACK_DEPTH_DEFAULT, const char* name = "") { static_assert(std::is_invocable_r_v); return pros::c::task_create( - [](void* parameters) { - std::unique_ptr> ptr{static_cast*>(parameters)}; - (*ptr)(); - }, - new std::function(std::forward(function)), prio, stack_depth, name); + [](void* parameters) { + std::unique_ptr> ptr{static_cast*>(parameters)}; + (*ptr)(); + }, + new std::function(std::forward(function)), prio, stack_depth, name); } /** @@ -131,7 +132,7 @@ class Task { static task_t create(F&& function, const char* name) { return Task::create(std::forward(function), TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, name); } - + /** * Creates a new task and add it to the list of tasks that are ready to run. * @@ -146,15 +147,21 @@ class Task { * TASK_PRIO_DEFAULT plus/minus 1 or 2 is typically used. * \param stack_depth * The number of words (i.e. 4 * stack_depth) available on the task's - * stack. TASK_STACK_DEPTH_DEFAULT is typically sufficienct. + * stack. TASK_STACK_DEPTH_DEFAULT is typically sufficient. * \param name * A descriptive name for the task. This is mainly used to facilitate * debugging. The name may be up to 32 characters long. * */ template - Task(F&& function, std::uint32_t prio = TASK_PRIORITY_DEFAULT, std::uint16_t stack_depth = TASK_STACK_DEPTH_DEFAULT, - const char* name = "") : Task(Task::create(std::forward(function), prio, stack_depth, name)) { + explicit Task(F&& function, std::uint32_t prio = TASK_PRIORITY_DEFAULT, std::uint16_t stack_depth = TASK_STACK_DEPTH_DEFAULT, + const char* name = "") + : Task( + [](void* parameters) { + std::unique_ptr> ptr{static_cast*>(parameters)}; + (*ptr)(); + }, + new std::function(std::forward(function)), prio, stack_depth, name) { static_assert(std::is_invocable_r_v); } @@ -183,7 +190,7 @@ class Task { * A task handle from task_create() for which to create a pros::Task * object. */ - Task(task_t task); + explicit Task(task_t task); /** * Get the currently running Task @@ -213,7 +220,7 @@ class Task { * * \return The priority of the task */ - std::uint32_t get_priority(void); + std::uint32_t get_priority(); /** * Sets the priority of the specified task. @@ -232,12 +239,12 @@ class Task { * * \return The state of the task */ - std::uint32_t get_state(void); + std::uint32_t get_state(); /** * Suspends the specified task, making it ineligible to be scheduled. */ - void suspend(void); + void suspend(); /** * Resumes the specified task, making it eligible to be scheduled. @@ -245,19 +252,19 @@ class Task { * \param task * The task to resume */ - void resume(void); + void resume(); /** * Gets the name of the specified task. * * \return A pointer to the name of the task */ - const char* get_name(void); + const char* get_name(); /** * Convert this object to a C task_t handle */ - operator task_t() { + explicit operator task_t() { return task; } @@ -270,7 +277,18 @@ class Task { * * \return Always returns true. */ - std::uint32_t notify(void); + std::uint32_t notify(); + + /** + * Utilizes task notifications to wait until specified task is complete and deleted, + * then continues to execute the program. Analogous to std::thread::join in C++. + * + * See https://pros.cs.purdue.edu/v5/tutorials/topical/notifications.html for + * details. + * + * \return void + */ + void join(); /** * Sends a notification to a task, optionally performing some action. Will @@ -322,7 +340,7 @@ class Task { * * \return False if there was not a notification waiting, true if there was */ - bool notify_clear(void); + bool notify_clear(); /** * Delays a task for a given number of milliseconds. @@ -359,15 +377,43 @@ class Task { * * \return The number of tasks that are currently being managed by the kernel. */ - static std::uint32_t get_count(void); + static std::uint32_t get_count(); private: - task_t task; + task_t task{}; +}; + +// STL Clock compliant clock +struct Clock { + using rep = std::uint32_t; + using period = std::milli; + using duration = std::chrono::duration; + using time_point = std::chrono::time_point; + const bool is_steady = true; + + /** + * Gets the current time. + * + * Effectively a wrapper around pros::millis() + * + * \return The current time + */ + static time_point now(); }; class Mutex { + std::shared_ptr> mutex; + public: - Mutex(void); + Mutex(); + + // disable copy and move construction and assignment per Mutex requirements + // (see https://en.cppreference.com/w/cpp/named_req/Mutex) + Mutex(const Mutex&) = delete; + Mutex(Mutex&&) = delete; + + Mutex& operator=(const Mutex&) = delete; + Mutex& operator=(Mutex&&) = delete; /** * Takes and locks a mutex indefinetly. @@ -380,7 +426,7 @@ class Mutex { * is returned, then errno is set with a hint about why the the mutex * couldn't be taken. */ - bool take(void); + bool take(); /** * Takes and locks a mutex, waiting for up to a certain number of milliseconds @@ -412,10 +458,80 @@ class Mutex { * false is returned, then errno is set with a hint about why the mutex * couldn't be returned. */ - bool give(void); + bool give(); - private: - std::shared_ptr> mutex; + /** + * Takes and locks a mutex, waiting for up to TIMEOUT_MAX milliseconds. + * + * Effectively equivalent to calling pros::Mutex::take with TIMEOUT_MAX as + * the parameter. + * + * Conforms to named requirment BasicLockable + * \see https://en.cppreference.com/w/cpp/named_req/BasicLockable + * + * \note Consider using a std::unique_lock, std::lock_guard, or + * std::scoped_lock instead of interacting with the Mutex directly. + * + * \exception std::system_error Mutex could not be locked within TIMEOUT_MAX + * milliseconds. see errno for details. + */ + void lock(); + + /** + * Unlocks a mutex. + * + * Equivalent to calling pros::Mutex::give. + * + * Conforms to named requirement BasicLockable + * \see https://en.cppreference.com/w/cpp/named_req/BasicLockable + * + * \note Consider using a std::unique_lock, std::lock_guard, or + * std::scoped_lock instead of interacting with the Mutex direcly. + */ + void unlock(); + + /** + * Try to lock a mutex. + * + * Returns immediately if unsucessful. + * + * Conforms to named requirement Lockable + * \see https://en.cppreference.com/w/cpp/named_req/Lockable + * + * \return True when lock was acquired succesfully, or false otherwise. + */ + bool try_lock(); + + /** + * Takes and locks a mutex, waiting for a specified duration. + * + * Equivalent to calling pros::Mutex::take with a duration specified in + * milliseconds. + * + * Conforms to named requirement TimedLockable + * \see https://en.cppreference.com/w/cpp/named_req/TimedLockable + * + * \param rel_time Time to wait before the mutex becomes available. + * \return True if the lock was acquired succesfully, otherwise false. + */ + template + bool try_lock_for(const std::chrono::duration& rel_time) { + return take(std::chrono::duration_cast(rel_time).count()); + } + + /** + * Takes and locks a mutex, waiting until a specified time. + * + * Conforms to named requirement TimedLockable + * \see https://en.cppreference.com/w/cpp/named_req/TimedLockable + * + * \param abs_time Time point until which to wait for the mutex. + * \return True if the lock was acquired succesfully, otherwise false. + */ + template + bool try_lock_until(const std::chrono::time_point& abs_time) { + return take(std::max(static_cast(0), (abs_time - Clock::now()).count())); + } }; /** @@ -427,7 +543,7 @@ using pros::c::millis; /** * Gets the number of microseconds since PROS initialized. - * + * * \return The number of microseconds since PROS initialized */ using pros::c::micros; @@ -440,9 +556,9 @@ using pros::c::micros; * To delay cyclically, use task_delay_until(). * * \param milliseconds - * The number of milliseconds to wait (1000 milliseconds per second) + * The number of milliseconds to wait (1000 milliseconds per second) */ using pros::c::delay; } // namespace pros -#endif // _PROS_RTOS_HPP_s +#endif // _PROS_RTOS_HPP_ diff --git a/include/pros/screen.h b/include/pros/screen.h index 9d0f2251..fc420b0f 100644 --- a/include/pros/screen.h +++ b/include/pros/screen.h @@ -46,7 +46,8 @@ typedef enum { typedef enum { E_TOUCH_RELEASED = 0, ///< Last interaction with screen was a quick press E_TOUCH_PRESSED, ///< Last interaction with screen was a release - E_TOUCH_HELD ///< User is holding screen down + E_TOUCH_HELD, ///< User is holding screen down + E_TOUCH_ERROR ///< An error occured while taking/returning the mutex } last_touch_e_t; /** @@ -96,46 +97,86 @@ namespace c { /** * Set the pen color for subsequent graphics operations + * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. * * \param color The pen color to set (it is recommended to use values * from the enum defined in colors.h) + * + * \return Returns 1 if the mutex was successfully returned, or PROS_ERR if + * there was an error either taking or returning the screen mutex. */ -void screen_set_pen(uint32_t color); +uint32_t screen_set_pen(uint32_t color); /** * Set the eraser color for erasing and the current background. * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param color The background color to set (it is recommended to use values * from the enum defined in colors.h) + * + * \return Returns 1 if the mutex was successfully returned, or + * prosERR if there was an error either taking or returning the screen mutex. */ -void screen_set_eraser(uint32_t color); +uint32_t screen_set_eraser(uint32_t color); /** * Get the current pen color. + * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. * - * \return The current pen color in the form of a value from the enum defined in colors.h. + * \return The current pen color in the form of a value from the enum defined + * in colors.h, or PROS_ERR if there was an error taking or returning + * the screen mutex. */ uint32_t screen_get_pen(void); /** * Get the current eraser color. * - * \return The current eraser color in the form of a value from the enum defined in colors.h. + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * + * \return The current eraser color in the form of a value from the enum + * defined in colors.h, or PROS_ERR if there was an error taking or + * returning the screen mutex. */ uint32_t screen_get_eraser(void); /** * Clear display with eraser color + * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_erase(void); +uint32_t screen_erase(void); /** * Scroll lines on the display upwards. * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param start_line The line from which scrolling will start * \param lines The number of lines to scroll up + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_scroll(int16_t start_line, int16_t lines); +uint32_t screen_scroll(int16_t start_line, int16_t lines); /** * Scroll lines within a region on the display @@ -144,18 +185,29 @@ void screen_scroll(int16_t start_line, int16_t lines); * specify a rectangular region within which to scroll lines instead of a start * line. * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x,y) coordinates of the first corner of the * rectangular region * \param x1, y1 The (x,y) coordinates of the second corner of the * rectangular region * \param lines The number of lines to scroll upwards + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_scroll_area(int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t lines); +uint32_t screen_scroll_area(int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t lines); /** * Copy a screen region (designated by a rectangle) from an off-screen buffer * to the screen * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x,y) coordinates of the first corner of the * rectangular region of the screen * \param x1, y1 The (x,y) coordinates of the second corner of the @@ -163,88 +215,161 @@ void screen_scroll_area(int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t * \param buf Off-screen buffer containing screen data * \param stride Off-screen buffer width in pixels, such that image size * is stride-padding + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_copy_area(int16_t x0, int16_t y0, int16_t x1, int16_t y1, uint32_t* buf, int32_t stride); +uint32_t screen_copy_area(int16_t x0, int16_t y0, int16_t x1, int16_t y1, uint32_t* buf, int32_t stride); /** * Draw a single pixel on the screen using the current pen color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x, y The (x,y) coordinates of the pixel + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_draw_pixel(int16_t x, int16_t y); +uint32_t screen_draw_pixel(int16_t x, int16_t y); /** * Erase a pixel from the screen (Sets the location) * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x, y The (x,y) coordinates of the erased + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_erase_pixel(int16_t x, int16_t y); +uint32_t screen_erase_pixel(int16_t x, int16_t y); /** * Draw a line on the screen using the current pen color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x, y) coordinates of the first point of the line * \param x1, y1 The (x, y) coordinates of the second point of the line + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_draw_line(int16_t x0, int16_t y0, int16_t x1, int16_t y1); +uint32_t screen_draw_line(int16_t x0, int16_t y0, int16_t x1, int16_t y1); /** * Erase a line on the screen using the current eraser color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x, y) coordinates of the first point of the line * \param x1, y1 The (x, y) coordinates of the second point of the line + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_erase_line(int16_t x0, int16_t y0, int16_t x1, int16_t y1); +uint32_t screen_erase_line(int16_t x0, int16_t y0, int16_t x1, int16_t y1); /** * Draw a rectangle on the screen using the current pen color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x,y) coordinates of the first point of the rectangle * \param x1, y1 The (x,y) coordinates of the second point of the rectangle + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_draw_rect(int16_t x0, int16_t y0, int16_t x1, int16_t y1); +uint32_t screen_draw_rect(int16_t x0, int16_t y0, int16_t x1, int16_t y1); /** * Erase a rectangle on the screen using the current eraser color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x,y) coordinates of the first point of the rectangle * \param x1, y1 The (x,y) coordinates of the second point of the rectangle + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_erase_rect(int16_t x0, int16_t y0, int16_t x1, int16_t y1); +uint32_t screen_erase_rect(int16_t x0, int16_t y0, int16_t x1, int16_t y1); /** * Fill a rectangular region of the screen using the current pen * color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x,y) coordinates of the first point of the rectangle * \param x1, y1 The (x,y) coordinates of the second point of the rectangle + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_fill_rect(int16_t x0, int16_t y0, int16_t x1, int16_t y1); +uint32_t screen_fill_rect(int16_t x0, int16_t y0, int16_t x1, int16_t y1); /** * Draw a circle on the screen using the current pen color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x, y The (x,y) coordinates of the center of the circle * \param r The radius of the circle + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_draw_circle(int16_t x, int16_t y, int16_t radius); +uint32_t screen_draw_circle(int16_t x, int16_t y, int16_t radius); /** * Erase a circle on the screen using the current eraser color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x, y The (x,y) coordinates of the center of the circle * \param r The radius of the circle + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_erase_circle(int16_t x, int16_t y, int16_t radius); +uint32_t screen_erase_circle(int16_t x, int16_t y, int16_t radius); /** * Fill a circular region of the screen using the current pen * color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x, y The (x,y) coordinates of the center of the circle * \param r The radius of the circle + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_fill_circle(int16_t x, int16_t y, int16_t radius); +uint32_t screen_fill_circle(int16_t x, int16_t y, int16_t radius); /******************************************************************************/ /** Screen Text Display Functions **/ @@ -261,8 +386,11 @@ void screen_fill_circle(int16_t x, int16_t y, int16_t radius); * \param line The line number on which to print * \param text Format string * \param ... Optional list of arguments for the format string + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_print(text_format_e_t txt_fmt, const int16_t line, const char* text, ...); +uint32_t screen_print(text_format_e_t txt_fmt, const int16_t line, const char* text, ...); /** * Print a formatted string to the screen at the specified point @@ -276,8 +404,11 @@ void screen_print(text_format_e_t txt_fmt, const int16_t line, const char* text, * \param y The x coordinate of the top left corner of the string * \param text Format string * \param ... Optional list of arguments for the format string + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ -void screen_print_at(text_format_e_t txt_fmt, const int16_t x, const int16_t y, const char* text, ...); +uint32_t screen_print_at(text_format_e_t txt_fmt, const int16_t x, const int16_t y, const char* text, ...); /** * Print a formatted string to the screen on the specified line @@ -287,13 +418,20 @@ void screen_print_at(text_format_e_t txt_fmt, const int16_t x, const int16_t y, * * Will default to a medium sized font by default if invalid txt_fmt is given. * Exposed mostly for writing libraries and custom functions. + * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. * * \param txt_fmt Text format enum that determines if the text is medium, large, medium_center, or large_center. (DOES NOT SUPPORT SMALL) * \param line The line number on which to print * \param text Format string * \param args List of arguments for the format string + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * while taking or returning the screen mutex. */ -void screen_vprintf(text_format_e_t txt_fmt, const int16_t line, const char* text, va_list args); +uint32_t screen_vprintf(text_format_e_t txt_fmt, const int16_t line, const char* text, va_list args); /** * Print a formatted string to the screen at the specified coordinates @@ -305,13 +443,20 @@ void screen_vprintf(text_format_e_t txt_fmt, const int16_t line, const char* tex * * Text formats medium_center and large_center will default to medium and large respectively. * Exposed mostly for writing libraries and custom functions. + * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. * * \param txt_fmt Text format enum that determines if the text is small, medium, or large. * \param x, y The (x,y) coordinates of the top left corner of the string * \param text Format string * \param args List of arguments for the format string + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * while taking or returning the screen mutex. */ -void screen_vprintf_at(text_format_e_t txt_fmt, const int16_t x, const int16_t y, const char* text, va_list args); +uint32_t screen_vprintf_at(text_format_e_t txt_fmt, const int16_t x, const int16_t y, const char* text, va_list args); /******************************************************************************/ /** Screen Touch Functions **/ @@ -325,16 +470,25 @@ void screen_vprintf_at(text_format_e_t txt_fmt, const int16_t x, const int16_t y * * \return The last_touch_e_t enum specifier that indicates the last touch status of the screen (E_TOUCH_EVENT_RELEASE, E_TOUCH_EVENT_PRESS, or E_TOUCH_EVENT_PRESS_AND_HOLD). * This will be released by default if no action was taken. + * If an error occured, the screen_touch_status_s_t will have its last_touch_e_t + * enum specifier set to E_TOUCH_ERR, and other values set to -1. */ screen_touch_status_s_t screen_touch_status(void); /** * Assigns a callback function to be called when a certain touch event happens. + * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. * * \param cb Function pointer to callback when event type happens * \param event_type Touch event that will trigger the callback. + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * while taking or returning the screen mutex. */ -void screen_touch_callback(touch_event_cb_fn_t cb, last_touch_e_t event_type); +uint32_t screen_touch_callback(touch_event_cb_fn_t cb, last_touch_e_t event_type); #ifdef __cplusplus } //namespace c diff --git a/include/pros/screen.hpp b/include/pros/screen.hpp index 75e3a62f..ccf69c0f 100644 --- a/include/pros/screen.hpp +++ b/include/pros/screen.hpp @@ -45,46 +45,86 @@ const char* convert_args(const std::string& arg) { /** * Set the pen color for subsequent graphics operations + * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. * * \param color The pen color to set (it is recommended to use values * from the enum defined in colors.h) + * + * \return Returns 1 if the mutex was successfully returned, or prosERR if + * there was an error either taking or returning the screen mutex. */ - void set_pen(const std::uint32_t color); + std::uint32_t set_pen(const std::uint32_t color); /** - * Set the eraser color for clearing and the current background. + * Set the eraser color for erasing and the current background. * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param color The background color to set (it is recommended to use values * from the enum defined in colors.h) + * + * \return Returns 1 if the mutex was successfully returned, or prosERR + * if there was an error either taking or returning the screen mutex. */ - void set_eraser(const std::uint32_t color); + std::uint32_t set_eraser(const std::uint32_t color); /** * Get the current pen color. + * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. * - * \return The current pen color of the screen object in the form of a value from the enum defined in colors.h. + * \return The current pen color in the form of a value from the enum + * defined in colors.h, or PROS_ERR if there was an error taking or + * returning the screen mutex. */ std::uint32_t get_pen(); /** * Get the current eraser color. * - * \return The current eraser color of the screen object in the form of a value from the enum defined in colors.h. + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * + * \return The current eraser color in the form of a value from the enum + * defined in colors.h, or PROS_ERR if there was an error taking or + * returning the screen mutex. */ std::uint32_t get_eraser(); /** * Clear display with eraser color + * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void erase(); + std::uint32_t erase(); /** * Scroll lines on the display upwards. * - * \param start_line The line from which scrolling will start + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * + * \param start_line The line from which scrolling will start * \param lines The number of lines to scroll up + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void scroll(const std::int16_t start_line, const std::int16_t lines); + std::uint32_t scroll(const std::int16_t start_line, const std::int16_t lines); /** * Scroll lines within a region on the display @@ -93,18 +133,29 @@ const char* convert_args(const std::string& arg) { * specify a rectangular region within which to scroll lines instead of a start * line. * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x,y) coordinates of the first corner of the * rectangular region * \param x1, y1 The (x,y) coordinates of the second corner of the * rectangular region * \param lines The number of lines to scroll upwards + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void scroll_area(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1, std::int16_t lines); + std::uint32_t scroll_area(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1, std::int16_t lines); /** * Copy a screen region (designated by a rectangle) from an off-screen buffer * to the screen * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x,y) coordinates of the first corner of the * rectangular region of the screen * \param x1, y1 The (x,y) coordinates of the second corner of the @@ -112,88 +163,161 @@ const char* convert_args(const std::string& arg) { * \param buf Off-screen buffer containing screen data * \param stride Off-screen buffer width in pixels, such that image size * is stride-padding + * + * \return 1 if there were no errors, or PROS_ERR if an error occured taking + * or returning the screen mutex. */ - void copy_area(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1, uint32_t* buf, const std::int32_t stride); + std::uint32_t copy_area(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1, uint32_t* buf, const std::int32_t stride); /** * Draw a single pixel on the screen using the current pen color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x, y The (x,y) coordinates of the pixel + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void draw_pixel(const std::int16_t x, const std::int16_t y); + std::uint32_t draw_pixel(const std::int16_t x, const std::int16_t y); /** * Erase a pixel from the screen (Sets the location) * - * \param x, y The (x,y) coordinates of the erased pixel + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * + * \param x, y The (x,y) coordinates of the erased + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void erase_pixel(const std::int16_t x, const std::int16_t y); + std::uint32_t erase_pixel(const std::int16_t x, const std::int16_t y); /** * Draw a line on the screen using the current pen color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x, y) coordinates of the first point of the line * \param x1, y1 The (x, y) coordinates of the second point of the line + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void draw_line(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1); + std::uint32_t draw_line(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1); /** * Erase a line on the screen using the current eraser color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x, y) coordinates of the first point of the line * \param x1, y1 The (x, y) coordinates of the second point of the line + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void erase_line(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1); + std::uint32_t erase_line(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1); /** * Draw a rectangle on the screen using the current pen color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x,y) coordinates of the first point of the rectangle * \param x1, y1 The (x,y) coordinates of the second point of the rectangle + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void draw_rect(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1); + std::uint32_t draw_rect(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1); /** * Erase a rectangle on the screen using the current eraser color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x,y) coordinates of the first point of the rectangle * \param x1, y1 The (x,y) coordinates of the second point of the rectangle + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void erase_rect(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1); + std::uint32_t erase_rect(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1); /** * Fill a rectangular region of the screen using the current pen * color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x0, y0 The (x,y) coordinates of the first point of the rectangle * \param x1, y1 The (x,y) coordinates of the second point of the rectangle + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void fill_rect(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1); + std::uint32_t fill_rect(const std::int16_t x0, const std::int16_t y0, const std::int16_t x1, const std::int16_t y1); /** * Draw a circle on the screen using the current pen color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x, y The (x,y) coordinates of the center of the circle * \param r The radius of the circle + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void draw_circle(const std::int16_t x, const std::int16_t y, const std::int16_t radius); + std::uint32_t draw_circle(const std::int16_t x, const std::int16_t y, const std::int16_t radius); /** * Erase a circle on the screen using the current eraser color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x, y The (x,y) coordinates of the center of the circle * \param r The radius of the circle + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void erase_circle(const std::int16_t x, const std::int16_t y, const std::int16_t radius); + std::uint32_t erase_circle(const std::int16_t x, const std::int16_t y, const std::int16_t radius); /** * Fill a circular region of the screen using the current pen * color * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. + * * \param x, y The (x,y) coordinates of the center of the circle * \param r The radius of the circle + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * taking or returning the screen mutex. */ - void fill_circle(const std::int16_t x, const std::int16_t y, const std::int16_t radius); + std::uint32_t fill_circle(const std::int16_t x, const std::int16_t y, const std::int16_t radius); /******************************************************************************/ /** Screen Text Display Functions **/ @@ -230,20 +354,30 @@ const char* convert_args(const std::string& arg) { /** information about screen touches **/ /******************************************************************************/ - /** - * Gets the touch status of the last touch of the screen. 0 by default. + /** + * Gets the touch status of the last touch of the screen. * - * \return The last_touch_e_t enum specifier that indicates the last touch status of the screen (E_TOUCH_EVENT_RELEASE, E_TOUCH_EVENT_PRESS, or E_TOUCH_EVENT_PRESS_AND_HOLD). + * \return The last_touch_e_t enum specifier that indicates the last touch status of the screen (E_TOUCH_EVENT_RELEASE, E_TOUCH_EVENT_PRESS, or E_TOUCH_EVENT_PRESS_AND_HOLD). + * This will be released by default if no action was taken. + * If an error occured, the screen_touch_status_s_t will have its + * last_touch_e_t enum specifier set to E_TOUCH_ERR, and other values set to -1. */ screen_touch_status_s_t touch_status(); /** * Assigns a callback function to be called when a certain touch event happens. + * + * This function uses the following values of errno when an error state is + * reached: + * EACCESS - Another resource is currently trying to access the screen mutex. * * \param cb Function pointer to callback when event type happens * \param event_type Touch event that will trigger the callback. + * + * \return 1 if there were no errors, or PROS_ERR if an error occured + * while taking or returning the screen mutex. */ - void touch_callback(touch_event_cb_fn_t cb, last_touch_e_t event_type); + std::uint32_t touch_callback(touch_event_cb_fn_t cb, last_touch_e_t event_type); } //namespace screen } //namespace pros diff --git a/include/pros/serial.h b/include/pros/serial.h index 62560454..6db69cd0 100644 --- a/include/pros/serial.h +++ b/include/pros/serial.h @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * * 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 diff --git a/include/pros/vision.h b/include/pros/vision.h index f565255f..8464b6c3 100644 --- a/include/pros/vision.h +++ b/include/pros/vision.h @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * All rights reserved. * * This Source Code Form is subject to the terms of the Mozilla Public diff --git a/include/pros/vision.hpp b/include/pros/vision.hpp index 05a4bd2e..eeb00462 100644 --- a/include/pros/vision.hpp +++ b/include/pros/vision.hpp @@ -9,7 +9,7 @@ * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. * - * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * Copyright (c) 2017-2022, Purdue University ACM SIGBots. * All rights reserved. * * This Source Code Form is subject to the terms of the Mozilla Public diff --git a/include/stateMachine/behavior.hpp b/include/stateMachine/behavior.hpp index 4b5d2567..669e7f73 100644 --- a/include/stateMachine/behavior.hpp +++ b/include/stateMachine/behavior.hpp @@ -3,19 +3,47 @@ #include namespace Pronounce { + + /** + * @brief Parent class that provides a template class for all behaviors. Inherit this class to create a new behavior + * + * @author @ad101-lab - Alex Dickhans + */ class Behavior { private: public: - Behavior(); + /** + * @brief Basic initializer + * + */ + Behavior() {} + /** + * @brief Abstract function to initialize the state. Called before the state starts running + * + */ virtual void initialize() {} + /** + * @brief Abstract function to update the state. Called every frame when the state is running + * + */ virtual void update() {} + /** + * @brief Abstract isDone behavior. Returns true when the state is done running + * + * @return true Return true when the state is done being run + * @return false Return false when the state isn't done + */ virtual bool isDone() { return false; } + /** + * @brief Abstract function to exit the state. Called after the state stops running. !NOT GUARANTEED TO RUN! + * + */ virtual void exit() {} - ~Behavior(); + ~Behavior() {} }; } // namespace Pronounce diff --git a/include/stateMachine/behaviorGroup.hpp b/include/stateMachine/behaviorGroup.hpp new file mode 100644 index 00000000..17523ddf --- /dev/null +++ b/include/stateMachine/behaviorGroup.hpp @@ -0,0 +1,70 @@ +#pragma once + +#ifndef DEBUG +#include "behavior.hpp" +#endif // !DEBUG +#include + +namespace Pronounce { + /** + * @brief A behavior group groups a lot of behaviors together + * + * @author Alex Dickhans + * + */ + class BehaviorGroup : public Behavior { + private: + /** + * @brief A list of all the behaviors in this state + * + */ + std::vector behaviors; + public: + /** + * @brief Construct a new Behavior Group object + * + */ + BehaviorGroup() {} + + /** + * @brief Initialize all the objects + * + */ + void initialize() { + for (int i = 0; i < behaviors.size(); i++) { + behaviors.at(i)->initialize(); + } + } + + /** + * @brief Update all the objects + * + */ + void update() { + for (int i = 0; i < behaviors.size(); i++) { + behaviors.at(i)->update(); + } + } + + /** + * @brief Exit all the objects + * + */ + void exit() { + for (int i = 0; i < behaviors.size(); i++) { + behaviors.at(i)->exit(); + } + } + + /** + * @brief Add a behavior to the list + * + * @param behavior New behavior to add + */ + void addBehavior(Behavior* behavior) { + behaviors.emplace_back(behavior); + } + + ~BehaviorGroup() {} + }; +} // namespace Pronounce diff --git a/include/stateMachine/parallel.hpp b/include/stateMachine/parallel.hpp index 908ae84b..47a1e573 100644 --- a/include/stateMachine/parallel.hpp +++ b/include/stateMachine/parallel.hpp @@ -5,44 +5,99 @@ #include namespace Pronounce { + /** + * @brief A class to run several classes in parallel + * + */ class Parallel : Behavior { private: std::unordered_map behaviors; public: + /** + * @brief Construct a new Parallel object + * + */ Parallel(); + /** + * @brief Start all the states + * + */ void initialize() { for (auto& behavior : behaviors) { behavior.first->setCurrentBehavior(behavior.second); } } + /** + * @brief Update, doesn't do anything yet + * + */ void update() { // Don't do anything } + /** + * @brief Exit all the behaviors + * + */ void exit() { for (auto& behavior : behaviors) { behavior.first->useDefaultBehavior(); } } + /** + * @brief Add a state to the parallel + * + * @param stateController + * @param behavior + */ void addBehavior(StateController* stateController, Behavior* behavior) { behaviors[stateController] = behavior; } + /** + * @brief Remove a behavior by stateController + * + * @param stateController + */ void removeBehavior(StateController* stateController) { behaviors.erase(stateController); } + /** + * @brief Get the Behaviors object + * + * @return std::unordered_map + */ std::unordered_map getBehaviors() { return behaviors; } + /** + * @brief Reset the list + * + */ void reset() { behaviors.clear(); } + /** + * @brief Return true if all the states are complete + * + * @return true Everything is complete + * @return false Not everything is complete + */ + bool isDone() { + // If any behavior is not done return not done + bool done = true; + for (auto& behavior : behaviors) { + done = done && behavior.first->isDone(); + } + return done; + } + ~Parallel(); }; } // namespace Pronounce diff --git a/include/stateMachine/sequence.hpp b/include/stateMachine/sequence.hpp index cfecdc89..aa952838 100644 --- a/include/stateMachine/sequence.hpp +++ b/include/stateMachine/sequence.hpp @@ -1,45 +1,55 @@ #pragma once +#ifndef DEBUG #include "behavior.hpp" #include "stateController.hpp" -#include +#endif // !DEBUG +#include +#include namespace Pronounce { class Sequence : public Behavior { private: std::vector stateControllers; std::vector behaviors; + int currentIndex = 0; public: - Sequence(); + Sequence() {} void initialize() { + currentIndex = 0; if (behaviors.size() > 0) { - stateControllers.at(0)->initialize(); + stateControllers.at(currentIndex)->setCurrentBehavior(behaviors.at(currentIndex)); } } void update() { - if (stateControllers.at(currentIndex)->isDone()) { + + if (behaviors.at(currentIndex)->isDone()) { if (currentIndex < stateControllers.size() - 1) { stateControllers.at(currentIndex)->useDefaultBehavior(); currentIndex++; stateControllers.at(currentIndex)->setCurrentBehavior(behaviors.at(currentIndex)); } else { + std::cout << "Ending" << std::endl; stateControllers.at(currentIndex)->useDefaultBehavior(); + currentIndex++; // Done } } } void exit() { - stateControllers.at(currentIndex)->useDefaultBehavior(); - currentIndex = stateControllers.size()-1; + if (!this->isDone()) { + stateControllers.at(currentIndex)->useDefaultBehavior(); + currentIndex = stateControllers.size() - 1; + } } bool isDone() { - return currentIndex >= stateControllers.size()-1; + return !(currentIndex < stateControllers.size()); } void addState(StateController* stateController, Behavior* behavior) { @@ -47,6 +57,6 @@ namespace Pronounce { behaviors.emplace_back(behavior); } - ~Sequence(); + ~Sequence() {} }; } // namespace Pronounce diff --git a/include/stateMachine/state/robotStatus.hpp b/include/stateMachine/state/robotStatus.hpp index e69de29b..e5e1059b 100644 --- a/include/stateMachine/state/robotStatus.hpp +++ b/include/stateMachine/state/robotStatus.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include "behavior.hpp" + +namespace Pronounce { + class RobotStatus : public Behavior { + private: + /* data */ + public: + RobotStatus(/* args */); + + void initialize() { + return; + } + + void update() { + + } + + void exit() { + + } + + ~RobotStatus(); + }; +} // namespace Pronounce diff --git a/include/stateMachine/stateController.hpp b/include/stateMachine/stateController.hpp index 2e3dd87f..e5aaff9b 100644 --- a/include/stateMachine/stateController.hpp +++ b/include/stateMachine/stateController.hpp @@ -5,45 +5,86 @@ #include namespace Pronounce { - class StateController : Behavior { + /** + * @brief State controller controls a subsystem so that only one state will ever be running at a time. Returns to default once a state finishes + * + * @author Alex Dickhans + */ + class StateController : public Behavior { private: + /** + * @brief Default behavior that the stateController returns to when the current state isn't running + * + */ Behavior* defaultBehavior; - Behavior* currentBehavior; + + /** + * @brief The current behavior that runs if it exists. + * + */ + Behavior* currentBehavior = nullptr; public: - StateController(); + /** + * @brief Construct a new State Controller object + * + * @param defaultBehavior + */ + StateController(Behavior* defaultBehavior) { + this->defaultBehavior = defaultBehavior; + } + /** + * @brief Initialize the behavior. If currentbehavior exists that one gets initialized + * + */ void initialize() { if (currentBehavior != nullptr) { currentBehavior->initialize(); } else { - if (defaultBehavior != nullptr) { - defaultBehavior->initialize(); - } + defaultBehavior->initialize(); } } + /** + * @brief Update the functions and check if you need to Transition + * + */ void update() { + // If currentBehavior exists run it or transition if (currentBehavior != nullptr) { if (currentBehavior->isDone()) { currentBehavior->exit(); currentBehavior = nullptr; defaultBehavior->initialize(); + defaultBehavior->update(); } else { currentBehavior->update(); } } else { + // Run the default behavior when that one doesn't exist defaultBehavior->update(); } } + /** + * @brief Return that the object is done when the current behavior isn't running + * + * @return true Current behavior is not running + * @return false Default behavior is not running + */ bool isDone() { return currentBehavior == nullptr; } + /** + * @brief Exit the behavior depending on which one is running + * + */ void exit() { + // Exit the right behavior depending on which one is running if (currentBehavior != nullptr) { currentBehavior->exit(); currentBehavior = nullptr; @@ -53,30 +94,55 @@ namespace Pronounce { } } + /** + * @brief Set the Default Behavior object + * + * @param behavior + */ void setDefaultBehavior(Behavior* behavior) { defaultBehavior = behavior; } + /** + * @brief Set the Current Behavior object depending on what is running and what it equals + * + * @param behavior + */ void setCurrentBehavior(Behavior* behavior) { + // If the defaultBehavior and current behavior are equal then switch to default behavior if (defaultBehavior == behavior) { this->useDefaultBehavior(); - } else if (currentBehavior != nullptr) { + } + // If the currentBehavior exists and transition from that if it does + else if (currentBehavior != nullptr) { currentBehavior->exit(); currentBehavior = behavior; currentBehavior->initialize(); - } else { + currentBehavior->update(); + } + // Transition from default behavior if it doesn't + else { defaultBehavior->exit(); currentBehavior = behavior; currentBehavior->initialize(); + currentBehavior->update(); } } - + + /** + * @brief Transition to the default behavior + * + */ void useDefaultBehavior() { - currentBehavior->exit(); - currentBehavior = nullptr; - defaultBehavior->initialize(); + // If we are currently running the current behavior transition + if (currentBehavior != nullptr) { + currentBehavior->exit(); + currentBehavior = nullptr; + defaultBehavior->initialize(); + } + // If not no transition needed. } - ~StateController(); + ~StateController() {} }; } // namespace Pronounce diff --git a/include/stateMachine/wait.hpp b/include/stateMachine/wait.hpp index bf7a4e95..d5436be8 100644 --- a/include/stateMachine/wait.hpp +++ b/include/stateMachine/wait.hpp @@ -1,7 +1,5 @@ #pragma once -#include "stateMachine/behavior.hpp" - int get_time_ms(); #ifdef DEBUG @@ -14,6 +12,8 @@ int get_time_ms() { } #else + +#include "stateMachine/behavior.hpp" // Robot specific includes/time calculation #include "api.h" @@ -25,7 +25,7 @@ int get_time_ms() { #endif // DEBUG namespace Pronounce { - class Wait { + class Wait : public Behavior { private: int startTime; int duration; diff --git a/project.pros b/project.pros index b7a6aabf..c683174a 100644 --- a/project.pros +++ b/project.pros @@ -1 +1 @@ -{"py/object": "pros.conductor.project.Project", "py/state": {"target": "v5", "templates": {"kernel": {"py/object": "pros.conductor.templates.local_template.LocalTemplate", "location": "/home/alex/.config/pros/templates/kernel@3.5.4", "system_files": ["include/display/lv_themes/lv_theme_night.h", "include/display/lv_hal/lv_hal.h", "include/display/lv_themes/lv_theme_templ.h", "include/display/lv_objx/lv_imgbtn.h", "include/display/lv_draw/lv_draw_img.h", "include/display/lv_objx/lv_cont.h", "include/display/lv_draw/lv_draw_arc.h", "include/display/lv_version.h", "include/display/lv_core/lv_style.h", "include/display/lv_objx/lv_ddlist.h", "include/display/lv_objx/lv_mbox.h", "include/display/lv_objx/lv_spinbox.h", "include/display/lv_objx/lv_objx_templ.h", "include/display/lv_objx/lv_line.h", "include/pros/adi.hpp", "include/display/lv_misc/lv_task.h", "include/pros/optical.hpp", "include/pros/serial.h", "include/pros/imu.hpp", "include/display/lv_core/lv_group.h", "include/display/lv_conf.h", "include/pros/vision.hpp", "include/pros/rtos.h", "firmware/libm.a", "firmware/v5.ld", "include/display/lv_objx/lv_bar.h", "include/pros/motors.hpp", "include/display/lv_misc/lv_fs.h", "include/display/lv_misc/lv_templ.h", "include/display/lv_draw/lv_draw_vbasic.h", "include/display/lv_objx/lv_list.h", "include/display/lv_themes/lv_theme.h", "include/display/lv_objx/lv_chart.h", "include/pros/motors.h", "include/display/lv_objx/lv_arc.h", "include/display/lv_core/lv_vdb.h", "include/display/lv_misc/lv_txt.h", "include/display/lvgl.h", "include/display/lv_misc/lv_color.h", "include/display/lv_core/lv_indev.h", "include/display/lv_objx/lv_kb.h", "include/pros/llemu.h", "firmware/v5-common.ld", "include/display/README.md", "include/pros/misc.hpp", "include/display/lv_objx/lv_tabview.h", "include/display/lv_draw/lv_draw_rbasic.h", "common.mk", "include/display/lv_misc/lv_font.h", "include/display/lv_objx/lv_led.h", "include/display/lv_core/lv_obj.h", "include/display/lv_draw/lv_draw_line.h", "include/pros/rtos.hpp", "include/pros/apix.h", "include/display/lv_objx/lv_sw.h", "include/display/lv_misc/lv_ll.h", "include/pros/vision.h", "include/display/lv_objx/lv_ta.h", "include/display/lv_draw/lv_draw_label.h", "include/display/lv_core/lv_refr.h", "include/api.h", "firmware/libpros.a", "firmware/libc.a", "include/pros/ext_adi.h", "include/display/lv_hal/lv_hal.mk", "include/pros/colors.h", "include/pros/distance.h", "include/pros/screen.hpp", "include/pros/distance.hpp", "include/display/lv_themes/lv_theme_alien.h", "include/pros/gps.h", "include/display/licence.txt", "include/display/lv_misc/lv_gc.h", "include/display/lv_objx/lv_btnm.h", "include/display/lv_objx/lv_page.h", "include/pros/gps.hpp", "include/pros/llemu.hpp", "include/display/lv_hal/lv_hal_disp.h", "include/display/lv_misc/lv_circ.h", "include/display/lv_objx/lv_btn.h", "include/display/lv_misc/lv_anim.h", "include/display/lv_draw/lv_draw.mk", "include/display/lv_core/lv_lang.h", "include/pros/api_legacy.h", "include/display/lv_themes/lv_theme_nemo.h", "include/display/lv_misc/lv_math.h", "include/display/lv_misc/lv_symbol_def.h", "include/display/lv_draw/lv_draw_rect.h", "include/display/lv_misc/lv_mem.h", "include/display/lv_conf_checker.h", "include/display/lv_fonts/lv_font_builtin.h", "include/display/lv_objx/lv_table.h", "include/pros/rotation.hpp", "include/display/lv_themes/lv_theme_default.h", "include/display/lv_core/lv_core.mk", "include/pros/screen.h", "include/display/lv_draw/lv_draw_triangle.h", "include/display/lv_objx/lv_lmeter.h", "include/display/lv_hal/lv_hal_indev.h", "include/display/lv_objx/lv_objx.mk", "include/display/lv_objx/lv_win.h", "include/display/lv_themes/lv_theme_zen.h", "include/pros/misc.h", "include/display/lv_objx/lv_img.h", "include/display/lv_themes/lv_theme_mono.h", "include/display/lv_hal/lv_hal_tick.h", "include/display/lv_fonts/lv_fonts.mk", "include/pros/optical.h", "include/display/lv_objx/lv_gauge.h", "include/display/lv_misc/lv_ufs.h", "include/display/lv_misc/lv_log.h", "include/display/lv_objx/lv_canvas.h", "include/display/lv_objx/lv_calendar.h", "include/pros/adi.h", "include/display/lv_themes/lv_theme_material.h", "include/display/lv_draw/lv_draw.h", "include/display/lv_misc/lv_misc.mk", "include/display/lv_objx/lv_slider.h", "include/display/lv_objx/lv_cb.h", "include/pros/imu.h", "include/display/lv_objx/lv_label.h", "include/display/lv_objx/lv_roller.h", "include/pros/serial.hpp", "firmware/v5-hot.ld", "include/display/lv_misc/lv_area.h", "include/pros/rotation.h", "include/display/lv_objx/lv_tileview.h", "include/display/lv_objx/lv_preload.h", "include/display/lv_themes/lv_themes.mk"], "user_files": ["Makefile", "src/main.c", "src/main.cc", "include/main.hpp", "include/main.hh", "src/main.cpp", "include/main.h", ".gitignore"], "name": "kernel", "version": "3.5.4", "supported_kernels": null, "target": "v5", "metadata": {"cold_addr": "58720256", "cold_output": "bin/cold.package.bin", "hot_addr": "125829120", "hot_output": "bin/hot.package.bin", "output": "bin/monolith.bin", "origin": "pros-mainline"}}, "okapilib": {"py/object": "pros.conductor.templates.local_template.LocalTemplate", "location": "/home/alex/.config/pros/templates/okapilib@4.2.0", "metadata": {"origin": "pros-mainline"}, "name": "okapilib", "supported_kernels": "^3.3.1", "system_files": ["include/okapi/impl/filter/velMathFactory.hpp", "include/okapi/api/odometry/twoEncoderOdometry.hpp", "include/okapi/api/control/util/pathfinderUtil.hpp", "include/okapi/impl/device/rotarysensor/adiGyro.hpp", "include/okapi/pathfinder/include/pathfinder/spline.h", "include/okapi/api/units/QAngularAcceleration.hpp", "include/okapi/pathfinder/include/pathfinder/trajectory.h", "include/okapi/api/control/iterative/iterativePosPidController.hpp", "include/okapi/pathfinder/include/pathfinder/io.h", "include/okapi/api/control/async/asyncController.hpp", "include/okapi/api/util/timeUtil.hpp", "include/okapi/api/control/async/asyncVelocityController.hpp", "include/okapi/pathfinder/include/pathfinder/fit.h", "include/okapi/api/filter/medianFilter.hpp", "include/okapi/api/chassis/model/threeEncoderSkidSteerModel.hpp", "include/okapi/api/device/motor/abstractMotor.hpp", "include/okapi/pathfinder/include/pathfinder/structs.h", "include/okapi/api/units/QAngle.hpp", "include/okapi/api/control/async/asyncPositionController.hpp", "include/okapi/pathfinder/include/pathfinder/lib.h", "include/okapi/impl/control/iterative/iterativeControllerFactory.hpp", "include/okapi/impl/device/rotarysensor/IMU.hpp", "include/okapi/pathfinder/include/pathfinder/followers/encoder.h", "include/okapi/impl/control/async/asyncVelControllerBuilder.hpp", "include/okapi/api/control/util/pidTuner.hpp", "include/okapi/api/units/QAcceleration.hpp", "include/okapi/impl/device/motor/adiMotor.hpp", "include/okapi/impl/util/rate.hpp", "include/okapi/api/filter/velMath.hpp", "include/okapi/api.hpp", "include/okapi/api/units/QJerk.hpp", "include/okapi/api/odometry/stateMode.hpp", "include/okapi/api/util/mathUtil.hpp", "include/okapi/api/control/async/asyncVelPidController.hpp", "include/okapi/impl/control/async/asyncMotionProfileControllerBuilder.hpp", "include/okapi/api/units/QLength.hpp", "include/okapi/pathfinder/include/pathfinder/followers/distance.h", "include/okapi/api/chassis/controller/defaultOdomChassisController.hpp", "include/okapi/api/util/logging.hpp", "include/okapi/pathfinder/include/pathfinder/modifiers/tank.h", "include/okapi/api/odometry/odomState.hpp", "include/okapi/api/device/rotarysensor/rotarySensor.hpp", "include/okapi/api/device/rotarysensor/continuousRotarySensor.hpp", "include/okapi/api/chassis/controller/chassisScales.hpp", "include/okapi/api/units/QTime.hpp", "include/okapi/api/control/async/asyncLinearMotionProfileController.hpp", "include/okapi/api/units/QTorque.hpp", "include/okapi/api/device/button/abstractButton.hpp", "include/okapi/impl/util/configurableTimeUtilFactory.hpp", "include/okapi/impl/device/button/adiButton.hpp", "include/okapi/api/chassis/controller/chassisControllerIntegrated.hpp", "include/okapi/api/control/iterative/iterativeMotorVelocityController.hpp", "include/okapi/api/filter/emaFilter.hpp", "include/okapi/api/util/abstractRate.hpp", "firmware/okapilib.a", "include/okapi/api/chassis/model/skidSteerModel.hpp", "include/okapi/impl/control/util/pidTunerFactory.hpp", "include/okapi/api/units/QSpeed.hpp", "include/okapi/api/units/QAngularJerk.hpp", "include/okapi/api/chassis/model/chassisModel.hpp", "include/okapi/api/control/offsettableControllerInput.hpp", "include/okapi/api/units/QArea.hpp", "include/okapi/api/filter/filteredControllerInput.hpp", "include/okapi/api/control/iterative/iterativeVelocityController.hpp", "include/okapi/impl/util/timer.hpp", "include/okapi/pathfinder/include/pathfinder/mathutil.h", "include/okapi/impl/device/distanceSensor.hpp", "include/okapi/api/coreProsAPI.hpp", "include/okapi/api/control/async/asyncPosPidController.hpp", "include/okapi/api/chassis/controller/chassisController.hpp", "include/okapi/api/control/async/asyncVelIntegratedController.hpp", "include/okapi/api/control/closedLoopController.hpp", "include/okapi/api/filter/composableFilter.hpp", "include/okapi/api/chassis/controller/chassisControllerPid.hpp", "include/okapi/impl/control/util/controllerRunnerFactory.hpp", "include/okapi/api/odometry/threeEncoderOdometry.hpp", "include/okapi/impl/device/opticalSensor.hpp", "include/okapi/api/chassis/model/readOnlyChassisModel.hpp", "include/okapi/api/units/QMass.hpp", "include/okapi/impl/device/rotarysensor/integratedEncoder.hpp", "include/okapi/api/filter/demaFilter.hpp", "include/okapi/impl/util/timeUtilFactory.hpp", "include/okapi/impl/device/motor/motor.hpp", "include/okapi/api/control/async/asyncPosIntegratedController.hpp", "include/okapi/api/odometry/odometry.hpp", "include/okapi/api/control/iterative/iterativeVelPidController.hpp", "include/okapi/impl/chassis/controller/chassisControllerBuilder.hpp", "include/okapi/api/odometry/point.hpp", "include/okapi/api/control/util/controllerRunner.hpp", "include/okapi/api/chassis/model/hDriveModel.hpp", "include/okapi/api/control/async/asyncMotionProfileController.hpp", "include/okapi/api/control/async/asyncWrapper.hpp", "include/okapi/impl/device/controller.hpp", "include/okapi/api/control/iterative/iterativeController.hpp", "include/okapi/api/units/QFrequency.hpp", "include/okapi/api/device/button/buttonBase.hpp", "include/okapi/api/control/iterative/iterativePositionController.hpp", "include/okapi/pathfinder/include/pathfinder/modifiers/swerve.h", "include/okapi/api/filter/filter.hpp", "include/okapi/api/odometry/odomMath.hpp", "include/okapi/impl/device/rotarysensor/adiEncoder.hpp", "include/okapi/api/util/abstractTimer.hpp", "include/okapi/impl/device/rotarysensor/potentiometer.hpp", "include/okapi/impl/device/button/controllerButton.hpp", "include/okapi/api/filter/ekfFilter.hpp", "include/okapi/api/util/supplier.hpp", "include/okapi/api/units/RQuantity.hpp", "include/okapi/api/units/QForce.hpp", "include/okapi/api/control/controllerOutput.hpp", "include/okapi/impl/device/adiUltrasonic.hpp", "include/okapi/api/units/QVolume.hpp", "include/okapi/pathfinder/include/pathfinder.h", "include/okapi/api/filter/passthroughFilter.hpp", "include/okapi/api/control/controllerInput.hpp", "include/okapi/impl/device/controllerUtil.hpp", "include/okapi/api/chassis/model/threeEncoderXDriveModel.hpp", "include/okapi/api/control/util/flywheelSimulator.hpp", "include/okapi/api/filter/averageFilter.hpp", "include/okapi/api/units/QPressure.hpp", "include/okapi/impl/control/async/asyncPosControllerBuilder.hpp", "include/okapi/api/units/QAngularSpeed.hpp", "include/okapi/impl/device/motor/motorGroup.hpp", "include/okapi/api/chassis/controller/odomChassisController.hpp", "include/okapi/api/chassis/model/xDriveModel.hpp", "include/okapi/api/control/util/settledUtil.hpp", "include/okapi/impl/device/rotarysensor/rotationSensor.hpp"], "target": "v5", "user_files": [], "version": "4.2.0"}, "pros-grafana-lib": {"py/object": "pros.conductor.templates.local_template.LocalTemplate", "location": "/home/alex/.config/pros/templates/pros-grafana-lib@1.0.0", "system_files": ["include\\pros-grafana-lib\\variables\\variable.h", "include\\pros-grafana-lib\\variables\\variabledatahandler.h", "include\\pros-grafana-lib\\lib\\json.hpp", "include\\pros-grafana-lib\\api.h", "include\\pros-grafana-lib\\guimanager.h", "firmware\\pros-grafana-lib.a", "include\\pros-grafana-lib\\variables\\variablegroup.h"], "user_files": [], "name": "pros-grafana-lib", "version": "1.0.0", "supported_kernels": "^3.5.4", "target": "v5", "metadata": {"origin": "local"}}}, "upload_options": {"slot": 1, "description": "VEX Team 2654P Code"}, "project_name": "Pronounce This"}} \ No newline at end of file +{"py/object": "pros.conductor.project.Project", "py/state": {"target": "v5", "templates": {"kernel": {"py/object": "pros.conductor.templates.local_template.LocalTemplate", "location": "/home/alex/.config/pros/templates/kernel@3.6.0", "system_files": ["include/display/lv_objx/lv_led.h", "include/display/lv_misc/lv_math.h", "include/display/lv_themes/lv_theme.h", "include/display/lv_objx/lv_roller.h", "include/display/lv_conf.h", "include/display/lv_themes/lv_theme_night.h", "include/display/lv_objx/lv_spinbox.h", "include/display/lv_conf_checker.h", "include/display/lvgl.h", "include/display/lv_objx/lv_preload.h", "include/display/lv_themes/lv_theme_material.h", "include/display/lv_draw/lv_draw_triangle.h", "include/pros/link.h", "include/display/lv_draw/lv_draw_label.h", "include/display/lv_objx/lv_cb.h", "include/display/lv_misc/lv_txt.h", "include/pros/colors.h", "include/display/lv_misc/lv_font.h", "include/display/lv_misc/lv_misc.mk", "include/display/lv_themes/lv_themes.mk", "include/pros/rtos.hpp", "include/display/lv_core/lv_refr.h", "include/display/lv_draw/lv_draw_rect.h", "include/display/lv_objx/lv_list.h", "include/pros/api_legacy.h", "include/pros/optical.hpp", "include/display/lv_hal/lv_hal.mk", "include/pros/rotation.hpp", "include/pros/optical.h", "include/display/lv_misc/lv_anim.h", "include/display/lv_draw/lv_draw_rbasic.h", "include/display/lv_misc/lv_task.h", "include/display/lv_themes/lv_theme_zen.h", "include/display/lv_objx/lv_btn.h", "include/display/lv_misc/lv_ll.h", "include/display/lv_misc/lv_area.h", "include/display/lv_objx/lv_table.h", "include/display/lv_hal/lv_hal_indev.h", "include/pros/serial.hpp", "include/display/lv_misc/lv_ufs.h", "include/display/lv_misc/lv_fs.h", "include/display/lv_draw/lv_draw_arc.h", "include/display/lv_themes/lv_theme_default.h", "include/display/licence.txt", "include/pros/misc.hpp", "include/display/lv_objx/lv_page.h", "include/display/lv_objx/lv_img.h", "include/pros/imu.h", "include/display/lv_core/lv_vdb.h", "include/display/lv_objx/lv_chart.h", "include/display/lv_core/lv_core.mk", "include/display/lv_version.h", "include/display/lv_objx/lv_arc.h", "include/display/lv_fonts/lv_font_builtin.h", "include/display/lv_objx/lv_objx.mk", "include/display/lv_themes/lv_theme_nemo.h", "firmware/v5-hot.ld", "include/pros/screen.hpp", "include/pros/llemu.h", "include/display/lv_objx/lv_ddlist.h", "include/display/lv_objx/lv_slider.h", "include/display/lv_objx/lv_win.h", "include/pros/link.hpp", "include/pros/serial.h", "firmware/libm.a", "include/pros/apix.h", "include/pros/rotation.h", "include/display/lv_objx/lv_lmeter.h", "include/display/lv_objx/lv_btnm.h", "include/display/lv_misc/lv_symbol_def.h", "include/display/lv_objx/lv_label.h", "include/pros/vision.h", "include/display/lv_misc/lv_circ.h", "include/display/lv_hal/lv_hal.h", "firmware/v5.ld", "include/pros/llemu.hpp", "include/display/lv_objx/lv_canvas.h", "include/pros/ext_adi.h", "include/display/lv_themes/lv_theme_templ.h", "include/display/lv_objx/lv_sw.h", "include/display/lv_draw/lv_draw.h", "include/display/lv_misc/lv_log.h", "include/display/lv_hal/lv_hal_disp.h", "include/display/lv_misc/lv_gc.h", "include/display/lv_core/lv_obj.h", "include/display/lv_objx/lv_objx_templ.h", "include/display/lv_objx/lv_tabview.h", "include/pros/motors.hpp", "include/display/lv_core/lv_style.h", "include/pros/gps.h", "include/display/lv_core/lv_indev.h", "include/display/lv_objx/lv_calendar.h", "include/pros/gps.hpp", "include/pros/motors.h", "include/display/lv_objx/lv_gauge.h", "include/pros/distance.hpp", "include/display/README.md", "firmware/v5-common.ld", "include/pros/adi.h", "include/display/lv_themes/lv_theme_alien.h", "include/display/lv_objx/lv_mbox.h", "include/display/lv_objx/lv_cont.h", "include/display/lv_draw/lv_draw.mk", "include/pros/screen.h", "firmware/libc.a", "include/display/lv_misc/lv_mem.h", "include/display/lv_objx/lv_line.h", "include/display/lv_objx/lv_imgbtn.h", "include/display/lv_misc/lv_templ.h", "include/display/lv_objx/lv_bar.h", "include/display/lv_fonts/lv_fonts.mk", "include/display/lv_draw/lv_draw_vbasic.h", "include/pros/rtos.h", "include/pros/adi.hpp", "include/display/lv_draw/lv_draw_img.h", "include/display/lv_objx/lv_kb.h", "include/pros/distance.h", "include/display/lv_hal/lv_hal_tick.h", "include/api.h", "include/pros/imu.hpp", "include/display/lv_core/lv_lang.h", "include/display/lv_draw/lv_draw_line.h", "firmware/libpros.a", "include/display/lv_core/lv_group.h", "common.mk", "include/pros/vision.hpp", "include/display/lv_themes/lv_theme_mono.h", "include/display/lv_objx/lv_ta.h", "include/display/lv_misc/lv_color.h", "include/pros/misc.h", "include/display/lv_objx/lv_tileview.h"], "user_files": ["src/main.cpp", "src/main.cc", "include/main.h", "include/main.hh", "src/main.c", "include/main.hpp", "Makefile", ".gitignore"], "name": "kernel", "version": "3.6.0", "supported_kernels": null, "target": "v5", "metadata": {"cold_addr": "58720256", "cold_output": "bin/cold.package.bin", "hot_addr": "125829120", "hot_output": "bin/hot.package.bin", "output": "bin/monolith.bin", "origin": "pros-mainline"}}, "okapilib": {"py/object": "pros.conductor.templates.local_template.LocalTemplate", "location": "/home/alex/.config/pros/templates/okapilib@4.4.0", "system_files": ["include/okapi/api/units/QJerk.hpp", "include/okapi/impl/filter/velMathFactory.hpp", "include/okapi/api/units/QPressure.hpp", "include/okapi/impl/device/rotarysensor/rotationSensor.hpp", "include/okapi/impl/util/configurableTimeUtilFactory.hpp", "include/okapi/api/control/closedLoopController.hpp", "include/okapi/impl/device/rotarysensor/potentiometer.hpp", "include/okapi/api/device/button/buttonBase.hpp", "include/okapi/api/units/QArea.hpp", "include/okapi/api/device/motor/abstractMotor.hpp", "include/okapi/api/chassis/model/skidSteerModel.hpp", "include/okapi/api/device/button/abstractButton.hpp", "include/okapi/api/control/iterative/iterativeMotorVelocityController.hpp", "include/okapi/api/units/QFrequency.hpp", "include/okapi/api/units/QAngularSpeed.hpp", "include/okapi/api/odometry/stateMode.hpp", "include/okapi/impl/device/rotarysensor/adiGyro.hpp", "include/okapi/api/units/QSpeed.hpp", "include/okapi/api/filter/filteredControllerInput.hpp", "include/okapi/api/control/iterative/iterativeVelPidController.hpp", "include/okapi/impl/device/motor/motorGroup.hpp", "firmware/squiggles.mk", "include/okapi/api/util/timeUtil.hpp", "include/okapi/squiggles/constraints.hpp", "include/okapi/api/control/util/pathfinderUtil.hpp", "include/okapi/api/control/util/pidTuner.hpp", "include/okapi/impl/control/util/pidTunerFactory.hpp", "include/okapi/api/util/mathUtil.hpp", "include/okapi/api/control/async/asyncPositionController.hpp", "include/okapi/impl/device/motor/adiMotor.hpp", "include/okapi/impl/device/rotarysensor/adiEncoder.hpp", "include/okapi/api/odometry/odomMath.hpp", "include/okapi/api/filter/filter.hpp", "include/okapi/api/chassis/model/hDriveModel.hpp", "include/okapi/api/chassis/controller/chassisControllerPid.hpp", "include/okapi/api/filter/demaFilter.hpp", "include/okapi/squiggles/geometry/profilepoint.hpp", "include/okapi/api/filter/averageFilter.hpp", "include/okapi/squiggles/io.hpp", "include/okapi/api/control/async/asyncLinearMotionProfileController.hpp", "include/okapi/squiggles/math/utils.hpp", "include/okapi/api/chassis/model/xDriveModel.hpp", "include/okapi/squiggles/geometry/pose.hpp", "include/okapi/api/odometry/twoEncoderOdometry.hpp", "include/okapi/impl/device/rotarysensor/IMU.hpp", "include/okapi/api/filter/velMath.hpp", "include/okapi/impl/chassis/controller/chassisControllerBuilder.hpp", "include/okapi/impl/util/rate.hpp", "include/okapi/api/control/controllerInput.hpp", "include/okapi/api/control/iterative/iterativeVelocityController.hpp", "include/okapi/api/device/rotarysensor/rotarySensor.hpp", "include/okapi/api/control/offsettableControllerInput.hpp", "include/okapi/api/util/abstractTimer.hpp", "include/okapi/api/filter/composableFilter.hpp", "include/okapi/api.hpp", "include/okapi/squiggles/physicalmodel/passthroughmodel.hpp", "include/okapi/api/chassis/controller/defaultOdomChassisController.hpp", "firmware/okapilib.a", "include/okapi/impl/device/controller.hpp", "include/okapi/impl/control/async/asyncMotionProfileControllerBuilder.hpp", "include/okapi/api/control/async/asyncController.hpp", "include/okapi/squiggles/squiggles.hpp", "include/okapi/api/chassis/controller/chassisController.hpp", "include/okapi/api/control/controllerOutput.hpp", "include/okapi/api/units/QTime.hpp", "include/okapi/impl/device/rotarysensor/integratedEncoder.hpp", "include/okapi/api/control/async/asyncVelIntegratedController.hpp", "include/okapi/api/chassis/model/chassisModel.hpp", "include/okapi/impl/device/adiUltrasonic.hpp", "include/okapi/api/odometry/point.hpp", "include/okapi/impl/device/distanceSensor.hpp", "include/okapi/api/util/supplier.hpp", "include/okapi/api/units/QLength.hpp", "include/okapi/api/units/QAngle.hpp", "include/okapi/api/device/rotarysensor/continuousRotarySensor.hpp", "include/okapi/api/units/QMass.hpp", "include/okapi/api/units/QVolume.hpp", "include/okapi/api/odometry/threeEncoderOdometry.hpp", "include/okapi/api/units/RQuantity.hpp", "include/okapi/impl/control/iterative/iterativeControllerFactory.hpp", "include/okapi/api/control/async/asyncVelPidController.hpp", "include/okapi/api/filter/emaFilter.hpp", "include/okapi/api/filter/ekfFilter.hpp", "include/okapi/api/control/async/asyncPosPidController.hpp", "include/okapi/impl/util/timer.hpp", "include/okapi/api/coreProsAPI.hpp", "include/okapi/api/odometry/odometry.hpp", "include/okapi/impl/device/button/controllerButton.hpp", "include/okapi/api/control/iterative/iterativePositionController.hpp", "include/okapi/api/chassis/controller/odomChassisController.hpp", "include/okapi/api/odometry/odomState.hpp", "include/okapi/api/chassis/controller/chassisScales.hpp", "include/okapi/api/control/async/asyncPosIntegratedController.hpp", "include/okapi/api/control/iterative/iterativeController.hpp", "include/okapi/squiggles/physicalmodel/physicalmodel.hpp", "include/okapi/impl/control/util/controllerRunnerFactory.hpp", "include/okapi/squiggles/math/quinticpolynomial.hpp", "include/okapi/impl/device/motor/motor.hpp", "include/okapi/api/filter/medianFilter.hpp", "include/okapi/api/chassis/controller/chassisControllerIntegrated.hpp", "include/okapi/squiggles/spline.hpp", "include/okapi/api/control/util/flywheelSimulator.hpp", "include/okapi/squiggles/geometry/controlvector.hpp", "include/okapi/api/control/util/controllerRunner.hpp", "include/okapi/api/control/iterative/iterativePosPidController.hpp", "include/okapi/api/chassis/model/readOnlyChassisModel.hpp", "include/okapi/impl/device/opticalSensor.hpp", "include/okapi/api/util/logging.hpp", "include/okapi/api/control/util/settledUtil.hpp", "include/okapi/api/control/async/asyncMotionProfileController.hpp", "include/okapi/impl/control/async/asyncVelControllerBuilder.hpp", "include/okapi/api/units/QAngularAcceleration.hpp", "include/okapi/api/filter/passthroughFilter.hpp", "include/okapi/impl/device/button/adiButton.hpp", "include/okapi/api/units/QAcceleration.hpp", "include/okapi/api/units/QAngularJerk.hpp", "include/okapi/impl/util/timeUtilFactory.hpp", "include/okapi/api/control/async/asyncVelocityController.hpp", "include/okapi/impl/device/controllerUtil.hpp", "include/okapi/api/control/async/asyncWrapper.hpp", "include/okapi/api/units/QTorque.hpp", "include/okapi/api/chassis/model/threeEncoderSkidSteerModel.hpp", "include/okapi/impl/control/async/asyncPosControllerBuilder.hpp", "include/okapi/api/units/QForce.hpp", "include/okapi/squiggles/physicalmodel/tankmodel.hpp", "include/okapi/api/util/abstractRate.hpp", "include/okapi/api/chassis/model/threeEncoderXDriveModel.hpp"], "user_files": [], "name": "okapilib", "version": "4.4.0", "supported_kernels": "^3.3.1", "target": "v5", "metadata": {"origin": "pros-mainline"}}, "pros-grafana-lib": {"py/object": "pros.conductor.templates.local_template.LocalTemplate", "location": "/home/alex/.config/pros/templates/pros-grafana-lib@1.0.0", "system_files": ["include\\pros-grafana-lib\\variables\\variable.h", "include\\pros-grafana-lib\\variables\\variabledatahandler.h", "include\\pros-grafana-lib\\lib\\json.hpp", "include\\pros-grafana-lib\\api.h", "include\\pros-grafana-lib\\guimanager.h", "firmware\\pros-grafana-lib.a", "include\\pros-grafana-lib\\variables\\variablegroup.h"], "user_files": [], "name": "pros-grafana-lib", "version": "1.0.0", "supported_kernels": "^3.5.4", "target": "v5", "metadata": {"origin": "local"}}}, "upload_options": {"slot": 1, "description": "VEX Team 2654P Code"}, "project_name": "Pronounce This"}} \ No newline at end of file diff --git a/src/feedbackControllers/pid.cpp b/src/feedbackControllers/pid.cpp index bbd75665..edf014d6 100644 --- a/src/feedbackControllers/pid.cpp +++ b/src/feedbackControllers/pid.cpp @@ -19,30 +19,9 @@ namespace Pronounce { this->turnPid = turnPid; } - double PID::update() { - if (turnPid) { - this->error = angleDifference(target, position); - } - else { - this->error = target - position; - } + double PID::update(double input) { - this->derivitive = error - prevError; - - if (abs(error) < integralBound) { - totalError += error; - } - else { - totalError = 0; - } - - totalError = abs(totalError) > maxIntegral ? signnum_c(totalError) * maxIntegral : totalError; - - this->power = error * kP + derivitive * kD + totalError * kI; - - prevError = error; - - return this->power; + return calculatePidValues(input); } PID::~PID() { diff --git a/src/main.cpp b/src/main.cpp index 96169828..88d28147 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -156,14 +156,6 @@ void runSelector() { */ void initLogger() { - Logger::setDefaultLogger( - std::make_shared( - TimeUtilFactory::createDefault().getTimer(), - "/usd/log.txt", - Logger::LogLevel::debug // Show everything - ) - ); - Logger::getDefaultLogger()->debug("LOGGER: Logger initialized"); } /** diff --git a/src/motionControl/balance.cpp b/src/motionControl/balance.cpp index 2d925af7..16ea1447 100644 --- a/src/motionControl/balance.cpp +++ b/src/motionControl/balance.cpp @@ -26,8 +26,7 @@ namespace Pronounce { double speed = linearController->update(imu->get_roll()+7); // Calculate the turing force with the orientation PID - this->orientationController->setPosition(toRadians(this->imu->get_heading())); - double turn = this->orientationController->update(); + double turn = this->orientationController->update(toRadians(this->imu->get_heading())); // Send the calculated values to the drivetrain this->drivetrain->skidSteerVelocity(speed, turn); diff --git a/src/motionControl/omniPurePursuit.cpp b/src/motionControl/omniPurePursuit.cpp index 8d2eb99a..db446209 100644 --- a/src/motionControl/omniPurePursuit.cpp +++ b/src/motionControl/omniPurePursuit.cpp @@ -41,10 +41,9 @@ namespace Pronounce { // Get the turn target this->getCurrentProfile().orientationPid->setTarget(this->getTurnTarget()); - this->getCurrentProfile().orientationPid->setPosition(this->getOdometry()->getPosition()->getTheta()); // Get the turn output - double turnOutput = this->getCurrentProfile().orientationPid->update(); + double turnOutput = this->getCurrentProfile().orientationPid->update(this->getOdometry()->getPosition()->getTheta()); // Send values to the drivetrain drivetrain->setDriveVectorVelocity(Vector(lateralOutput, pointData.normalizedLookaheadVector.getAngle()), turnOutput); diff --git a/src/motionControl/tankPurePursuit.cpp b/src/motionControl/tankPurePursuit.cpp index b65031f1..669bcb87 100644 --- a/src/motionControl/tankPurePursuit.cpp +++ b/src/motionControl/tankPurePursuit.cpp @@ -29,8 +29,7 @@ namespace Pronounce { if (orientationControl) { double currentOrientation = this->getOdometry()->getPosition()->getTheta(); - this->turnPid->setPosition(angleDifference(currentOrientation, 0)); - double spinSpeed = this->turnPid->update(); + double spinSpeed = this->turnPid->update(angleDifference(currentOrientation, 0)); std::cout << "Angle difference: " << this->turnPid->getError() << std::endl; diff --git a/test/behavior b/test/behavior new file mode 100755 index 00000000..b1e56687 Binary files /dev/null and b/test/behavior differ diff --git a/test/behavior.cpp b/test/behavior.cpp new file mode 100644 index 00000000..d7017bd5 --- /dev/null +++ b/test/behavior.cpp @@ -0,0 +1,109 @@ +// Enable debug for some classes +#define DEBUG + +#include "../include/stateMachine/behavior.hpp" +#include "../include/stateMachine/wait.hpp" +#include "../include/stateMachine/stateController.hpp" +#include "../include/stateMachine/sequence.hpp" +#include "../include/stateMachine/behaviorGroup.hpp" +#include "testBehvaior.hpp" +#include +#include +#include + +// Import timers so it works on both linux and windows +#ifdef _WIN32 +#include +#else +#include +#endif + +using namespace Pronounce; + +/** + * @brief Tester class for the state machine + * + * @return int + */ +int main() { + // Create state objects to test the state machine with + TestBehavior codeOrange = TestBehavior("Code Orange"); + TestBehavior codeGreen = TestBehavior("Code Green"); + TestBehavior defaultState = TestBehavior("Default"); + + // Create wait states to test the wait class with + Wait codeOrange2Seconds = Wait(&codeOrange, 2000); + Wait codeGreen3Seconds = Wait(&codeGreen, 3000); + + // Create a state machine for both the state extensions(Parallel/sequences) and a state machine for example a subsystem + StateController stateController = StateController(&defaultState); + StateController stateExtensions = StateController(new Behavior()); + + // Create a test sequence + Sequence codeBrown = Sequence(); + + // Add states to the sequence + codeBrown.addState(&stateController, &codeOrange2Seconds); + codeBrown.addState(&stateController, &codeGreen3Seconds); + + // Group all the behaviors to make it easier to use + BehaviorGroup behaviorGroup = BehaviorGroup(); + + // Add the behaviors + //! Have to be in this order or else there will be an error + behaviorGroup.addBehavior(&stateExtensions); + behaviorGroup.addBehavior(&stateController); + + // Initialize all the behaviors + behaviorGroup.initialize(); + + for (int i = 0; i < 150; i++) { + // Update all the behaviors + behaviorGroup.update(); + + // Print out the index + std::cout << i << ": "; + + // Start codeOrange behavior at index 10 + if (i == 10) { + stateController.setCurrentBehavior(&codeOrange); + } + + // End the behavior at index 20 by telling the state it is done + if (i == 20) { + codeOrange.setDone(true); + } + + // Test if it will stop quickly + if (i == 30) { + stateController.setCurrentBehavior(&codeOrange); + } + + // Start the behavior again + if (i == 40) { + codeOrange.setDone(false); + stateController.setCurrentBehavior(&codeOrange); + } + + // Start the default behavior by giving a pointer to it + if (i == 50) { + stateController.setCurrentBehavior(&defaultState); + } + + // Run the wait state for 2 seconds or 20 frames + if (i == 60) { + stateController.setCurrentBehavior(&codeOrange2Seconds); + } + + // Start the sequence. + if (i == 90) { + stateExtensions.setCurrentBehavior(&codeBrown); + } + + // Delay for 100 seconds so that wait sequences will work + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // Exit all the states + behaviorGroup.exit(); +} \ No newline at end of file diff --git a/test/testBehvaior.hpp b/test/testBehvaior.hpp new file mode 100644 index 00000000..37098570 --- /dev/null +++ b/test/testBehvaior.hpp @@ -0,0 +1,83 @@ +#pragma once + +#include "../include/stateMachine/behavior.hpp" +#include +#include + +namespace Pronounce +{ + /** + * @brief A behavior to test the state machine systems + * + */ + class TestBehavior : public Behavior { + private: + /** + * @brief Manually inputted name to differentiate objects + * + */ + std::string name; + + /** + * @brief Be able to manually trigger the states stop + * + */ + bool done = false; + public: + /** + * @brief Construct a new Test Behavior object + * + * @param name - Name for this object + */ + TestBehavior(std::string name) { + this->name = name; + } + + /** + * @brief Print out Init: + * + */ + void initialize() { + std::cout << "Init: " << name << std::endl; + } + + /** + * @brief Print out Init: + * + */ + void update() { + std::cout << "Update: " << name << std::endl; + } + + /** + * @brief Print out Init: + * + */ + void exit() { + std::cout << "Exit: " << name << std::endl; + } + + /** + * @brief Print out Done: if it is done + * + * @return done the done variable + */ + bool isDone() { + if (done) { + std::cout << "Done: " << name << std::endl; + } + return done; + } + + /** + * @brief Set the Done object, can manually tell the task to end + * + * @param done + */ + void setDone(bool done) { + this->done = done; + } + + ~TestBehavior() {} + }; +} // namespace Pronounce