Motion chaining and quick exits are functional #109

include/EZ-Template/drive/drive.hpp

@@ -1323,6 +1323,17 @@ 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;
+
+
+  double chain_target_start = 0.0;
+  double chain_sensor_start = 0.0;
+  void pid_wait_quick();
+  void pid_wait_chain();
+
  private:  // !Auton
   double IMU_SCALER = 1.0;
   bool drive_toggle = true;

src/EZ-Template/drive/exit_conditions.cpp

@@ -268,4 +268,41 @@ void Drive::pid_wait_until(double target) {
   } else {
     printf("Not in a valid drive mode!\n");
   }
-}
+}
+
+// Pid wait, but quickly :)
+void Drive::pid_wait_quick() {
+  if (!(mode == DRIVE || mode == TURN || mode == SWING)) {
+    printf("Not in a valid drive mode!\n");
+    return;
+  }
+
+  // This is the target the user set, not the modified chained target
+  pid_wait_until(chain_target_start);
+}
+
+// Pid wait that hold momentum into the next motion
+void Drive::pid_wait_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);
+    leftPID.target_set(leftPID.target_get() + used_motion_chain_scale);
+    rightPID.target_set(leftPID.target_get() + used_motion_chain_scale);
+  }
+  // If turning, add turn_motion_chain_scale to target
+  else if (mode == TURN) {
+    used_motion_chain_scale = turn_motion_chain_scale * util::sgn(chain_target_start - chain_sensor_start);
+    turnPID.target_set(turnPID.target_get() + used_motion_chain_scale);
+  }
+  // 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);
+    swingPID.target_set(swingPID.target_get() + used_motion_chain_scale);
+  } else {
+    printf("Not in a valid drive mode!\n");
+    return;
+  }
+
+  // Exit at the real target
+  pid_wait_quick();
+}

src/EZ-Template/drive/set_pid.cpp

@@ -133,6 +133,9 @@ void Drive::pid_drive_set(double target, int speed, bool slew_on, bool toggle_he
   if (print_toggle) printf("Drive Started... Target Value: %f in", target);
   if (slew_on && print_toggle) printf(" with slew");
   if (print_toggle) printf("\n");
+  chain_target_start = target;
+  chain_sensor_start = drive_sensor_left();
+  used_motion_chain_scale = 0.0;
 
   // Global setup
   pid_speed_max_set(speed);
@@ -185,14 +188,17 @@ void Drive::pid_drive_set(okapi::QLength p_target, int speed, bool slew_on, bool
 void Drive::pid_turn_set(double target, int speed, bool slew_on) {
   // Print targets
   if (print_toggle) printf("Turn Started... Target Value: %f\n", target);
+  chain_sensor_start = drive_imu_get();
+  chain_target_start = target;
+  used_motion_chain_scale = 0.0;
 
   // Set PID targets
   turnPID.target_set(target);
   headingPID.target_set(target);  // Update heading target for next drive motion
   pid_speed_max_set(speed);
 
   // Initialize slew
-  slew_turn.initialize(slew_on, max_speed, target, drive_imu_get());
+  slew_turn.initialize(slew_on, max_speed, target, chain_sensor_start);
 
   // Run task
   drive_mode_set(TURN);
@@ -225,10 +231,13 @@ void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_s
   // Print targets
   if (print_toggle) printf("Swing Started... Target Value: %f\n", target);
   current_swing = type;
+  chain_sensor_start = drive_imu_get();
+  chain_target_start = target;
+  used_motion_chain_scale = 0.0;
 
   // Figure out if going forward or backward
   int side = type == ez::LEFT_SWING ? 1 : -1;
-  int direction = util::sgn((target - drive_imu_get()) * side);
+  int direction = util::sgn((target - chain_sensor_start) * side);
 
   // Set constants according to the robots direction
   PID::Constants pid_consts;
@@ -264,7 +273,7 @@ void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_s
 
   // Initialize slew
   double slew_tar = slew_swing_using_angle ? target : direction * 100;
-  double current = slew_swing_using_angle ? drive_imu_get() : (current_swing == LEFT_SWING ? drive_sensor_left() : drive_sensor_right());
+  double current = slew_swing_using_angle ? chain_sensor_start : (current_swing == LEFT_SWING ? drive_sensor_left() : drive_sensor_right());
   slew_swing.initialize(slew_on, max_speed, slew_tar, current);
 
   // Run task

src/main.cpp

@@ -117,7 +117,38 @@ void autonomous() {
   chassis.drive_sensor_reset(); // Reset drive sensors to 0
   chassis.drive_brake_set(MOTOR_BRAKE_HOLD); // Set motors to hold.  This helps autonomous consistency
 
-  ez::as::auton_selector.selected_auton_call(); // Calls selected auton from autonomous selector
+  chassis.pid_turn_set(45_deg, 110);
+  chassis.pid_wait_chain();
+
+  chassis.pid_turn_set(0_deg, 110);
+  chassis.pid_wait_chain();
+
+  chassis.pid_drive_set(-12_in, 110);
+  chassis.pid_wait_chain(); 
+
+  chassis.pid_drive_set(12_in, 110);
+  chassis.pid_wait_chain();
+
+  /*
+  chassis.pid_drive_set(12_in, 110);
+  chassis.pid_wait_quick();
+
+  //chassis.motion_chaining_enabled = false;
+  chassis.pid_turn_set(-45_deg, 110);
+  chassis.pid_wait_quick();
+
+  chassis.pid_turn_set(45_deg, 110);
+  chassis.pid_wait_quick();
+
+  chassis.pid_turn_set(0_deg, 110);
+  chassis.pid_wait_quick();
+
+  chassis.pid_drive_set(-12_in, 110);
+  chassis.pid_wait();
+  */
+
+
+  //ez::as::auton_selector.selected_auton_call(); // Calls selected auton from autonomous selector
 }