Skip to content

Commit

Permalink
#49 #59 #65 #84
Browse files Browse the repository at this point in the history 
All functions that have a parameter using okapi units has the ability to be called without using okapi units.

Fixed relative bug that would stop a swing from working off of a turn

Naming convention was fixed on setting constants.

Forward and Reverse PID for swings was implemented.
  • Loading branch information
@ssejrog
ssejrog committed Jan 25, 2024
1 parent 83dcb5d commit 329bfd9
Show file tree
Hide file tree
Showing 4 changed files with 264 additions and 60 deletions.
192 changes: 165 additions & 27 deletions include/EZ-Template/drive/drive.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,8 @@ class Drive {
PID rightPID;
PID backward_drivePID;
PID swingPID;
PID forward_swingPID;
PID backward_swingPID;

/**
* Slew objects.
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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.
*/
Expand All @@ -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.
Expand All @@ -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.
*/
Expand Down Expand Up @@ -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.
Expand All @@ -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.
Expand All @@ -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
Expand All @@ -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
*
Expand All @@ -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.
Expand All @@ -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
Expand All @@ -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.
Expand All @@ -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.
Expand Down Expand Up @@ -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()
*/
Expand Down
Loading

0 comments on commit 329bfd9

Please sign in to comment.