Cleaned relative turns, added wide swings #59 #83

include/EZ-Template/drive/drive.hpp

@@ -594,6 +594,16 @@ class Drive {
    */
   void pid_drive_set(okapi::QLength p_target, int speed, bool slew_on = false, bool toggle_heading = true);
 
+  /**
+   * Sets the robot to turn using PID.
+   *
+   * \param p_target
+   *        target value as a double
+   * \param speed
+   *        0 to 127, max speed during motion
+   */
+  void pid_turn_raw_set(double target, int speed);
+
   /**
    * Sets the robot to turn using PID.
    *
@@ -614,6 +624,20 @@ class Drive {
    */
   void pid_turn_relative_set(okapi::QAngle p_target, int speed);
 
+  /**
+   * Turn using only the left or right side.
+   *
+   * \param type
+   *        L_SWING or R_SWING
+   * \param p_target
+   *        target value as a double
+   * \param speed
+   *        0 to 127, max speed during motion
+   * \param opposite_speed
+   *        0 to 127, max speed of the opposite side of the drive during the swing.  This is used for arcs, and is defaulted to 0.
+   */
+  void pid_swing_raw_set(e_swing type, double target, int speed, int opposite_speed);
+
   /**
    * Turn using only the left or right side.
    *
@@ -623,8 +647,10 @@ class Drive {
    *        target value in degrees
    * \param speed
    *        0 to 127, max speed during motion
+   * \param opposite_speed
+   *        0 to 127, max speed of the opposite side of the drive during the swing.  This is used for arcs, and is defaulted to 0.
    */
-  void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed);
+  void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed = 0);
 
   /**
    * Sets the robot to turn only using the left or right side relative to current heading using PID.
@@ -635,19 +661,26 @@ class Drive {
    *        target value in okapi angle units
    * \param speed
    *        0 to 127, max speed during motion
+   * \param opposite_speed
+   *        0 to 127, max speed of the opposite side of the drive during the swing.  This is used for arcs, and is defaulted to 0.
    */
-  void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed);
+  void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed = 0);
 
   /**
    * Resets all PID targets to 0.
    */
   void pid_targets_reset();
 
   /**
-   * Sets heading of gyro and target of PID.
+   * Sets heading of gyro and target of PID, okapi angle.
    */
   void drive_angle_set(okapi::QAngle p_angle);
 
+  /**
+   * Sets heading of gyro and target of PID, takes double as an angle.
+   */
+  void drive_angle_raw_set(double angle);
+
   /**
    * Lock the code in a while loop until the robot has settled.
    */
