⬆️3.2.0-beta.5

EZ-Template-Example-Project/include/EZ-Template/drive/drive.hpp

@@ -88,22 +88,22 @@ class Drive {
   /**
    * Left vertical tracking wheel.
    */
-  tracking_wheel* odom_left_tracker;
+  tracking_wheel* odom_tracker_left;
 
   /**
    * Right vertical tracking wheel.
    */
-  tracking_wheel* odom_right_tracker;
+  tracking_wheel* odom_tracker_right;
 
   /**
    * Front horizontal tracking wheel.
    */
-  tracking_wheel* odom_front_tracker;
+  tracking_wheel* odom_tracker_front;
 
   /**
    * Back horizontal tracking wheel.
    */
-  tracking_wheel* odom_back_tracker;
+  tracking_wheel* odom_tracker_back;
 
   /**
    * PID objects.
@@ -118,7 +118,9 @@ class Drive {
   PID forward_swingPID;
   PID backward_swingPID;
   PID xyPID;
-  PID aPID;
+  PID current_a_odomPID;
+  PID boomerangPID;
+  PID odom_angularPID;
   PID internal_leftPID;
   PID internal_rightPID;
 
@@ -2737,6 +2739,26 @@ class Drive {
    */
   int pid_turn_min_get();
 
+  /**
+   * @brief Set the heading pid constants object
+   *
+   * @param p           kP
+   * @param i           kI
+   * @param d           kD
+   * @param p_start_i   start_I
+   */
+  void pid_odom_angular_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);
+
+  /**
+   * @brief Set the heading pid constants object
+   *
+   * @param p           kP
+   * @param i           kI
+   * @param d           kD
+   * @param p_start_i   start_I
+   */
+  void pid_odom_boomerang_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);
+
   /**
    * Set's constants for odom driving exit conditions.
    *
@@ -3041,12 +3063,46 @@ class Drive {
   std::vector<const_and_name> pid_tuner_pids = {
       {"Drive Forward PID Constants", &forward_drivePID.constants},
       {"Drive Backward PID Constants", &backward_drivePID.constants},
+      {"Odom Angular PID Constants", &odom_angularPID.constants},
+      {"Boomerang Angular PID Constants", &boomerangPID.constants},
       {"Heading PID Constants", &headingPID.constants},
       {"Turn PID Constants", &turnPID.constants},
       {"Swing Forward PID Constants", &forward_swingPID.constants},
       {"Swing Backward PID Constants", &backward_swingPID.constants}};
 
+  /**
+   * Sets the max speed for user control
+   *
+   * \param int
+   *        the speed limit
+   */
+  void opcontrol_speed_max_set(int speed);
+
+  /**
+   * Returns the max speed for user control
+   */
+  int opcontrol_speed_max_get();
+
+  /**
+   * Toggles vector scaling for arcade control.  True enables, false disables.
+   *
+   * \param bool
+   *        true enables, false disables
+   */
+  void opcontrol_arcade_scaling(bool enable);
+
+  /**
+   * Returns if vector scaling for arcade control is enabled.  True enables, false disables.
+   */
+  bool opcontrol_arcade_scaling_enabled();
+
+  bool odom_use_left = true;
+  double odom_ime_track_width_left = 0.0;
+  double odom_ime_track_width_right = 0.0;
+
  private:
+  double opcontrol_speed_max = 127.0;
+  bool arcade_vector_scaling = false;
   // odom privates
   std::vector<odom> pp_movements;
   std::vector<int> injected_pp_index;
