Added reverse constants, okapi units, finalized function names and co…

…mments

include/EZ-Template/drive/drive.hpp

@@ -890,6 +890,19 @@ class Drive {
    */
   void pid_wait_until(double target);
 
+  /**
+   * Lock the code in a while loop until the robot has settled.
+   * Wrapper for pid_wait_until(target), target is your previously input target
+   */
+  void pid_wait_quick();
+
+  /**
+   * Lock the code in a while loop until the robot has settled.
+   * This also adds distance to target, and then exits with pid_wait_quick
+   * This will exit the motion while carrying momentum into the next motion.
+   */
+  void pid_wait_quick_chain();
+
   /**
    * Autonomous interference detection.  Returns true when interfered, and false when nothing happened.
    */
@@ -936,6 +949,29 @@ class Drive {
    */
   PID::Constants pid_turn_constants_get();
 
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This sets turning constants.
+   *
+   * \param input
+   *        okapi angle unit
+   */
+  void pid_turn_chain_constant_set(okapi::QAngle input);
+
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This sets turning constants.
+   *
+   * \param input
+   *        angle in degrees
+   */
+  void pid_turn_chain_constant_set(double input);
+
+  /**
+   * Returns the amount that the PID will overshoot target by to maintain momentum into the next motion for turning.
+   */
+  double pid_turn_chain_constant_get();
+
   /**
    * @brief Set the swing pid constants object
    *
@@ -996,6 +1032,70 @@ class Drive {
    */
   PID::Constants pid_swing_constants_backward_get();
 
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This sets forward and backwards swing constants.
+   *
+   * \param input
+   *        okapi angle unit
+   */
+  void pid_swing_chain_constant_set(okapi::QAngle input);
+
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This only sets forward swing constants.
+   *
+   * \param input
+   *        okapi angle unit
+   */
+  void pid_swing_chain_forward_constant_set(okapi::QAngle input);
+
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This only sets backward swing constants.
+   *
+   * \param input
+   *        okapi angle unit
+   */
+  void pid_swing_chain_backward_constant_set(okapi::QAngle input);
+
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This sets forward and backwards swing constants.
+   *
+   * \param input
+   *        angle in degrees
+   */
+  void pid_swing_chain_constant_set(double input);
+
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This only sets forward constants.
+   *
+   * \param input
+   *        angle in degrees
+   */
+  void pid_swing_chain_forward_constant_set(double input);
+
+  /**
+   * Returns the amount that the PID will overshoot target by to maintain momentum into the next motion for swinging forward.
+   */
+  double pid_swing_chain_forward_constant_get();
+
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This only sets backwards swing constants.
+   *
+   * \param input
+   *        angle in degrees
+   */
+  void pid_swing_chain_backward_constant_set(double input);
+
+  /**
+   * Returns the amount that the PID will overshoot target by to maintain momentum into the next motion for swinging backward.
+   */
+  double pid_swing_chain_backward_constant_get();
+
   /**
    * @brief Set the heading pid constants object
    *
@@ -1076,6 +1176,70 @@ class Drive {
    */
   PID::Constants pid_drive_constants_backward_get();
 
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This sets forward and backwards driving constants.
+   *
+   * \param input
+   *        okapi length unit
+   */
+  void pid_drive_chain_constant_set(okapi::QLength input);
+
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This only sets forward driving constants.
+   *
+   * \param input
+   *        okapi length unit
+   */
+  void pid_drive_chain_forward_constant_set(okapi::QLength input);
+
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This only sets backward driving constants.
+   *
+   * \param input
+   *        okapi length unit
+   */
+  void pid_drive_chain_backward_constant_set(okapi::QLength input);
+
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This sets forward and backwards driving constants.
+   *
+   * \param input
+   *        distance in inches
+   */
+  void pid_drive_chain_constant_set(double input);
+
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This only sets forward driving constants.
+   *
+   * \param input
+   *        distance in inches
+   */
+  void pid_drive_chain_forward_constant_set(double input);
+
+  /**
+   * Returns the amount that the PID will overshoot target by to maintain momentum into the next motion for driving forward.
+   */
+  double pid_drive_chain_forward_constant_get();
+
+  /**
+   * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion.
+   * This only sets backward driving constants.
+   *
+   * \param input
+   *        distance in inches
+   */
+  void pid_drive_chain_backward_constant_set(double input);
+
+  /**
+   * Returns the amount that the PID will overshoot target by to maintain momentum into the next motion for driving backward.
+   */
+  double pid_drive_chain_backward_constant_get();
+
   /**
    * Sets minimum power for swings when kI and startI are enabled.
    *
@@ -1323,19 +1487,19 @@ class Drive {
    */
   double pid_tuner_increment_start_i_get();
 
-  double drive_motion_chain_scale = 3.0;
-  double swing_motion_chain_scale = 3.0;
-  double turn_motion_chain_scale = 3.0;
-  double used_motion_chain_scale = 0.0;
-
-
+ private:  // !Auton
   double chain_target_start = 0.0;
   double chain_sensor_start = 0.0;
-  void pid_wait_quick();
-  void pid_wait_chain();
+  double drive_forward_motion_chain_scale = 0.0;
+  double drive_backward_motion_chain_scale = 0.0;
+  double swing_forward_motion_chain_scale = 0.0;
+  double swing_backward_motion_chain_scale = 0.0;
+  double turn_motion_chain_scale = 0.0;
+  double used_motion_chain_scale = 0.0;
+  bool motion_chain_backward = false;
 
- private:  // !Auton
   double IMU_SCALER = 1.0;
+
   bool drive_toggle = true;
   bool print_toggle = true;
   int swing_min = 0;

include/autons.hpp

@@ -9,6 +9,7 @@ void turn_example();
 void drive_and_turn();
 void wait_until_change_speed();
 void swing_example();
+void motion_chaining();
 void combining_movements();
 void interfered_example();
 

src/EZ-Template/drive/drive.cpp

@@ -165,6 +165,10 @@ void Drive::drive_defaults_set() {
   pid_swing_exit_condition_set(300_ms, 3_deg, 500_ms, 7_deg, 750_ms, 750_ms);
   pid_drive_exit_condition_set(300_ms, 1_in, 500_ms, 3_in, 750_ms, 750_ms);
 
+  pid_turn_chain_constant_set(3_deg);
+  pid_swing_chain_constant_set(5_deg);
+  pid_drive_chain_constant_set(3_in);
+
   // Modify joystick curve on controller (defaults to disabled)
   opcontrol_curve_buttons_toggle(true);
 

src/EZ-Template/drive/exit_conditions.cpp

@@ -281,11 +281,45 @@ void Drive::pid_wait_quick() {
   pid_wait_until(chain_target_start);
 }
 
+// Set drive motion chain constants
+void Drive::pid_drive_chain_constant_set(double input) {
+  pid_drive_chain_forward_constant_set(input);
+  pid_drive_chain_backward_constant_set(input);
+}
+void Drive::pid_drive_chain_forward_constant_set(double input) { drive_forward_motion_chain_scale = fabs(input); }
+void Drive::pid_drive_chain_backward_constant_set(double input) { drive_backward_motion_chain_scale = fabs(input); }
+void Drive::pid_drive_chain_constant_set(okapi::QLength input) { pid_drive_chain_constant_set(input.convert(okapi::inch)); }
+void Drive::pid_drive_chain_forward_constant_set(okapi::QLength input) { pid_drive_chain_forward_constant_set(input.convert(okapi::inch)); }
+void Drive::pid_drive_chain_backward_constant_set(okapi::QLength input) { pid_drive_chain_backward_constant_set(input.convert(okapi::inch)); }
+
+// Set turn motion chain constants
+void Drive::pid_turn_chain_constant_set(double input) { turn_motion_chain_scale = fabs(input); }
+void Drive::pid_turn_chain_constant_set(okapi::QAngle input) { pid_turn_chain_constant_set(input.convert(okapi::degree)); }
+
+// Set swing motion chain constants
+void Drive::pid_swing_chain_constant_set(double input) {
+  pid_swing_chain_forward_constant_set(input);
+  pid_swing_chain_backward_constant_set(input);
+}
+void Drive::pid_swing_chain_forward_constant_set(double input) { swing_forward_motion_chain_scale = fabs(input); }
+void Drive::pid_swing_chain_backward_constant_set(double input) { swing_backward_motion_chain_scale = fabs(input); }
+void Drive::pid_swing_chain_constant_set(okapi::QAngle input) { pid_swing_chain_constant_set(input.convert(okapi::degree)); }
+void Drive::pid_swing_chain_forward_constant_set(okapi::QAngle input) { pid_swing_chain_forward_constant_set(input.convert(okapi::degree)); }
+void Drive::pid_swing_chain_backward_constant_set(okapi::QAngle input) { pid_swing_chain_backward_constant_set(input.convert(okapi::degree)); }
+
+// Get motion chain constants
+double Drive::pid_drive_chain_forward_constant_get() { return drive_forward_motion_chain_scale; }
+double Drive::pid_drive_chain_backward_constant_get() { return drive_backward_motion_chain_scale; }
+double Drive::pid_turn_chain_constant_get() { return turn_motion_chain_scale; }
+double Drive::pid_swing_chain_forward_constant_get() { return swing_forward_motion_chain_scale; }
+double Drive::pid_swing_chain_backward_constant_get() { return swing_backward_motion_chain_scale; }
+
 // Pid wait that hold momentum into the next motion
-void Drive::pid_wait_chain() {
+void Drive::pid_wait_quick_chain() {
   // If driving, add drive_motion_chain_scale to target
   if (mode == DRIVE) {
-    used_motion_chain_scale = drive_motion_chain_scale * util::sgn(chain_target_start);
+    double chain_scale = motion_chain_backward ? drive_backward_motion_chain_scale : drive_forward_motion_chain_scale;
+    used_motion_chain_scale = chain_scale * util::sgn(chain_target_start);
     leftPID.target_set(leftPID.target_get() + used_motion_chain_scale);
     rightPID.target_set(leftPID.target_get() + used_motion_chain_scale);
   }
@@ -296,7 +330,8 @@ void Drive::pid_wait_chain() {
   }
   // If swinging, add swing_motion_chain_scale to target
   else if (mode == SWING) {
-    used_motion_chain_scale = swing_motion_chain_scale * util::sgn(chain_target_start - chain_sensor_start);
+    double chain_scale = motion_chain_backward ? swing_backward_motion_chain_scale : swing_forward_motion_chain_scale;
+    used_motion_chain_scale = chain_scale * util::sgn(chain_target_start - chain_sensor_start);
     swingPID.target_set(swingPID.target_get() + used_motion_chain_scale);
   } else {
     printf("Not in a valid drive mode!\n");

src/EZ-Template/drive/set_pid.cpp

@@ -156,10 +156,11 @@ void Drive::pid_drive_set(double target, int speed, bool slew_on, bool toggle_he
   if (l_target_encoder < l_start && r_target_encoder < r_start) {
     pid_consts = backward_drivePID.constants_get();
     slew_consts = slew_backward.constants_get();
-
+    motion_chain_backward = true;
   } else {
     pid_consts = forward_drivePID.constants_get();
     slew_consts = slew_forward.constants_get();
+    motion_chain_backward=false;
   }
   leftPID.constants_set(pid_consts.kp, pid_consts.ki, pid_consts.kd, pid_consts.start_i);
   rightPID.constants_set(pid_consts.kp, pid_consts.ki, pid_consts.kd, pid_consts.start_i);
@@ -249,12 +250,14 @@ void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_s
     pid_swing_consts = backward_swingPID.constants_get();
     slew_consts = slew_swing_backward.constants_get();
     slew_swing_using_angle = slew_swing_rev_using_angle;
+    motion_chain_backward = true;
 
   } else {
     pid_consts = forward_drivePID.constants_get();
     pid_swing_consts = forward_swingPID.constants_get();
     slew_consts = slew_swing_forward.constants_get();
     slew_swing_using_angle = slew_swing_fwd_using_angle;
+    motion_chain_backward = false;
   }
 
   // Set targets for the side that isn't moving

src/autons.cpp

@@ -23,6 +23,10 @@ void default_constants() {
   chassis.pid_swing_exit_condition_set(300_ms, 3_deg, 500_ms, 7_deg, 750_ms, 750_ms);
   chassis.pid_drive_exit_condition_set(300_ms, 1_in, 500_ms, 3_in, 750_ms, 750_ms);
 
+  chassis.pid_turn_chain_constant_set(3_deg);
+  chassis.pid_swing_chain_constant_set(5_deg);
+  chassis.pid_drive_chain_constant_set(3_in);
+
   chassis.slew_drive_constants_set(7_in, 80);
 }
 
@@ -133,18 +137,42 @@ void swing_example() {
   chassis.pid_wait();
 }
 
+///
+// Combining Turn + Drive
+///
+void motion_chaining() {
+  // Motion chaining is where motions all try to blend together instead of individual movements.
+  // This works by exiting while the robot is still moving a little bit. 
+  // To use this, replace pid_wait with pid_wait_quick_chain.
+  chassis.pid_drive_set(24_in, DRIVE_SPEED, true);
+  chassis.pid_wait_quick_chain();
+
+  chassis.pid_turn_set(45_deg, TURN_SPEED);
+  chassis.pid_wait_quick_chain();
+
+  chassis.pid_turn_set(-45_deg, TURN_SPEED);
+  chassis.pid_wait_quick_chain();
+
+  chassis.pid_turn_set(0_deg, TURN_SPEED);
+  chassis.pid_wait_quick_chain();
+
+  // Your final motion should still be a normal pid_wait
+  chassis.pid_drive_set(-24_in, DRIVE_SPEED, true);
+  chassis.pid_wait();
+}
+
 ///
 // Auto that tests everything
 ///
 void combining_movements() {
   chassis.pid_drive_set(24_in, DRIVE_SPEED, true);
-  chassis.pid_wait();
+  chassis.pid_wait_quick_chain();
 
   chassis.pid_turn_set(45_deg, TURN_SPEED);
   chassis.pid_wait();
 
   chassis.pid_swing_set(ez::RIGHT_SWING, -45_deg, SWING_SPEED, 45);
-  chassis.pid_wait();
+  chassis.pid_wait_quick_chain();
 
   chassis.pid_turn_set(0_deg, TURN_SPEED);
   chassis.pid_wait();