✨ Added set_relative_turn_pid() (#59)

* issue #59 - add set_relative_turn_pid function, no issue - fix various cases of wrong function documentation

* issue #59 - set_relative_turn_pid changed casing of variables to snake_case and made it relative to current target, not current heading

include/EZ-Template/PID.hpp

@@ -85,7 +85,7 @@ class PID {
   void set_exit_condition(int p_small_exit_time, double p_small_error, int p_big_exit_time = 0, double p_big_error = 0, int p_velocity_exit_time = 0, int p_mA_timeout = 0);
 
   /**
-   * Set's target.
+   * Sets target.
    *
    * \param target
    *        Target for PID.

include/EZ-Template/drive/drive.hpp

@@ -476,7 +476,7 @@ class Drive {
   void reset_gyro(double new_heading = 0);
 
   /**
-   * Resets the imu so that where the drive is pointing is zero in set_drive_pid(turn)
+   * Returns the current gyro value.
    */
   double get_gyro();
 
@@ -530,6 +530,16 @@ class Drive {
    */
   void set_turn_pid(double target, int speed);
 
+  /**
+  * Sets the robot to turn relative to current heading using PID.
+  * 
+  * \param target
+  *        target in degrees relative to current heading
+  * \param speed
+  *        0 to 127, max speed during motion
+  */
+  void set_relative_turn_pid(double target, int speed);
+
   /**
    * Turn using only the left or right side.
    *
@@ -548,7 +558,7 @@ class Drive {
   void reset_pid_targets();
 
   /**
-   * Resets all PID targets to 0.
+   * Sets heading of gyro and target of PID.
    */
   void set_angle(double angle);
 

src/EZ-Template/drive/set_pid.cpp

@@ -122,6 +122,22 @@ void Drive::set_turn_pid(double target, int speed) {
   set_mode(TURN);
 }
 
+void Drive::set_relative_turn_pid(double target, int speed) {
+  // Compute absolute target by adding to current heading
+  double absolute_target = turnPID.get_target() + target;
+
+  // Print targets
+  if (print_toggle) printf("Turn Started... Target Value: %f\n", absolute_target);
+
+  // Set PID targets
+  turnPID.set_target(absolute_target);
+  headingPID.set_target(absolute_target);
+  set_max_speed(speed);
+
+  // Run task
+  set_mode(TURN);
+}
+
 // Set swing PID
 void Drive::set_swing_pid(e_swing type, double target, int speed) {
   // Print targets