@@ -3075,7 +3131,7 @@ class Drive {
   bool odom_turn_bias_enabled();
   void odom_turn_bias_set(bool set);
   double angle_rad = 0.0;
-  double track_width = 0.0;
+  double global_track_width = 0.0;
   bool odometry_enabled = true;
   pose odom_target = {0, 0, 0};
   pose odom_current = {0, 0, 0};
@@ -3093,8 +3149,13 @@ class Drive {
   double max_boomerang_distance = 12.0;
   double odom_turn_bias_amount = 1.375;
   drive_directions current_drive_direction = fwd;
-  double h_last = 0, v_last = 0;
-  double last_theta = 0;
+  double h_last = 0.0, t_last = 0.0, l_last = 0.0, r_last = 0.0;
+  pose l_pose{0.0, 0.0, 0.0};
+  pose r_pose{0.0, 0.0, 0.0};
+  pose central_pose{0.0, 0.0, 0.0};
+  std::pair<float, float> decide_vert_sensor(ez::tracking_wheel* tracker, bool is_tracker_enabled, float ime = 0.0, float ime_track = 0.0);
+  pose solve_xy_vert(float p_track_width, float current_t, float delta_vert, float delta_t);
+  pose solve_xy_horiz(float p_track_width, float current_t, float delta_horiz, float delta_t);
   bool was_last_pp_mode_boomerang = false;
   bool global_forward_drive_slew_enabled = false;
   bool global_backward_drive_slew_enabled = false;
@@ -3110,10 +3171,10 @@ class Drive {
   std::vector<odom> set_odoms_direction(std::vector<odom> inputs);
   odom set_odom_direction(odom input);
   pose flip_pose(pose input);
-  bool odom_left_tracker_enabled = false;
-  bool odom_right_tracker_enabled = false;
-  bool odom_front_tracker_enabled = false;
-  bool odom_back_tracker_enabled = false;
+  bool odom_tracker_left_enabled = false;
+  bool odom_tracker_right_enabled = false;
+  bool odom_tracker_front_enabled = false;
+  bool odom_tracker_back_enabled = false;
 
   double chain_target_start = 0.0;
   double chain_sensor_start = 0.0;

EZ-Template-Example-Project/include/api.h

@@ -41,9 +41,9 @@
 
 #define PROS_VERSION_MAJOR 4
 #define PROS_VERSION_MINOR 1
+#define PROS_VERSION_PATCH 1
+#define PROS_VERSION_STRING "4.1.1"
 
-#define PROS_VERSION_PATCH 0
-#define PROS_VERSION_STRING "4.1.0"
 
 #include "pros/adi.h"
 #include "pros/colors.h"

EZ-Template-Example-Project/include/pros/optical.h

@@ -460,6 +460,39 @@ int32_t optical_enable_gesture(uint8_t port);
  */
 int32_t optical_disable_gesture(uint8_t port);
 
+/**
+ * Get integration time (update rate) of the optical sensor in milliseconds, with
+ * minimum time being 
+ *
+ * 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 Optical Sensor
+ *
+ * \param port
+ *        The V5 Optical Sensor port number from 1-21
+ * \return Integration time in milliseconds if the operation is successful 
+ * or PROS_ERR if the operation failed, setting errno.
+ */
+double optical_get_integration_time(uint8_t port);
+
+/**
+ * Set integration time (update rate) of the optical sensor in milliseconds.
+ *
+ * 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 Optical Sensor
+ *
+ * \param port
+ *        The V5 Optical Sensor port number from 1-21
+ * \param time
+ *        The desired integration time in milliseconds
+ * \return 1 if the operation is successful or PROS_ERR if the operation failed,
+ * setting errno.
+ */
+int32_t optical_set_integration_time(uint8_t port, double time);
+
 ///@}
 
 ///@}

EZ-Template-Example-Project/include/pros/optical.hpp

@@ -398,6 +398,36 @@ class Optical : public Device {
 	 */
 	virtual std::int32_t disable_gesture();
 
+	/**
+	 * Set integration time (update rate) of the optical sensor in milliseconds, with
+	 * minimum time being 3 ms and maximum time being 712 ms. Default is 100 ms, with the 
+	 * optical sensor communciating with the V5 brain every 20 ms. 
+	 *
+	 * 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 Optical Sensor
+ 	 *
+	 * \return 1 if the operation is successful or PROS_ERR_F if the operation failed,
+	 * setting errno.
+	 */
+	double get_integration_time();
+
+	/**
+	 * Get integration time (update rate) of the optical sensor in milliseconds.
+	 *
+	 * 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 Optical Sensor
+	 *
+	 * \param time
+	 *        The desired integration time in milliseconds
+	 * \return Integration time in milliseconds if the operation is successful 
+	 *  or PROS_ERR if the operation failed, setting errno.
+	 */
+	std::int32_t set_integration_time(double time);
+
 	/**
 	 * This is the overload for the << operator for printing to streams
 	 *