diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index ec7ba996..5d6565f1 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -18,53 +18,53 @@ using namespace ez; class Drive { public: /** - * Joysticks will return 0 when they are within this number. Set with set_joystick_threshold() - */ + * Joysticks will return 0 when they are within this number. Set with set_joystick_threshold() + */ int JOYSTICK_THRESHOLD; /** - * Current swing type. - */ + * Current swing type. + */ e_swing current_swing; /** - * Vector of pros motors for the left chassis. - */ + * Vector of pros motors for the left chassis. + */ std::vector left_motors; /** - * Vector of pros motors for the right chassis. - */ + * Vector of pros motors for the right chassis. + */ std::vector right_motors; /** - * Inertial sensor. - */ + * Inertial sensor. + */ pros::Imu imu; /** - * Left tracking wheel. - */ + * Left tracking wheel. + */ pros::ADIEncoder left_tracker; /** - * Right tracking wheel. - */ + * Right tracking wheel. + */ pros::ADIEncoder right_tracker; /** - * Left rotation tracker. - */ + * Left rotation tracker. + */ pros::Rotation left_rotation; /** - * Right rotation tracker. - */ + * Right rotation tracker. + */ pros::Rotation right_rotation; /** - * PID objects. - */ + * PID objects. + */ PID headingPID; PID turnPID; PID forward_drivePID; @@ -75,102 +75,102 @@ class Drive { /** - * Calibrates imu and initializes sd card to curve. - */ + * Calibrates imu and initializes sd card to curve. + */ void initialize(); /** - * Tasks for autonomous. - */ + * Tasks for autonomous. + */ pros::Task ez_auto; /** - * Creates a Drive Controller using internal encoders. - * - * \param left_motor_ports - * Input {1, -2...}. Make ports negative if reversed! - * \param right_motor_ports - * Input {-3, 4...}. Make ports negative if reversed! - * \param imu_port - * Port the IMU is plugged into. - * \param wheel_diameter - * Diameter of your drive wheels. Remember 4" is 4.125"! - * \param ticks - * Motor cartidge RPM - * \param ratio - * External gear ratio, wheel gear / motor gear. - */ + * Creates a Drive Controller using internal encoders. + * + * \param left_motor_ports + * Input {1, -2...}. Make ports negative if reversed! + * \param right_motor_ports + * Input {-3, 4...}. Make ports negative if reversed! + * \param imu_port + * Port the IMU is plugged into. + * \param wheel_diameter + * Diameter of your drive wheels. Remember 4" is 4.125"! + * \param ticks + * Motor cartidge RPM + * \param ratio + * External gear ratio, wheel gear / motor gear. + */ Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio); /** - * Creates a Drive Controller using encoders plugged into the brain. - * - * \param left_motor_ports - * Input {1, -2...}. Make ports negative if reversed! - * \param right_motor_ports - * Input {-3, 4...}. Make ports negative if reversed! - * \param imu_port - * Port the IMU is plugged into. - * \param wheel_diameter - * Diameter of your sensored wheels. Remember 4" is 4.125"! - * \param ticks - * Ticks per revolution of your encoder. - * \param ratio - * External gear ratio, wheel gear / sensor gear. - * \param left_tracker_ports - * Input {1, 2}. Make ports negative if reversed! - * \param right_tracker_ports - * Input {3, 4}. Make ports negative if reversed! - */ + * Creates a Drive Controller using encoders plugged into the brain. + * + * \param left_motor_ports + * Input {1, -2...}. Make ports negative if reversed! + * \param right_motor_ports + * Input {-3, 4...}. Make ports negative if reversed! + * \param imu_port + * Port the IMU is plugged into. + * \param wheel_diameter + * Diameter of your sensored wheels. Remember 4" is 4.125"! + * \param ticks + * Ticks per revolution of your encoder. + * \param ratio + * External gear ratio, wheel gear / sensor gear. + * \param left_tracker_ports + * Input {1, 2}. Make ports negative if reversed! + * \param right_tracker_ports + * Input {3, 4}. Make ports negative if reversed! + */ Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector left_tracker_ports, std::vector right_tracker_ports); /** - * Creates a Drive Controller using encoders plugged into a 3 wire expander. - * - * \param left_motor_ports - * Input {1, -2...}. Make ports negative if reversed! - * \param right_motor_ports - * Input {-3, 4...}. Make ports negative if reversed! - * \param imu_port - * Port the IMU is plugged into. - * \param wheel_diameter - * Diameter of your sensored wheels. Remember 4" is 4.125"! - * \param ratio - * External gear ratio, wheel gear / sensor gear. - * \param left_tracker_ports - * Input {1, 2}. Make ports negative if reversed! - * \param right_tracker_ports - * Input {3, 4}. Make ports negative if reversed! - */ + * Creates a Drive Controller using encoders plugged into a 3 wire expander. + * + * \param left_motor_ports + * Input {1, -2...}. Make ports negative if reversed! + * \param right_motor_ports + * Input {-3, 4...}. Make ports negative if reversed! + * \param imu_port + * Port the IMU is plugged into. + * \param wheel_diameter + * Diameter of your sensored wheels. Remember 4" is 4.125"! + * \param ratio + * External gear ratio, wheel gear / sensor gear. + * \param left_tracker_ports + * Input {1, 2}. Make ports negative if reversed! + * \param right_tracker_ports + * Input {3, 4}. Make ports negative if reversed! + */ Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ratio, int left_tracker_ports, int right_tracker_ports); /** - * Creates a Drive Controller using rotation sensors. - * - * \param left_motor_ports - * Input {1, -2...}. Make ports negative if reversed! - * \param right_motor_ports - * Input {-3, 4...}. Make ports negative if reversed! - * \param imu_port - * Port the IMU is plugged into. - * \param wheel_diameter - * Diameter of your sensored wheels. Remember 4" is 4.125"! - * \param ticks - * Ticks per revolution of your encoder. - * \param ratio - * External gear ratio, wheel gear / sensor gear. - * \param left_tracker_ports - * Input {1, 2}. Make ports negative if reversed! - * \param right_tracker_ports - * Input {3, 4}. Make ports negative if reversed! - * \param expander_smart_port - * Port the expander is plugged into. - */ + * Creates a Drive Controller using rotation sensors. + * + * \param left_motor_ports + * Input {1, -2...}. Make ports negative if reversed! + * \param right_motor_ports + * Input {-3, 4...}. Make ports negative if reversed! + * \param imu_port + * Port the IMU is plugged into. + * \param wheel_diameter + * Diameter of your sensored wheels. Remember 4" is 4.125"! + * \param ticks + * Ticks per revolution of your encoder. + * \param ratio + * External gear ratio, wheel gear / sensor gear. + * \param left_tracker_ports + * Input {1, 2}. Make ports negative if reversed! + * \param right_tracker_ports + * Input {3, 4}. Make ports negative if reversed! + * \param expander_smart_port + * Port the expander is plugged into. + */ Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector left_tracker_ports, std::vector right_tracker_ports, int expander_smart_port); /** - * Sets drive defaults. - */ + * Sets drive defaults. + */ void set_defaults(); ///// @@ -180,102 +180,102 @@ class Drive { ///// /** - * Sets the chassis to controller joysticks using tank control. Run is usercontrol. - * This passes the controller through the curve functions, but is disabled by default. Use toggle_controller_curve_modifier() to enable it. - */ + * Sets the chassis to controller joysticks using tank control. Run is usercontrol. + * This passes the controller through the curve functions, but is disabled by default. Use toggle_controller_curve_modifier() to enable it. + */ void tank(); /** - * Sets the chassis to controller joysticks using standard arcade control. Run is usercontrol. - * This passes the controller through the curve functions, but is disabled by default. Use toggle_controller_curve_modifier() to enable it. - * - * \param stick_type - * ez::SINGLE or ez::SPLIT control - */ + * Sets the chassis to controller joysticks using standard arcade control. Run is usercontrol. + * This passes the controller through the curve functions, but is disabled by default. Use toggle_controller_curve_modifier() to enable it. + * + * \param stick_type + * ez::SINGLE or ez::SPLIT control + */ void arcade_standard(e_type stick_type); /** - * Sets the chassis to controller joysticks using flipped arcade control. Run is usercontrol. - * This passes the controller through the curve functions, but is disabled by default. Use toggle_controller_curve_modifier() to enable it. - * - * \param stick_type - * ez::SINGLE or ez::SPLIT control - */ + * Sets the chassis to controller joysticks using flipped arcade control. Run is usercontrol. + * This passes the controller through the curve functions, but is disabled by default. Use toggle_controller_curve_modifier() to enable it. + * + * \param stick_type + * ez::SINGLE or ez::SPLIT control + */ void arcade_flipped(e_type stick_type); /** - * Initializes left and right curves with the SD card, reccomended to run in initialize(). - */ + * Initializes left and right curves with the SD card, reccomended to run in initialize(). + */ void init_curve_sd(); /** - * Sets the default joystick curves. - * - * \param left - * Left default curve. - * \param right - * Right default curve. - */ + * Sets the default joystick curves. + * + * \param left + * Left default curve. + * \param right + * Right default curve. + */ void set_curve_default(double left, double right); /** - * Runs a P loop on the drive when the joysticks are released. - * - * \param kp - * Constant for the p loop. - */ + * Runs a P loop on the drive when the joysticks are released. + * + * \param kp + * Constant for the p loop. + */ void set_active_brake(double kp); /** - * Enables/disables modifying the joystick input curves with the controller. True enables, false disables. - * - * \param input - * bool input - */ + * Enables/disables modifying the joystick input curves with the controller. True enables, false disables. + * + * \param input + * bool input + */ void toggle_modify_curve_with_controller(bool toggle); /** - * Sets buttons for modifying the left joystick curve. - * - * \param decrease - * a pros button enumerator - * \param increase - * a pros button enumerator - */ + * Sets buttons for modifying the left joystick curve. + * + * \param decrease + * a pros button enumerator + * \param increase + * a pros button enumerator + */ void set_left_curve_buttons(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase); /** - * Sets buttons for modifying the right joystick curve. - * - * \param decrease - * a pros button enumerator - * \param increase - * a pros button enumerator - */ + * Sets buttons for modifying the right joystick curve. + * + * \param decrease + * a pros button enumerator + * \param increase + * a pros button enumerator + */ void set_right_curve_buttons(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase); /** - * Outputs a curve from 5225A In the Zone. This gives more control over the robot at lower speeds. https://www.desmos.com/calculator/rcfjjg83zx - * - * \param x - * joystick input - */ + * Outputs a curve from 5225A In the Zone. This gives more control over the robot at lower speeds. https://www.desmos.com/calculator/rcfjjg83zx + * + * \param x + * joystick input + */ double left_curve_function(double x); /** - * Outputs a curve from 5225A In the Zone. This gives more control over the robot at lower speeds. https://www.desmos.com/calculator/rcfjjg83zx - * - * \param x - * joystick input - */ + * Outputs a curve from 5225A In the Zone. This gives more control over the robot at lower speeds. https://www.desmos.com/calculator/rcfjjg83zx + * + * \param x + * joystick input + */ double right_curve_function(double x); /** - * Sets a new threshold for the joystick. The joysticks wil not return a value if they are within this. - * - * \param threshold - * new threshold - */ + * Sets a new threshold for the joystick. The joysticks wil not return a value if they are within this. + * + * \param threshold + * new threshold + */ void set_joystick_threshold(int threshold); ///// @@ -285,29 +285,29 @@ class Drive { ///// /** - * Sets the chassis to voltage - * - * \param left - * voltage for left side, -127 to 127 - * \param right - * voltage for right side, -127 to 127 - */ + * Sets the chassis to voltage + * + * \param left + * voltage for left side, -127 to 127 + * \param right + * voltage for right side, -127 to 127 + */ void set_tank(int left, int right); /** - * Changes the way the drive behaves when it is not under active user control - * - * \param brake_type - * the 'brake mode' of the motor e.g. 'pros::E_MOTOR_BRAKE_COAST' 'pros::E_MOTOR_BRAKE_BRAKE' 'pros::E_MOTOR_BRAKE_HOLD' - */ + * Changes the way the drive behaves when it is not under active user control + * + * \param brake_type + * the 'brake mode' of the motor e.g. 'pros::E_MOTOR_BRAKE_COAST' 'pros::E_MOTOR_BRAKE_BRAKE' 'pros::E_MOTOR_BRAKE_HOLD' + */ void set_drive_brake(pros::motor_brake_mode_e_t brake_type); /** - * Sets the limit for the current on the drive. - * - * \param mA - * input in milliamps - */ + * Sets the limit for the current on the drive. + * + * \param mA + * input in milliamps + */ void set_drive_current_limit(int mA); ///// @@ -317,71 +317,71 @@ class Drive { ///// /** - * The position of the right motor. - */ + * The position of the right motor. + */ int right_sensor(); /** - * The velocity of the right motor. - */ + * The velocity of the right motor. + */ int right_velocity(); /** - * The watts of the right motor. - */ + * The watts of the right motor. + */ double right_mA(); /** - * Return TRUE when the motor is over current. - */ + * Return TRUE when the motor is over current. + */ bool right_over_current(); /** - * The position of the left motor. - */ + * The position of the left motor. + */ int left_sensor(); /** - * The velocity of the left motor. - */ + * The velocity of the left motor. + */ int left_velocity(); /** - * The watts of the left motor. - */ + * The watts of the left motor. + */ double left_mA(); /** - * Return TRUE when the motor is over current. - */ + * Return TRUE when the motor is over current. + */ bool left_over_current(); /** - * Reset all the chassis motors, reccomended to run at the start of your autonomous routine. - */ + * Reset all the chassis motors, reccomended to run at the start of your autonomous routine. + */ void reset_drive_sensor(); /** - * Reets the current gyro value. Defaults to 0, reccomended to run at the start of your autonomous routine. - * - * \param new_heading - * New heading value. - */ + * Resets the current gyro value. Defaults to 0, reccomended to run at the start of your autonomous routine. + * + * \param new_heading + * New heading value. + */ void reset_gyro(double new_heading = 0); /** - * Resets the imu so that where the drive is pointing is zero in set_drive_pid(turn) - */ + * Resets the imu so that where the drive is pointing is zero in set_drive_pid(turn) + */ double get_gyro(); /** - * Calibrates the IMU, reccomended to run in initialize(). - */ + * Calibrates the IMU, reccomended to run in initialize(). + */ bool imu_calibrate(); /** - * Loading display whlie the IMU calibrates. - */ + * Loading display while the IMU calibrates. + */ void imu_loading_display(int iter); ///// @@ -391,96 +391,96 @@ class Drive { ///// /** - * Sets the robot to move forward using PID. - * - * \param target - * target value in inches - * \param speed - * 0 to 127, max speed during motion - * \param slew_on - * ramp up from slew_min to speed over slew_distance. only use when you're going over about 14" - * \param toggle_heading - * toggle for heading correction - */ + * Sets the robot to move forward using PID. + * + * \param target + * target value in inches + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from slew_min to speed over slew_distance. only use when you're going over about 14" + * \param toggle_heading + * toggle for heading correction + */ void set_drive_pid(double target, int speed, bool slew_on = false, bool toggle_heading = true); /** - * Sets the robot to turn using PID. - * - * \param target - * target value in degrees - * \param speed - * 0 to 127, max speed during motion - */ + * Sets the robot to turn using PID. + * + * \param target + * target value in degrees + * \param speed + * 0 to 127, max speed during motion + */ void set_turn_pid(double target, int speed); /** - * Turn using only the left or right side. - * - * \param type - * L_SWING or R_SWING - * \param target - * target value in degrees - * \param speed - * 0 to 127, max speed during motion - */ + * Turn using only the left or right side. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value in degrees + * \param speed + * 0 to 127, max speed during motion + */ void set_swing_pid(e_swing type, double target, int speed); /** - * Lock the code in a while loop until the robot has settled. - */ + * Lock the code in a while loop until the robot has settled. + */ void wait_drive(); /** - * Lock the code in a while loop until this position has passed. - * - * \param target - * when driving, this is inches. when turning, this is degrees. - */ + * Lock the code in a while loop until this position has passed. + * + * \param target + * when driving, this is inches. when turning, this is degrees. + */ void wait_until(double target); /** - * Autonomous interference detection. Returns true when interfered, and false when nothing happened. - */ + * Autonomous interference detection. Returns true when interfered, and false when nothing happened. + */ bool interfered = false; /** - * Changes max speed during a drive motion. - * - * \param speed - * new clipped speed - */ + * Changes max speed during a drive motion. + * + * \param speed + * new clipped speed + */ void set_max_speed(int speed); /** - * Set Either the headingPID, turnPID, forwardPID, backwardPID, activeBrakePID, or swingPID - * IF NOT DONE PID WILL DEFAULT TO 0! - */ + * Set Either the headingPID, turnPID, forwardPID, backwardPID, activeBrakePID, or swingPID + * IF NOT DONE PID WILL DEFAULT TO 0! + */ void set_pid_constants(PID* pid, double p, double i, double d, double p_start_i); /** - * Sets minimum slew speed constants. - * - * \param fwd - * minimum power for forward drive pd - * \param rev - * minimum power for backwards drive pd - */ + * Sets minimum slew speed constants. + * + * \param fwd + * minimum power for forward drive pd + * \param rev + * minimum power for backwards drive pd + */ void set_slew_min_power(int fwd, int rev); /** - * Sets minimum slew distance constants. - * - * \param fw - * minimum distance for forward drive pd - * \param bw - * minimum distance for backwards drive pd - */ + * Sets minimum slew distance constants. + * + * \param fw + * minimum distance for forward drive pd + * \param bw + * minimum distance for backwards drive pd + */ void set_slew_distance(int fwd, int rev); /** - * Exit condition struct. - */ + * Exit condition struct. + */ struct exit_condition_ { int small_exit_time = 0; double small_error = 0; @@ -491,49 +491,68 @@ class Drive { }; /** - * Exit condition for turning. - */ + * Exit condition for turning. + */ exit_condition_ turn_exit; /** - * Exit condition for swinging. - */ + * Exit condition for swinging. + */ exit_condition_ swing_exit; /** - * Exit condition for driving. - */ + * Exit condition for driving. + */ exit_condition_ drive_exit; /** - * Set's constants for exit conditions. - * - * \param &type - * turn_exit, swing_exit, or drive_exit - * \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. - */ + * Set's constants for exit conditions. + * + * \param &type + * turn_exit, swing_exit, or drive_exit + * \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 set_exit_condition(exit_condition_ &type, int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_timeint, int p_mA_timeout); private: // !Auton + + /** + * Resets drive sensors at the start of opcontrol. + */ void reset_drive_sensors_opcontrol(); + + /** + * Sets minimum slew distance constants. + * + * \param l_stick + * input for left joystick + * \param r_stick + * input for right joystick + */ void joy_thresh_opcontrol(int l_stick, int r_stick); - // Heading toggle + /** + * Heading bool. + */ bool heading_on = true; - // Active brakeKP + /** + * Active brake kp constant. + */ double active_brake_kp = 0; - // Slew + /** + * Slew enum. + */ struct slew_ { int sign = 0; double error = 0; @@ -548,10 +567,39 @@ class Drive { slew_ left_slew; slew_ right_slew; - double slew_calculate(slew_ &input, double current); + /** + * Initialize slew. + * + * \param input + * slew_ enum + * \param slew_on + * is slew on? + * \param max_speed + * target speed during the slew + * \param target + * target sensor value + * \param current + * current sensor value + * \param start + * starting position + * \param backwards + * slew direction for constants + */ void slew_initialize(slew_ &input, bool slew_on, double max_speed, double target, double current, double start, bool backwards); - // Tick per inch calculation + /** + * Calculate slew. + * + * \param input + * slew_ enum + * \param current + * current sensor value + */ + double slew_calculate(slew_ &input, double current); + + /** + * Tick per inch calculation. + */ double TICK_PER_REV; double TICK_PER_INCH; double CIRCUMFERENCE; @@ -562,43 +610,64 @@ class Drive { double RATIO; double WHEEL_DIAMETER; - // auto max speed + /** + * Max speed for autonomous. + */ int max_speed; - // Exit conditions + /** + * Exit conditions + */ bool exit_condition(std::tuple> targets, exit_condition_ exitConditions, bool wait_until = false); - // Tasks + + /** + * Tasks + */ void drive_pid_task(); void swing_pid_task(); void turn_pid_task(); void ez_auto_task(); - // Slew constants + /** + * Constants for slew + */ double SLEW_DISTANCE[2]; double SLEW_MIN_POWER[2]; - // Starting value for left and right. + /** + * Starting value for left/right + */ double l_start = 0; double r_start = 0; - // Enable/disable modifying controller curve with controller. + /** + * Enable/disable modifying controller curve with controller. + */ bool disable_controller = true; // True enables, false disables. - // Is tank drive running? + /** + * Is tank drive running? + */ bool is_tank; #define DRIVE_INTEGRATED 1 #define DRIVE_ADI_ENCODER 2 #define DRIVE_ROTATION 3 - // Is tracker? + /** + * Is tracking? + */ int is_tracker = DRIVE_INTEGRATED; - // Save input curve to SD card + /** + * Save input to sd card + */ void save_l_curve_sd(); void save_r_curve_sd(); - // Structs for the buttons. + /** + * Struct for buttons for increasing/decreasing curve with controller + */ struct button_ { bool lock = false; bool release_reset = false; @@ -613,17 +682,25 @@ class Drive { button_ r_increase_; button_ r_decrease_; - // Uses button presses to modify controller curve. + /** + * Uses button presses to modify controller curve. + */ void modify_curve_with_controller(); - // Function for button presses. + /** + * Function for button presses. + */ void button_press(button_ *input_name, int button, std::function changeCurve, std::function save); - // The left and right curve scalers. + /** + * The left and right curve scalers. + */ double left_curve_scale; double right_curve_scale; - // Increase and decrease left and right curve scale. + /** + * Increase and decrease left and right curve scale. + */ void l_decrease(); void l_increase(); void r_decrease(); diff --git a/src/EZ-Template/drive/slew.cpp b/src/EZ-Template/drive/slew.cpp index 1665d896..3f800176 100644 --- a/src/EZ-Template/drive/slew.cpp +++ b/src/EZ-Template/drive/slew.cpp @@ -33,17 +33,17 @@ void Drive::slew_initialize(slew_ &input, bool slew_on, double max_speed, double // Slew calculation double Drive::slew_calculate(slew_ &input, double current) { - // Is lsew still on? + // Is slew still on? if (input.enabled) { // Error is distance away from completed slew input.error = input.x_intercept - current; // When the sign of error flips, slew is completed - if (ez::util::sgn(input.error) != input.sign) + if (util::sgn(input.error) != input.sign) input.enabled = false; // Return y=mx+b - else if (ez::util::sgn(input.error) == input.sign) + else if (util::sgn(input.error) == input.sign) return (input.slope * input.error) + input.y_intercept; } // When slew is completed, return max speed