@@ -970,6 +1003,7 @@ class Drive {
   int swing_min = 0;
   int turn_min = 0;
   bool practice_mode_is_on = false;
+  int swing_opposite_speed = 0;
 
   /**
    * Private wait until for drive

src/EZ-Template/drive/pid_tasks.cpp

@@ -95,6 +95,8 @@ void Drive::turn_pid_task() {
 void Drive::swing_pid_task() {
   // Compute PID
   swingPID.compute(drive_imu_get());
+  leftPID.compute(drive_sensor_left());
+  rightPID.compute(drive_sensor_right());
 
   // Clip swingPID to max speed
   double swing_out = util::clamp(swingPID.output, max_speed, -max_speed);
@@ -105,11 +107,17 @@ void Drive::swing_pid_task() {
       swing_out = util::clamp(swing_out, pid_swing_min_get(), -pid_swing_min_get());
   }
 
+  // Set the motors powers, and decide what to do with the "still" side of the drive
+  double opposite_output = 0;
+  double scale = swing_out / max_speed;
   if (drive_toggle) {
     // Check if left or right swing, then set motors accordingly
-    if (current_swing == LEFT_SWING)
-      private_drive_set(swing_out, 0);
-    else if (current_swing == RIGHT_SWING)
-      private_drive_set(0, -swing_out);
+    if (current_swing == LEFT_SWING) {
+      opposite_output = swing_opposite_speed == 0 ? rightPID.output : swing_opposite_speed * scale;
+      private_drive_set(swing_out, opposite_output);
+    } else if (current_swing == RIGHT_SWING) {
+      opposite_output = swing_opposite_speed == 0 ? leftPID.output : swing_opposite_speed * scale;
+      private_drive_set(-opposite_output, -swing_out);
+    }
   }
-}
+}

src/EZ-Template/drive/set_pid.cpp

@@ -5,6 +5,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */
 
 #include "main.h"
+#include "okapi/api/units/QAngle.hpp"
 
 // Set PID constants
 void Drive::pid_drive_constants_set(double p, double i, double d, double p_start_i) {
@@ -64,7 +65,7 @@ PID::Constants Drive::pid_heading_constants_get() {
 
 // Updates max speed
 void Drive::pid_speed_max_set(int speed) {
-  max_speed = util::clamp(abs(speed), 127, -127);
+  max_speed = abs(util::clamp(speed, 127, -127));
 }
 
 int Drive::pid_speed_max_get() {
@@ -80,13 +81,16 @@ void Drive::pid_targets_reset() {
   turnPID.target_set(0);
 }
 
-void Drive::drive_angle_set(okapi::QAngle p_angle) {
-  double angle = p_angle.convert(okapi::degree);  // Convert okapi unit to degree
-
+void Drive::drive_angle_raw_set(double angle) {
   headingPID.target_set(angle);
   drive_imu_reset(angle);
 }
 
+void Drive::drive_angle_set(okapi::QAngle p_angle) {
+  double angle = p_angle.convert(okapi::degree);  // Convert okapi unit to degree
+  drive_angle_raw_set(angle);
+}
+
 void Drive::drive_mode_set(e_mode p_mode) { mode = p_mode; }
 e_mode Drive::drive_mode_get() { return mode; }
 
@@ -143,10 +147,8 @@ void Drive::pid_drive_set(okapi::QLength p_target, int speed, bool slew_on, bool
   drive_mode_set(DRIVE);
 }
 
-// Set turn PID
-void Drive::pid_turn_set(okapi::QAngle p_target, int speed) {
-  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
-
+// Raw Set Turn PID
+void Drive::pid_turn_raw_set(double target, int speed) {
   // Print targets
   if (print_toggle) printf("Turn Started... Target Value: %f\n", target);
 
@@ -159,55 +161,56 @@ void Drive::pid_turn_set(okapi::QAngle p_target, int speed) {
   drive_mode_set(TURN);
 }
 
+// Set turn PID
+void Drive::pid_turn_set(okapi::QAngle p_target, int speed) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_turn_raw_set(target, speed);
+}
+
 void Drive::pid_turn_relative_set(okapi::QAngle p_target, int speed) {
   double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
   // Compute absolute target by adding to current heading
   double absolute_target = turnPID.target_get() + target;
+  pid_turn_raw_set(absolute_target, speed);
 
-  // Print targets
-  if (print_toggle) printf("Turn Started... Target Value: %f\n", absolute_target);
-
-  // Set PID targets
-  turnPID.target_set(absolute_target);
-  headingPID.target_set(absolute_target);
-  pid_speed_max_set(speed);
-
-  // Run task
-  drive_mode_set(TURN);
+  if (print_toggle) printf("Relative ");
 }
 
-// Set swing PID
-void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed) {
-  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+// Raw Set Swing PID
+void Drive::pid_swing_raw_set(e_swing type, double target, int speed, int opposite_speed) {
   // Print targets
   if (print_toggle) printf("Swing Started... Target Value: %f\n", target);
   current_swing = type;
 
+  // Set targets for the side that isn't moving
+  auto consts = forward_drivePID.constants_get();
+  leftPID.constants_set(consts.kp, consts.ki, consts.kd, consts.start_i);
+  rightPID.constants_set(consts.kp, consts.ki, consts.kd, consts.start_i);
+  leftPID.target_set(drive_sensor_left());
+  rightPID.target_set(drive_sensor_right());
+
   // Set PID targets
   swingPID.target_set(target);
   headingPID.target_set(target);  // Update heading target for next drive motion
   pid_speed_max_set(speed);
+  swing_opposite_speed = opposite_speed;
 
   // Run task
   drive_mode_set(SWING);
 }
 
 // Set swing PID
-void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed) {
+void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed) {
   double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_raw_set(type, target, speed, opposite_speed);
+}
 
+// Set swing PID
+void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
   // Compute absolute target by adding to current heading
   double absolute_target = swingPID.target_get() + target;
+  pid_swing_raw_set(type, absolute_target, speed, opposite_speed);
 
-  // Print targets
-  if (print_toggle) printf("Swing Started... Target Value: %f\n", absolute_target);
-  current_swing = type;
-
-  // Set PID targets
-  swingPID.target_set(absolute_target);
-  headingPID.target_set(absolute_target);  // Update heading target for next drive motion
-  pid_speed_max_set(speed);
-
-  // Run task
-  drive_mode_set(SWING);
+  if (print_toggle) printf("Relative ");
 }

src/main.cpp

@@ -14,7 +14,7 @@ Drive chassis (
   // IMU Port
   ,6
 
-  // Wheel Diameter (Remember, 4" wheels are actually 4.125!)
+  // Wheel Diameter (Remember, old 4" wheels are actually 4.125!)
   //    (or tracking wheel diameter)
   ,3.25