🔼 2.2.0: Kernel Updates (#77)

Makefile

@@ -27,7 +27,7 @@ EXCLUDE_COLD_LIBRARIES:=
 IS_LIBRARY:=1
 # TODO: CHANGE THIS!
 LIBNAME:=EZ-Template
-VERSION:=2.1.2
+VERSION:=2.2.0
 # EXCLUDE_SRC_FROM_LIB= $(SRCDIR)/unpublishedfile.c
 # this line excludes opcontrol.c and similar files
 EXCLUDE_SRC_FROM_LIB+=$(foreach file, $(SRCDIR)/autons $(SRCDIR)/main,$(foreach cext,$(CEXTS),$(file).$(cext)) $(foreach cxxext,$(CXXEXTS),$(file).$(cxxext)))

README.md

@@ -11,7 +11,7 @@ permalink: /
 Simple plug-and-play PROS template that handles drive base functions for VEX robots.  
 
 
-[EZ-Template Version](https://github.com/EZ-Robotics/EZ-Template): 2.1.1  
+[EZ-Template Version](https://github.com/EZ-Robotics/EZ-Template): 2.2.0  
 
 [Autonomous routines that used EZ-Template](https://photos.app.goo.gl/yRwuvmq7hDoM4f6EA)
 

common.mk

@@ -201,7 +201,7 @@ library: $(LIBAR)
 
 .PHONY: template
 template: clean-template $(LIBAR)
-	$Dprosv5 c create-template . $(LIBNAME) $(VERSION) $(foreach file,$(TEMPLATE_FILES) $(LIBAR),--system "$(file)") --target v5 $(CREATE_TEMPLATE_FLAGS)
+	$Dpros c create-template . $(LIBNAME) $(VERSION) $(foreach file,$(TEMPLATE_FILES) $(LIBAR),--system "$(file)") --target v5 $(CREATE_TEMPLATE_FLAGS)
 endif
 
 # if project is a library source, compile the archive and link output.elf against the archive rather than source objects
@@ -280,7 +280,7 @@ $(VV)mkdir -p $(dir $(LDTIMEOBJ))
 @# The shell command $$(($$(date +%s)+($$(date +%-z)/100*3600))) fetches the current
 @# unix timestamp, and then adds the UTC timezone offset to account for time zones.
 
-$(call test_output_2,Adding timestamp ,echo 'const int _PROS_COMPILE_TIMESTAMP_INT = $(shell echo $$(($$(date +%s)+($$(date +%-z)/100*3600)))); char const * const _PROS_COMPILE_TIMESTAMP = __DATE__ " " __TIME__; char const * const _PROS_COMPILE_DIRECTORY = "$(shell pwd | tail -c 23)";' | $(CC) -c -x c $(CFLAGS) $(EXTRA_CFLAGS) -o $(LDTIMEOBJ) -,$(OK_STRING))
+$(call test_output_2,Adding timestamp ,echo 'const int _PROS_COMPILE_TIMESTAMP_INT = $(shell echo $$(($$(date +%s)+($$(date +%-z)/100*3600)))); char const * const _PROS_COMPILE_TIMESTAMP = __DATE__ " " __TIME__; char const * const _PROS_COMPILE_DIRECTORY = "$(wildcard $(shell pwd | tail -c 23))";' | $(CC) -c -x c $(CFLAGS) $(EXTRA_CFLAGS) -o $(LDTIMEOBJ) -,$(OK_STRING))
 endef
 
 # these rules are for build-compile-commands, which just print out sysroot information

firmware/squiggles.mk

@@ -0,0 +1 @@
+INCLUDE+=-iquote"$(ROOT)/include/okapi/squiggles"

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-2022, Purdue University ACM SIGBots.
+ * \copyright Copyright (c) 2017-2023, 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 7
-#define PROS_VERSION_PATCH 3
-#define PROS_VERSION_STRING "3.7.3"
+#define PROS_VERSION_MINOR 8
+#define PROS_VERSION_PATCH 0
+#define PROS_VERSION_STRING "3.8.0"
 
 #include "pros/adi.h"
 #include "pros/colors.h"

include/okapi/api.hpp

@@ -121,12 +121,14 @@
 #include "okapi/api/units/QTime.hpp"
 #include "okapi/api/units/QTorque.hpp"
 #include "okapi/api/units/QVolume.hpp"
+#include "okapi/api/units/RQuantityName.hpp"
 
 #include "okapi/api/util/abstractRate.hpp"
 #include "okapi/api/util/abstractTimer.hpp"
 #include "okapi/api/util/mathUtil.hpp"
 #include "okapi/api/util/supplier.hpp"
 #include "okapi/api/util/timeUtil.hpp"
+#include "okapi/impl/util/configurableTimeUtilFactory.hpp"
 #include "okapi/impl/util/rate.hpp"
 #include "okapi/impl/util/timeUtilFactory.hpp"
 #include "okapi/impl/util/timer.hpp"

include/okapi/api/chassis/controller/defaultOdomChassisController.hpp

@@ -99,7 +99,10 @@ class DefaultOdomChassisController : public OdomChassisController {
   void moveRawAsync(double itarget) override;
 
   /**
-   * This delegates to the input ChassisController.
+   * Turns chassis to desired angle (turns in the direction of smallest angle)
+   * (ex. If current angle is 0 and target is 270, the chassis will turn -90 degrees)
+   * 
+   * @param idegTarget target angle
    */
   void turnAngle(QAngle idegTarget) override;
 
@@ -109,7 +112,10 @@ class DefaultOdomChassisController : public OdomChassisController {
   void turnRaw(double idegTarget) override;
 
   /**
-   * This delegates to the input ChassisController.
+   * Turns chassis to desired angle (turns in the direction of smallest angle)
+   * (ex. If current angle is 0 and target is 270, the chassis will turn -90 degrees)
+   * 
+   * @param idegTarget target angle 
    */
   void turnAngleAsync(QAngle idegTarget) override;
 

include/okapi/api/chassis/model/chassisModel.hpp

@@ -83,6 +83,18 @@ class ChassisModel : public ReadOnlyChassisModel {
    */
   virtual void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) = 0;
 
+  /**
+   * Drive the robot with a curvature drive layout. The robot drives in constant radius turns
+   * where you control the curvature (inverse of radius) you drive in. This is advantageous
+   * because the forward speed will not affect the rate of turning. The algorithm switches to
+   * arcade if the forward speed is 0. Uses voltage mode.
+   *
+   * @param iforwardSpeed speed in the forward direction
+   * @param icurvature curvature (inverse of radius) to drive in
+   * @param ithreshold deadband on joystick values
+   */
+  virtual void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) = 0;
+
   /**
    * Power the left side motors. Uses velocity mode.
    *

include/okapi/api/chassis/model/hDriveModel.hpp

@@ -100,6 +100,18 @@ class HDriveModel : public ChassisModel {
    */
   void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) override;
 
+  /**
+   * Drive the robot with a curvature drive layout. The robot drives in constant radius turns
+   * where you control the curvature (inverse of radius) you drive in. This is advantageous
+   * because the forward speed will not affect the rate of turning. The algorithm switches to
+   * arcade if the forward speed is 0. Uses voltage mode. Sets the middle motor to zero velocity.
+   *
+   * @param iforwardSpeed speed in the forward direction
+   * @param icurvature curvature (inverse of radius) to drive in
+   * @param ithreshold deadband on joystick values
+   */
+  void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) override;
+
   /**
    * Drive the robot with an arcade drive layout. Uses voltage mode.
    *
@@ -111,6 +123,17 @@ class HDriveModel : public ChassisModel {
   virtual void
   hArcade(double irightSpeed, double iforwardSpeed, double iyaw, double ithreshold = 0);
 
+  /**
+   * Drive the robot with an curvature drive layout. Uses voltage mode.
+   *
+   * @param irightSpeed speed to the right
+   * @param iforwardSpeed speed in the forward direction
+   * @param icurvature curvature (inverse of radius) to drive in
+   * @param ithreshold deadband on joystick values
+   */
+  virtual void
+  hCurvature(double irightSpeed, double iforwardSpeed, double icurvature, double ithreshold = 0);
+
   /**
    * Power the left side motors. Uses velocity mode.
    *

include/okapi/api/chassis/model/skidSteerModel.hpp

@@ -87,6 +87,18 @@ class SkidSteerModel : public ChassisModel {
    */
   void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) override;
 
+  /**
+   * Drive the robot with a curvature drive layout. The robot drives in constant radius turns
+   * where you control the curvature (inverse of radius) you drive in. This is advantageous
+   * because the forward speed will not affect the rate of turning. The algorithm switches to
+   * arcade if the forward speed is 0. Uses voltage mode.
+   *
+   * @param iforwardSpeed speed in the forward direction
+   * @param icurvature curvature (inverse of radius) to drive in
+   * @param ithreshold deadband on joystick values
+   */
+  void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) override;
+
   /**
    * Power the left side motors. Uses velocity mode.
    *

include/okapi/api/chassis/model/xDriveModel.hpp

@@ -8,6 +8,7 @@
 #include "okapi/api/chassis/model/chassisModel.hpp"
 #include "okapi/api/device/motor/abstractMotor.hpp"
 #include "okapi/api/device/rotarysensor/continuousRotarySensor.hpp"
+#include "okapi/api/units/QAngle.hpp"
 
 namespace okapi {
 class XDriveModel : public ChassisModel {
@@ -113,6 +114,18 @@ class XDriveModel : public ChassisModel {
    */
   void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) override;
 
