diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index 7f4f51e0..82fe626d 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/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. * @@ -1324,7 +1488,18 @@ class Drive { double pid_tuner_increment_start_i_get(); private: // !Auton + double chain_target_start = 0.0; + double chain_sensor_start = 0.0; + 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; + double IMU_SCALER = 1.0; + bool drive_toggle = true; bool print_toggle = true; int swing_min = 0; diff --git a/include/autons.hpp b/include/autons.hpp index 6d96879f..654902d8 100644 --- a/include/autons.hpp +++ b/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(); diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp index f4e89b03..2725c80f 100644 --- a/src/EZ-Template/drive/drive.cpp +++ b/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); diff --git a/src/EZ-Template/drive/exit_conditions.cpp b/src/EZ-Template/drive/exit_conditions.cpp index 96116bdf..2306feb0 100644 --- a/src/EZ-Template/drive/exit_conditions.cpp +++ b/src/EZ-Template/drive/exit_conditions.cpp @@ -270,4 +270,76 @@ void Drive::pid_wait_until(double target) { } else { printf("Not in a valid drive mode!\n"); } -} \ No newline at end of file +} + +// 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); +} + +// 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_quick_chain() { + // If driving, add drive_motion_chain_scale to target + if (mode == DRIVE) { + 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); + } + // 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) { + 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"); + return; + } + + // Exit at the real target + pid_wait_quick(); +} diff --git a/src/EZ-Template/drive/set_pid.cpp b/src/EZ-Template/drive/set_pid.cpp index 78625c20..1dfdffdb 100644 --- a/src/EZ-Template/drive/set_pid.cpp +++ b/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", 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); @@ -153,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); @@ -185,6 +189,9 @@ 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); @@ -192,7 +199,7 @@ void Drive::pid_turn_set(double target, int speed, bool slew_on) { 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 +232,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; @@ -240,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 @@ -264,7 +276,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 diff --git a/src/autons.cpp b/src/autons.cpp index 65715767..a703af7c 100644 --- a/src/autons.cpp +++ b/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(); diff --git a/src/main.cpp b/src/main.cpp index 37b2c70e..f44d1837 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -62,6 +62,7 @@ void initialize() { Auton("Drive and Turn\n\nDrive forward, turn, come back. ", drive_and_turn), Auton("Drive and Turn\n\nSlow down during drive.", wait_until_change_speed), Auton("Swing Example\n\nSwing in an 'S' curve", swing_example), + Auton("Motion Chaining\n\nDrive forward, turn, and come back, but blend everything together :D", motion_chaining), Auton("Combine all 3 movements", combining_movements), Auton("Interference\n\nAfter driving forward, robot performs differently if interfered or not.", interfered_example), });