From 5a6807f6c1b3273d5e921d468f36af25bbbfe120 Mon Sep 17 00:00:00 2001 From: Jess Zarchi Date: Sun, 2 Jun 2024 14:01:04 -0700 Subject: [PATCH] Added reverse constants, okapi units, finalized function names and comments --- include/EZ-Template/drive/drive.hpp | 182 ++++++++++++++++++++-- include/autons.hpp | 1 + src/EZ-Template/drive/drive.cpp | 4 + src/EZ-Template/drive/exit_conditions.cpp | 41 ++++- src/EZ-Template/drive/set_pid.cpp | 5 +- src/autons.cpp | 32 +++- src/main.cpp | 34 +--- 7 files changed, 252 insertions(+), 47 deletions(-) diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index f414ddc0..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. * @@ -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; 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 6515af34..971f5de9 100644 --- a/src/EZ-Template/drive/exit_conditions.cpp +++ b/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"); diff --git a/src/EZ-Template/drive/set_pid.cpp b/src/EZ-Template/drive/set_pid.cpp index 5ba59129..c83c28fa 100644 --- a/src/EZ-Template/drive/set_pid.cpp +++ b/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 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 1a65c2e7..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), }); @@ -117,38 +118,7 @@ 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 - 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 + ez::as::auton_selector.selected_auton_call(); // Calls selected auton from autonomous selector }