+  /**
+   * Drive the robot with a curvature drive layout. The robot drives in constant radius turns
+   * where you control the curvature (inverse of radius) you drive in. This is advantageous
+   * because the forward speed will not affect the rate of turning. The algorithm switches to
+   * arcade if the forward speed is 0. Uses voltage mode.
+   *
+   * @param iforwardSpeed speed forward direction
+   * @param icurvature curvature (inverse of radius) to drive in
+   * @param ithreshold deadband on joystick values
+   */
+  void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) override;
+
   /**
    * Drive the robot with an arcade drive layout. Uses voltage mode.
    *
@@ -124,6 +137,27 @@ class XDriveModel : public ChassisModel {
   virtual void
   xArcade(double irightSpeed, double iforwardSpeed, double iyaw, double ithreshold = 0);
 
+  /**
+   * Drive the robot with a field-oriented arcade drive layout. Uses voltage mode.
+   * For example:
+   *    Both `fieldOrientedXArcade(1, 0, 0, 0_deg)` and `fieldOrientedXArcade(1, 0, 0, 90_deg)`
+   *    will drive the chassis in the forward/north direction. In other words, no matter
+   *    the robot's heading, the robot will move forward/north when you tell it
+   *    to move forward/north and will move right/east when you tell it to move right/east.
+   *
+   *
+   * @param ixSpeed forward speed -- (`+1`) forward, (`-1`) backward
+   * @param iySpeed sideways speed -- (`+1`) right, (`-1`) left
+   * @param iyaw turn speed -- (`+1`) clockwise, (`-1`) counter-clockwise
+   * @param iangle current chassis angle (`0_deg` = no correction, winds clockwise)
+   * @param ithreshold deadband on joystick values
+   */
+  virtual void fieldOrientedXArcade(double ixSpeed,
+                                    double iySpeed,
+                                    double iyaw,
+                                    QAngle iangle,
+                                    double ithreshold = 0);
+
   /**
    * Power the left side motors. Uses velocity mode.
    *

include/okapi/api/control/async/asyncLinearMotionProfileController.hpp

@@ -15,9 +15,7 @@
 #include <atomic>
 #include <map>
 
-extern "C" {
-#include "okapi/pathfinder/include/pathfinder.h"
-}
+#include "squiggles.hpp"
 
 namespace okapi {
 class AsyncLinearMotionProfileController : public AsyncPositionController<std::string, double> {
@@ -250,16 +248,8 @@ class AsyncLinearMotionProfileController : public AsyncPositionController<std::s
   void forceRemovePath(const std::string &ipathId);
 
   protected:
-  using TrajectoryPtr = std::unique_ptr<TrajectoryCandidate, void (*)(TrajectoryCandidate *)>;
-  using SegmentPtr = std::unique_ptr<Segment, void (*)(void *)>;
-
-  struct TrajectoryPair {
-    SegmentPtr segment;
-    int length;
-  };
-
   std::shared_ptr<Logger> logger;
-  std::map<std::string, TrajectoryPair> paths{};
+  std::map<std::string, std::vector<squiggles::ProfilePoint>> paths{};
   PathfinderLimits limits;
   std::shared_ptr<ControllerOutput<double>> output;
   QLength diameter;
@@ -283,7 +273,8 @@ class AsyncLinearMotionProfileController : public AsyncPositionController<std::s
   /**
    * Follow the supplied path. Must follow the disabled lifecycle.
    */
