diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index f6154f05..7e01e4dd 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -92,6 +92,8 @@ class Drive { PID rightPID; PID backward_drivePID; PID swingPID; + PID forward_swingPID; + PID backward_swingPID; /** * Slew objects. @@ -135,7 +137,7 @@ class Drive { */ void slew_swing_backward_constants_set(okapi::QLength distance, int min_speed); - /** + /** * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance @@ -694,7 +696,7 @@ class Drive { ///// /** - * Sets the robot to move forward using PID. + * Sets the robot to move forward using PID with okapi units. * * \param target * target value in inches @@ -708,20 +710,34 @@ 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. + * Sets the robot to move forward using PID without okapi units. * - * \param p_target - * target value as a double + * \param target + * target value as a double, unit is inches * \param speed * 0 to 127, max speed during motion * \param slew_on * ramp up from a lower speed to your target speed + * \param toggle_heading + * toggle for heading correction */ - void pid_turn_raw_set(double target, int speed, bool slew_on = false); + void pid_drive_set(double target, int speed, bool slew_on, bool toggle_heading = true); /** * Sets the robot to turn using PID. * + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(double target, int speed, bool slew_on = false); + + /** + * Sets the robot to turn using PID with okapi units. + * * \param p_target * target value in degrees * \param speed @@ -732,7 +748,7 @@ class Drive { void pid_turn_set(okapi::QAngle p_target, int speed, bool slew_on = false); /** - * Sets the robot to turn relative to current heading using PID. + * Sets the robot to turn relative to current heading using PID with okapi units. * * \param p_target * target value in okapi angle units @@ -744,21 +760,33 @@ class Drive { void pid_turn_relative_set(okapi::QAngle p_target, int speed, bool slew_on = false); /** - * Turn using only the left or right side. + * Sets the robot to turn relative to current heading using PID without okapi units. + * + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_relative_set(double target, int speed, bool slew_on = false); + + /** + * Turn using only the left or right side without okapi units. * * \param type * L_SWING or R_SWING * \param p_target - * target value as a double + * target value as a double, unit is 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_raw_set(e_swing type, double target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed = 0, bool slew_on = false); /** - * Turn using only the left or right side. + * Turn using only the left or right side with okapi units. * * \param type * L_SWING or R_SWING @@ -772,7 +800,7 @@ class Drive { void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed = 0, bool slew_on = false); /** - * Sets the robot to turn only using the left or right side relative to current heading using PID. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units. * * \param type * L_SWING or R_SWING @@ -785,6 +813,20 @@ class Drive { */ void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed = 0, bool slew_on = false); + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value as a double, unit is 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_relative_set(e_swing type, double target, int speed, int opposite_speed = 0, bool slew_on = false); + /** * Resets all PID targets to 0. */ @@ -798,7 +840,7 @@ class Drive { /** * Sets heading of gyro and target of PID, takes double as an angle. */ - void drive_angle_raw_set(double angle); + void drive_angle_set(double angle); /** * Lock the code in a while loop until the robot has settled. @@ -821,6 +863,14 @@ class Drive { */ void pid_wait_until(okapi::QLength target); + /** + * Lock the code in a while loop until this position has passed for driving without okapi units. + * + * \param target + * for driving or turning, using a double. degrees for turns/swings, inches for driving. + */ + void pid_wait_until(double target); + /** * Autonomous interference detection. Returns true when interfered, and false when nothing happened. */ @@ -848,34 +898,54 @@ class Drive { int pid_speed_max_get(); /** - * @brief Set the drive pid constants object + * @brief Set the turn pid constants object * * @param p kP * @param i kI * @param d kD * @param p_start_i start_I */ - void pid_drive_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); + void pid_turn_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. Returns -1 if fwd and rev constants aren't the same! + * @brief returns PID constants with PID::Constants. * * @param p kP * @param i kI * @param d kD * @param p_start_i start_I */ - PID::Constants pid_drive_constants_get(); + PID::Constants pid_turn_constants_get(); /** - * @brief Set the turn pid constants object + * @brief Set the swing pid constants object * * @param p kP * @param i kI * @param d kD * @param p_start_i start_I */ - void pid_turn_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); + void pid_swing_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); + + /** + * @brief returns PID constants with PID::Constants. Returns -1 if fwd and rev constants aren't the same! + * + * @param p kP + * @param i kI + * @param d kD + * @param p_start_i start_I + */ + PID::Constants pid_swing_constants_get(); + + /** + * @brief Set the forward swing pid constants object + * + * @param p kP + * @param i kI + * @param d kD + * @param p_start_i start_I + */ + void pid_swing_constants_forward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** * @brief returns PID constants with PID::Constants. @@ -885,17 +955,17 @@ class Drive { * @param d kD * @param p_start_i start_I */ - PID::Constants pid_turn_constants_get(); + PID::Constants pid_swing_constants_forward_get(); /** - * @brief Set the swing pid constants object + * @brief Set the backward swing pid constants object * * @param p kP * @param i kI * @param d kD * @param p_start_i start_I */ - void pid_swing_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); + void pid_swing_constants_backward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** * @brief returns PID constants with PID::Constants. @@ -905,7 +975,7 @@ class Drive { * @param d kD * @param p_start_i start_I */ - PID::Constants pid_swing_constants_get(); + PID::Constants pid_swing_constants_backward_get(); /** * @brief Set the heading pid constants object @@ -927,6 +997,26 @@ class Drive { */ PID::Constants pid_heading_constants_get(); + /** + * @brief Set the drive pid constants object + * + * @param p kP + * @param i kI + * @param d kD + * @param p_start_i start_I + */ + void pid_drive_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); + + /** + * @brief returns PID constants with PID::Constants. Returns -1 if fwd and rev constants aren't the same! + * + * @param p kP + * @param i kI + * @param d kD + * @param p_start_i start_I + */ + PID::Constants pid_drive_constants_get(); + /** * @brief Set the forward pid constants object * @@ -935,7 +1025,7 @@ class Drive { * @param d kD * @param p_start_i start_I */ - void pid_drive_forward_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); + void pid_drive_constants_forward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** * @brief returns PID constants with PID::Constants. @@ -945,7 +1035,7 @@ class Drive { * @param d kD * @param p_start_i start_I */ - PID::Constants pid_drive_forward_constants_get(); + PID::Constants pid_drive_constants_forward_get(); /** * @brief Set the backwards pid constants object @@ -955,7 +1045,7 @@ class Drive { * @param d kD * @param p_start_i start_I */ - void pid_drive_backward_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); + void pid_drive_constants_backward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** * @brief returns PID constants with PID::Constants. @@ -965,7 +1055,7 @@ class Drive { * @param d kD * @param p_start_i start_I */ - PID::Constants pid_drive_backward_constants_get(); + PID::Constants pid_drive_constants_backward_get(); /** * Sets minimum power for swings when kI and startI are enabled. @@ -1041,6 +1131,54 @@ class Drive { */ void pid_swing_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QAngle p_small_error, okapi::QTime p_big_exit_time, okapi::QAngle p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout); + /** + * Set's constants for drive exit conditions. + * + * \param p_small_exit_time + * Sets small_exit_time. Timer for to exit within smalL_error. + * \param p_small_error + * Sets smalL_error. Timer will start when error is within this. + * \param p_big_exit_time + * Sets big_exit_time. Timer for to exit within big_error. + * \param p_big_error + * Sets big_error. Timer will start when error is within this. + * \param p_velocity_exit_time + * Sets velocity_exit_time. Timer will start when velocity is 0. + */ + void pid_drive_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout); + + /** + * Set's constants for turn exit conditions. + * + * \param p_small_exit_time + * Sets small_exit_time. Timer for to exit within smalL_error. + * \param p_small_error + * Sets smalL_error. Timer will start when error is within this. + * \param p_big_exit_time + * Sets big_exit_time. Timer for to exit within big_error. + * \param p_big_error + * Sets big_error. Timer will start when error is within this. + * \param p_velocity_exit_time + * Sets velocity_exit_time. Timer will start when velocity is 0. + */ + void pid_turn_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout); + + /** + * Set's constants for swing exit conditions. + * + * \param p_small_exit_time + * Sets small_exit_time. Timer for to exit within smalL_error. + * \param p_small_error + * Sets smalL_error. Timer will start when error is within this. + * \param p_big_exit_time + * Sets big_exit_time. Timer for to exit within big_error. + * \param p_big_error + * Sets big_error. Timer will start when error is within this. + * \param p_velocity_exit_time + * Sets velocity_exit_time. Timer will start when velocity is 0. + */ + void pid_swing_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout); + /** * Returns current tick_per_inch() */ diff --git a/src/EZ-Template/drive/exit_conditions.cpp b/src/EZ-Template/drive/exit_conditions.cpp index 68764303..6297b7ab 100644 --- a/src/EZ-Template/drive/exit_conditions.cpp +++ b/src/EZ-Template/drive/exit_conditions.cpp @@ -9,6 +9,11 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. using namespace ez; +void Drive::pid_drive_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout) { + leftPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); + rightPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); +} + void Drive::pid_drive_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QLength p_small_error, okapi::QTime p_big_exit_time, okapi::QLength p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout) { // Convert okapi units to doubles double se = p_small_error.convert(okapi::inch); @@ -18,8 +23,11 @@ void Drive::pid_drive_exit_condition_set(okapi::QTime p_small_exit_time, okapi:: double vet = p_velocity_exit_time.convert(okapi::millisecond); double mAt = p_mA_timeout.convert(okapi::millisecond); - leftPID.exit_condition_set(set, se, bet, be, vet, mAt); - rightPID.exit_condition_set(set, se, bet, be, vet, mAt); + pid_drive_exit_condition_set(set, se, bet, be, vet, mAt); +} + +void Drive::pid_turn_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout) { + turnPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); } void Drive::pid_turn_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QAngle p_small_error, okapi::QTime p_big_exit_time, okapi::QAngle p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout) { @@ -31,7 +39,11 @@ void Drive::pid_turn_exit_condition_set(okapi::QTime p_small_exit_time, okapi::Q double vet = p_velocity_exit_time.convert(okapi::millisecond); double mAt = p_mA_timeout.convert(okapi::millisecond); - turnPID.exit_condition_set(set, se, bet, be, vet, mAt); + pid_turn_exit_condition_set(set, se, bet, be, vet, mAt); +} + +void Drive::pid_swing_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout) { + swingPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); } void Drive::pid_swing_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QAngle p_small_error, okapi::QTime p_big_exit_time, okapi::QAngle p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout) { @@ -43,7 +55,7 @@ void Drive::pid_swing_exit_condition_set(okapi::QTime p_small_exit_time, okapi:: double vet = p_velocity_exit_time.convert(okapi::millisecond); double mAt = p_mA_timeout.convert(okapi::millisecond); - swingPID.exit_condition_set(set, se, bet, be, vet, mAt); + pid_swing_exit_condition_set(set, se, bet, be, vet, mAt); } // User wrapper for exit condition @@ -231,4 +243,17 @@ void Drive::pid_wait_until(okapi::QAngle target) { } else { printf("QAngle not supported for drive!\n"); } +} + +void Drive::pid_wait_until(double target) { + // If driving... + if (mode == DRIVE) { + wait_until_drive(target); + } + // If turning or swinging... + else if (mode == TURN || mode == SWING) { + wait_until_turn_swing(target); + } else { + printf("Not in a valid drive mode!\n"); + } } \ No newline at end of file diff --git a/src/EZ-Template/drive/set_pid.cpp b/src/EZ-Template/drive/set_pid.cpp index 35910d3b..4497cae9 100644 --- a/src/EZ-Template/drive/set_pid.cpp +++ b/src/EZ-Template/drive/set_pid.cpp @@ -10,13 +10,13 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. // Set PID constants void Drive::pid_drive_constants_set(double p, double i, double d, double p_start_i) { - pid_drive_forward_constants_set(p, i, d, p_start_i); - pid_drive_backward_constants_set(p, i, d, p_start_i); + pid_drive_constants_forward_set(p, i, d, p_start_i); + pid_drive_constants_backward_set(p, i, d, p_start_i); } PID::Constants Drive::pid_drive_constants_get() { - auto fwd_const = pid_drive_forward_constants_get(); - auto rev_const = pid_drive_backward_constants_get(); + auto fwd_const = pid_drive_constants_forward_get(); + auto rev_const = pid_drive_constants_backward_get(); if (!(fwd_const.kp == rev_const.kp && fwd_const.ki == rev_const.ki && fwd_const.kd == rev_const.kd && fwd_const.start_i == rev_const.start_i)) { printf("\nForward and Reverse constants are not the same!"); return {-1, -1, -1, -1}; @@ -24,19 +24,19 @@ PID::Constants Drive::pid_drive_constants_get() { return fwd_const; } -void Drive::pid_drive_forward_constants_set(double p, double i, double d, double p_start_i) { +void Drive::pid_drive_constants_forward_set(double p, double i, double d, double p_start_i) { forward_drivePID.constants_set(p, i, d, p_start_i); } -PID::Constants Drive::pid_drive_forward_constants_get() { +PID::Constants Drive::pid_drive_constants_forward_get() { return forward_drivePID.constants_get(); } -void Drive::pid_drive_backward_constants_set(double p, double i, double d, double p_start_i) { +void Drive::pid_drive_constants_backward_set(double p, double i, double d, double p_start_i) { backward_drivePID.constants_set(p, i, d, p_start_i); } -PID::Constants Drive::pid_drive_backward_constants_get() { +PID::Constants Drive::pid_drive_constants_backward_get() { return backward_drivePID.constants_get(); } @@ -49,11 +49,34 @@ PID::Constants Drive::pid_turn_constants_get() { } void Drive::pid_swing_constants_set(double p, double i, double d, double p_start_i) { - swingPID.constants_set(p, i, d, p_start_i); + pid_swing_constants_forward_set(p, i, d, p_start_i); + pid_swing_constants_backward_set(p, i, d, p_start_i); } PID::Constants Drive::pid_swing_constants_get() { - return swingPID.constants_get(); + auto fwd_const = pid_swing_constants_forward_get(); + auto rev_const = pid_swing_constants_backward_get(); + if (!(fwd_const.kp == rev_const.kp && fwd_const.ki == rev_const.ki && fwd_const.kd == rev_const.kd && fwd_const.start_i == rev_const.start_i)) { + printf("\nForward and Reverse constants are not the same!"); + return {-1, -1, -1, -1}; + } + return fwd_const; +} + +void Drive::pid_swing_constants_forward_set(double p, double i, double d, double p_start_i) { + forward_swingPID.constants_set(p, i, d, p_start_i); +} + +PID::Constants Drive::pid_swing_constants_forward_get() { + return forward_swingPID.constants_get(); +} + +void Drive::pid_swing_constants_backward_set(double p, double i, double d, double p_start_i) { + backward_swingPID.constants_set(p, i, d, p_start_i); +} + +PID::Constants Drive::pid_swing_constants_backward_get() { + return backward_swingPID.constants_get(); } void Drive::pid_heading_constants_set(double p, double i, double d, double p_start_i) { @@ -82,14 +105,14 @@ void Drive::pid_targets_reset() { turnPID.target_set(0); } -void Drive::drive_angle_raw_set(double angle) { +void Drive::drive_angle_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); + drive_angle_set(angle); } void Drive::drive_mode_set(e_mode p_mode) { mode = p_mode; } @@ -101,10 +124,8 @@ int Drive::pid_turn_min_get() { return turn_min; } void Drive::pid_swing_min_set(int min) { swing_min = abs(min); } int Drive::pid_swing_min_get() { return swing_min; } -// Set drive PID -void Drive::pid_drive_set(okapi::QLength p_target, int speed, bool slew_on, bool toggle_heading) { - double target = p_target.convert(okapi::inch); // Convert okapi unit to inches - +// Set drive PID raw +void Drive::pid_drive_set(double target, int speed, bool slew_on, bool toggle_heading) { // Print targets if (print_toggle) printf("Drive Started... Target Value: %f in", target); if (slew_on && print_toggle) printf(" with slew"); @@ -151,8 +172,14 @@ void Drive::pid_drive_set(okapi::QLength p_target, int speed, bool slew_on, bool drive_mode_set(DRIVE); } +// Set drive PID +void Drive::pid_drive_set(okapi::QLength p_target, int speed, bool slew_on, bool toggle_heading) { + double target = p_target.convert(okapi::inch); // Convert okapi unit to inches + pid_drive_set(target, speed, slew_on, toggle_heading); +} + // Raw Set Turn PID -void Drive::pid_turn_raw_set(double target, int speed, bool slew_on) { +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); @@ -173,20 +200,25 @@ void Drive::pid_turn_raw_set(double target, int speed, bool slew_on) { // Set turn PID void Drive::pid_turn_set(okapi::QAngle p_target, int speed, bool slew_on) { double target = p_target.convert(okapi::degree); // Convert okapi unit to degree - pid_turn_raw_set(target, speed, slew_on); + pid_turn_set(target, speed, slew_on); } // Set relative turn PID void Drive::pid_turn_relative_set(okapi::QAngle p_target, int speed, bool slew_on) { double target = p_target.convert(okapi::degree); // Convert okapi unit to degree + pid_turn_relative_set(target, speed, slew_on); +} + +// Set relative turn PID +void Drive::pid_turn_relative_set(double target, int speed, bool slew_on) { // Compute absolute target by adding to current heading - double absolute_target = turnPID.target_get() + target; + double absolute_target = headingPID.target_get() + target; if (print_toggle) printf("Relative "); - pid_turn_raw_set(absolute_target, speed, slew_on); + pid_turn_set(absolute_target, speed, slew_on); } // Raw Set Swing PID -void Drive::pid_swing_raw_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on) { +void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on) { // use left/right as 1 and -1, and multiply along with sgn of error to find if fwd or rev // Print targets @@ -199,20 +231,24 @@ void Drive::pid_swing_raw_set(e_swing type, double target, int speed, int opposi // Set constants according to the robots direction PID::Constants pid_consts; + PID::Constants pid_swing_consts; slew::Constants slew_consts; if (direction == -1) { pid_consts = backward_drivePID.constants_get(); + pid_swing_consts = backward_swingPID.constants_get(); slew_consts = slew_swing_backward.constants_get(); slew_swing_using_angle = slew_swing_rev_using_angle; } 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; } // Set targets for the side that isn't moving + swingPID.constants_set(pid_swing_consts.kp, pid_swing_consts.ki, pid_swing_consts.kd, pid_swing_consts.start_i); 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); leftPID.target_set(drive_sensor_left()); @@ -237,14 +273,18 @@ void Drive::pid_swing_raw_set(e_swing type, double target, int speed, int opposi // Set swing PID void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, bool slew_on) { double target = p_target.convert(okapi::degree); // Convert okapi unit to degree - pid_swing_raw_set(type, target, speed, opposite_speed, slew_on); + pid_swing_set(type, target, speed, opposite_speed, slew_on); } // Set relative swing PID void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, bool slew_on) { double target = p_target.convert(okapi::degree); // Convert okapi unit to degree + pid_swing_relative_set(type, target, speed, opposite_speed, slew_on); +} + +void Drive::pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on) { // Compute absolute target by adding to current heading - double absolute_target = swingPID.target_get() + target; + double absolute_target = headingPID.target_get() + target; if (print_toggle) printf("Relative "); - pid_swing_raw_set(type, absolute_target, speed, opposite_speed, slew_on); -} + pid_swing_set(type, absolute_target, speed, opposite_speed, slew_on); +} \ No newline at end of file diff --git a/src/autons.cpp b/src/autons.cpp index 124b901e..13c45983 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -15,10 +15,11 @@ const int SWING_SPEED = 90; /// void default_constants() { - chassis.pid_heading_constants_set(3, 0, 20, 0); - chassis.pid_drive_constants_set(10, 0, 100, 0); - chassis.pid_turn_constants_set(3, 0, 20, 0); - chassis.pid_swing_constants_set(5, 0, 30, 0); + // p, i, d + chassis.pid_heading_constants_set(3, 0, 20); + chassis.pid_drive_constants_set(10, 0, 100); + chassis.pid_turn_constants_set(3, 0, 20); + chassis.pid_swing_constants_set(5, 0, 30); chassis.pid_turn_exit_condition_set(300_ms, 3_deg, 500_ms, 7_deg, 750_ms, 750_ms); chassis.pid_swing_exit_condition_set(300_ms, 3_deg, 500_ms, 7_deg, 750_ms, 750_ms);