-  virtual void executeSinglePath(const TrajectoryPair &path, std::unique_ptr<AbstractRate> rate);
+  virtual void executeSinglePath(const std::vector<squiggles::ProfilePoint> &path,
+                                 std::unique_ptr<AbstractRate> rate);
 
   /**
    * Converts linear "chassis" speed to rotational motor speed.
@@ -293,15 +284,8 @@ class AsyncLinearMotionProfileController : public AsyncPositionController<std::s
    */
   QAngularSpeed convertLinearToRotational(QSpeed linear) const;
 
-  std::string
-  getPathErrorMessage(const std::vector<Waypoint> &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);
+  std::string getPathErrorMessage(const std::vector<PathfinderPoint> &points,
+                                  const std::string &ipathId,
+                                  int length);
 };
 } // namespace okapi

include/okapi/api/control/async/asyncMotionProfileController.hpp

@@ -14,11 +14,10 @@
 #include "okapi/api/util/logging.hpp"
 #include "okapi/api/util/timeUtil.hpp"
 #include <atomic>
+#include <iostream>
 #include <map>
 
-extern "C" {
-#include "okapi/pathfinder/include/pathfinder.h"
-}
+#include "squiggles.hpp"
 
 namespace okapi {
 class AsyncMotionProfileController : public AsyncPositionController<std::string, PathfinderPoint> {
@@ -68,6 +67,10 @@ class AsyncMotionProfileController : public AsyncPositionController<std::string,
    * `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
    * are no waypoints, no path is generated.
    *
+   * NOTE: The waypoints are expected to be in the
+   * okapi::State::FRAME_TRANSFORMATION format where +x is forward, +y is right,
+   * and 0 theta is measured from the +x axis to the +y axis.
+   *
    * @param iwaypoints The waypoints to hit on the path.
    * @param ipathId A unique identifier to save the path with.
    * @param ilimits The limits to use for this path only.
@@ -235,17 +238,17 @@ class AsyncMotionProfileController : public AsyncPositionController<std::string,
   CrossplatformThread *getThread() const;
 
   /**
-   * Saves a generated path to files. Paths are stored as `<ipathId>.<left/right>.csv`. An SD card
+   * Saves a generated path to a file. Paths are stored as `<ipathId>.csv`. An SD card
    * must be inserted into the brain and the directory must exist. `idirectory` can be prefixed with
    * `/usd/`, but it this is not required.
    *
-   * @param idirectory The directory to store the path 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<std::string,
   void forceRemovePath(const std::string &ipathId);
 
   protected:
-  using TrajectoryPtr = std::unique_ptr<TrajectoryCandidate, void (*)(TrajectoryCandidate *)>;
-  using SegmentPtr = std::unique_ptr<Segment, void (*)(void *)>;
-
-  struct TrajectoryPair {
-    SegmentPtr left;
-    SegmentPtr right;
-    int length;
-  };
-
   std::shared_ptr<Logger> logger;
-  std::map<std::string, TrajectoryPair> paths{};
+  std::map<std::string, std::vector<squiggles::ProfilePoint>> paths{};
   PathfinderLimits limits;
   std::shared_ptr<ChassisModel> model;
   ChassisScales scales;
@@ -296,7 +290,8 @@ class AsyncMotionProfileController : public AsyncPositionController<std::string,
   /**
    * Follow the supplied path. Must follow the disabled lifecycle.
    */
-  virtual void executeSinglePath(const TrajectoryPair &path, std::unique_ptr<AbstractRate> rate);
+  virtual void executeSinglePath(const std::vector<squiggles::ProfilePoint> &path,
+                                 std::unique_ptr<AbstractRate> rate);
 
   /**
    * Converts linear chassis speed to rotational motor speed.
@@ -306,8 +301,9 @@ class AsyncMotionProfileController : public AsyncPositionController<std::string,
    */
   QAngularSpeed convertLinearToRotational(QSpeed linear) const;
 
-  std::string
-  getPathErrorMessage(const std::vector<Waypoint> &points, const std::string &ipathId, int length);
+  std::string getPathErrorMessage(const std::vector<PathfinderPoint> &points,
+                                  const std::string &ipathId,
+                                  int length);
 
   /**
    * Joins and escapes a directory and file name
@@ -319,15 +315,12 @@ class AsyncMotionProfileController : public AsyncPositionController<std::string,
    */
   static std::string makeFilePath(const std::string &directory, const std::string &filename);
 
-  void internalStorePath(FILE *leftPathFile, FILE *rightPathFile, const std::string &ipathId);
-  void internalLoadPath(FILE *leftPathFile, FILE *rightPathFile, const std::string &ipathId);
+  void internalStorePath(std::ostream &file, const std::string &ipathId);
+  void internalLoadPath(std::istream &file, const std::string &ipathId);
+  void internalLoadPathfinderPath(std::istream &leftFile,
+                                  std::istream &rightFile,
+                                  const std::string &ipathId);
 
-  /**
-   * 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);
+  static constexpr double DT = 0.01;
 };
 } // namespace okapi