diff --git a/CLOUDS/include/ary-lib/PID.hpp b/CLOUDS/include/ary-lib/PID.hpp new file mode 100644 index 0000000..3d5bb91 --- /dev/null +++ b/CLOUDS/include/ary-lib/PID.hpp @@ -0,0 +1,184 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include "ary-lib/util.hpp" +#include "api.h" + +class PID { + public: + /** + * Default constructor. + */ + PID(); + + /** + * Constructor with constants. + * + * \param p + * kP + * \param i + * ki + * \param d + * kD + * \param p_start_i + * error value that i starts within + * \param name + * std::string of name that prints + */ + PID(double p, double i = 0, double d = 0, double start_i = 0, std::string name = ""); + + /** + * Set constants for PID. + * + * \param p + * kP + * \param i + * ki + * \param d + * kD + * \param p_start_i + * error value that i starts within + */ + void set_constants(double p, double i = 0, double d = 0, double p_start_i = 0); + + /** + * Struct for constants. + */ + struct Constants { + double kp; + double ki; + double kd; + double start_i; + }; + + /** + * Struct for exit condition. + */ + struct exit_condition_ { + int small_exit_time = 0; + double small_error = 0; + int big_exit_time = 0; + double big_error = 0; + int velocity_exit_time = 0; + int mA_timeout = 0; + }; + + /** + * Set's constants for 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 set_exit_condition(int p_small_exit_time, double p_small_error, int p_big_exit_time = 0, double p_big_error = 0, int p_velocity_exit_time = 0, int p_mA_timeout = 0); + + /** + * Set's target. + * + * \param target + * Target for PID. + */ + void set_target(double input); + + /** + * Computes PID. + * + * \param current + * Current sensor library. + */ + double calculate(double current); + + /** + * Returns target value. + */ + double get_target(); + + /** + * Returns constants. + */ + Constants get_constants(); + + /** + * Resets all variables to 0. This does not reset constants. + */ + void reset_variables(); + + /** + * Constants + */ + Constants constants; + + /** + * Exit + */ + exit_condition_ exit; + + /** + * Iterative exit condition for PID. + * + * \param print = false + * if true, prints when complete. + */ + ary::exit_output exit_condition(bool print = false); + + /** + * Iterative exit condition for PID. + * + * \param sensor + * A pros motor on your mechanism. + * \param print = false + * if true, prints when complete. + */ + ary::exit_output exit_condition(pros::Motor sensor, bool print = false); + + /** + * Iterative exit condition for PID. + * + * \param sensor + * Pros motors on your mechanism. + * \param print = false + * if true, prints when complete. + */ + ary::exit_output exit_condition(std::vector sensor, bool print = false); + + /** + * Sets the name of the PID that prints during exit conditions. + * + * \param name + * a string that is the name you want to print + */ + void set_name(std::string name); + + /** + * PID variables. + */ + double output; + double cur; + double error; + double target; + double lastError; + double integral; + double derivative; + long time; + long prev_time; + + private: + int i = 0, j = 0, k = 0, l = 0; + bool is_mA = false; + void reset_timers(); + std::string name; + bool is_name = false; + void print_exit(ary::exit_output exit_type); +}; diff --git a/CLOUDS/include/ary-lib/api.hpp b/CLOUDS/include/ary-lib/api.hpp new file mode 100644 index 0000000..8d8709f --- /dev/null +++ b/CLOUDS/include/ary-lib/api.hpp @@ -0,0 +1,14 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include "ary-lib/PID.hpp" +#include "ary-lib/auton.hpp" +#include "ary-lib/auton_selector.hpp" +#include "ary-lib/drive/drive.hpp" +#include "ary-lib/autonselector.hpp" +#include "ary-lib/util.hpp" \ No newline at end of file diff --git a/CLOUDS/include/ary-lib/auton.hpp b/CLOUDS/include/ary-lib/auton.hpp new file mode 100644 index 0000000..448e1f5 --- /dev/null +++ b/CLOUDS/include/ary-lib/auton.hpp @@ -0,0 +1,19 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#pragma once +#include +#include + +class Auton { + public: + Auton(); + Auton(std::string, std::function); + std::string Name; + std::function auton_call; + + private: +}; diff --git a/CLOUDS/include/ary-lib/auton_selector.hpp b/CLOUDS/include/ary-lib/auton_selector.hpp new file mode 100644 index 0000000..8d6fdd8 --- /dev/null +++ b/CLOUDS/include/ary-lib/auton_selector.hpp @@ -0,0 +1,23 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#pragma once +#include + +#include "ary-lib/auton.hpp" + +using namespace std; +class AutonSelector { + public: + std::vector Autons; + int current_auton_page; + int auton_count; + AutonSelector(); + AutonSelector(std::vector autons); + void call_selected_auton(); + void print_selected_auton(); + void add_autons(std::vector autons); +}; diff --git a/CLOUDS/include/ary-lib/autonselector.hpp b/CLOUDS/include/ary-lib/autonselector.hpp new file mode 100644 index 0000000..580f896 --- /dev/null +++ b/CLOUDS/include/ary-lib/autonselector.hpp @@ -0,0 +1,64 @@ + /* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include "ary-lib/auton_selector.hpp" +#include "api.h" + +namespace ary { +namespace autonselector { +extern AutonSelector auton_selector; +/** + * Sets sd card to current page. + */ +void init_auton_selector(); + +/** + * Sets the sd card to current page. + */ +void update_auto_sd(); + +/** + * Increases the page by 1. + */ +void page_up(); + +/** + * Decreases the page by 1. + */ +void page_down(); + +/** + * Initializes LLEMU and sets up callbacks for auton selector. + */ +void initialize(); + +/** + * Wrapper for pros::lcd::shutdown. + */ +void shutdown(); + +extern bool turn_off; + +extern pros::ADIDigitalIn* left_limit_switch; +extern pros::ADIDigitalIn* right_limit_switch; +/** + * Initialize two limitswithces to change pages on the lcd + * + * @param left_limit_port + * port for the left limit switch + * @param right_limit_port + * port for the right limit switch + */ +void limit_switch_lcd_initialize(pros::ADIDigitalIn* right_limit, pros::ADIDigitalIn* left_limit = nullptr); + +/** + * pre_auto_task + */ +void limitSwitchTask(); +} +} \ No newline at end of file diff --git a/CLOUDS/include/ary-lib/drive/drive.hpp b/CLOUDS/include/ary-lib/drive/drive.hpp new file mode 100644 index 0000000..1affc95 --- /dev/null +++ b/CLOUDS/include/ary-lib/drive/drive.hpp @@ -0,0 +1,856 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include +#include +#include +#include + +#include "ary-lib/PID.hpp" +#include "ary-lib/util.hpp" +#include "pros/motors.h" + +#define VELOCITY_TO_VOLTS_LIN 196.001723856 +#define VELOCITY_TO_VOLTS_ANG 127 / 0.796332147963 + +#define LINEAR_MAX_VEL 0.647953484803 +#define LINEAR_FWD_ACCEL 0.647953484803 / 26 +#define LINEAR_FWD_DECEL 0.647953484803 / 18 +#define LINEAR_REV_ACCEL 0.647953484803 / 18 +#define LINEAR_REV_DECEL 0.647953484803 / 32 + +#define ANGULAR_MAX_VEL 0.796332147963 +#define ANGULAR_FWD_ACCEL 0.796332147963 / 40 +#define ANGULAR_FWD_DECEL 0.796332147963 / 40 +#define ANGULAR_REV_ACCEL 0.796332147963 / 40 +#define ANGULAR_REV_DECEL 0.796332147963 / 40 + +using namespace ary; + +class Drive { + public: + /** + * Joysticks will return 0 when they are within this number. Set with set_joystick_threshold() + */ + int JOYSTICK_THRESHOLD; + /** + * Global current brake mode. + */ + pros::motor_brake_mode_e_t CURRENT_BRAKE = pros::E_MOTOR_BRAKE_COAST; + + /** + * Global current mA. + */ + int CURRENT_MA = 2500; + + /** + * Current swing type. + */ + e_swing current_swing; + + /** + * Vector of pros motors for the left chassis. + */ + std::vector left_motors; + + /** + * Vector of pros motors for the right chassis. + */ + std::vector right_motors; + + /** + * Vector of pros motors that are disconnected from the drive. + */ + std::vector pto_active; + + /** + * Inertial sensor. + */ + pros::Imu imu; + + /** + * Left tracking wheel. + */ + pros::ADIEncoder left_tracker; + + /** + * Right tracking wheel. + */ + pros::ADIEncoder right_tracker; + + /** + * Left rotation tracker. + */ + pros::Rotation left_rotation; + + /** + * Right rotation tracker. + */ + pros::Rotation right_rotation; + + /** + * PID objects. + */ + PID headingPID; + PID turnPID; + PID forward_drivePID; + PID leftPID; + PID rightPID; + PID backward_drivePID; + PID swingPID; + + /** + * Current mode of the drive. + */ + e_mode mode; + + /** + * Sets current mode of drive. + */ + void set_mode(e_mode p_mode); + + /** + * Returns current mode of drive. + */ + e_mode get_mode(); + + /** + * Calibrates imu and initializes sd card to curve. + */ + void initialize(); + + /** + * 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 cartridge 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! + */ + 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 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); + + /** + * 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 ratio + * External gear ratio, wheel gear / sensor gear. + * \param left_tracker_port + * Make ports negative if reversed! + * \param right_tracker_port + * 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_rotation_port, int right_rotation_port); + + /** + * Sets drive defaults. + */ + void set_defaults(); + + ///// + // + // User Control + // + ///// + + /** + * 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_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 + * \param curve_type + */ + void arcade_standard(e_type stick_type, e_curve_type curve_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 + */ + void arcade_flipped(e_type stick_type); + + /** + * 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. + */ + void set_curve_default(double left, double right = 0); + + /** + * 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); + + void set_cata_active_brake(double kp); + + double get_cata_active_brake(); + + /** + * 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 + */ + 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 + */ + void set_right_curve_buttons(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase); + + void set_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 + */ + 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 + */ + 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 + */ + void set_joystick_threshold(int threshold); + + void set_joystick_drivescale(double scaleval); + + void set_joystick_turnscale(double scaleval); + + /** + * 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); + + ///// + // + // PTO + // + ///// + + /** + * Checks if the motor is currently in pto_list. Returns true if it's already in pto_list. + * + * \param check_if_pto + * motor to check. + */ + bool pto_check(pros::Motor check_if_pto); + + /** + * Adds motors to the pto list, removing them from the drive. + * + * \param pto_list + * list of motors to remove from the drive. + */ + void pto_add(std::vector pto_list); + + /** + * Removes motors from the pto list, adding them to the drive. You cannot use the first index in a pto. + * + * \param pto_list + * list of motors to add to the drive. + */ + void pto_remove(std::vector pto_list); + + /** + * Adds/removes motors from drive. You cannot use the first index in a pto. + * + * \param pto_list + * list of motors to add/remove from the drive. + * \param toggle + * if true, adds to list. if false, removes from list. + */ + void pto_toggle(std::vector pto_list, bool toggle); + + ///// + // + // PROS Wrapers + // + ///// + + /** + * 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' + */ + 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 + */ + void set_drive_current_limit(int mA); + + /** + * Toggles set drive in autonomous. True enables, false disables. + */ + void toggle_auto_drive(bool toggle); + + /** + * Toggles printing in autonomous. True enables, false disables. + */ + void toggle_auto_print(bool toggle); + + ///// + // + // Telemetry + // + ///// + + /** + * The position of the right motor. + */ + int right_sensor(); + + /** + * The velocity of the right motor. + */ + int right_velocity(); + + /** + * The watts of the right motor. + */ + double right_mA(); + + /** + * Return TRUE when the motor is over current. + */ + bool right_over_current(); + + /** + * The position of the left motor. + */ + int left_sensor(); + + /** + * The velocity of the left motor. + */ + int left_velocity(); + + /** + * The watts of the left motor. + */ + double left_mA(); + + /** + * 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. + */ + void reset_drive_sensor(); + + /** + * 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) + */ + double get_gyro(); + + /** + * Calibrates the IMU, reccomended to run in initialize(). + * + * \param run_loading_animation + * bool for running loading animation + */ + bool imu_calibrate(bool run_loading_animation = true); + + /** + * Loading display while the IMU calibrates. + */ + void imu_loading_display(int iter); + + ///// + // + // Autonomous Functions + // + ///// + + /** + * 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(double target, int speed, bool slew_on = false, bool toggle_heading = true); + + /* + + Stops movement on the drivetrain + */ + void stop_drive(Drive& chassis); + + /** + * Drives the robot forward using a trapezoidal motional profile + * + * /param target + * target value in inches + * /param endTimeout + * timeout value + */ + void set_proifiled_drive(Drive& chassis, double target, int endTimeout); + + /** + * 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(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 + */ + void set_swing(e_swing type, double target, int speed); + + /** + * Resets all PID targets to 0. + */ + void reset_pid_targets(); + + /** + * Resets all PID targets to 0. + */ + void set_angle(double angle); + + /** + * 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. + */ + void wait_until(double target); + + /** + * 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 + */ + void set_max_speed(int speed); + + /** + * Set Either the headingPID, turnPID, forwardPID, backwardPID, activeBrakePID, or swingPID + */ + void set_pid_constants(PID *pid, double p, double i, double d, double p_start_i); + + /** + * Sets minimum power for swings when kI and startI are enabled. + * + * \param min + * new clipped speed + */ + void set_swing_min(int min); + + /** + * The minimum power for turns when kI and startI are enabled. + * + * \param min + * new clipped speed + */ + void set_turn_min(int min); + + /** + * Returns minimum power for swings when kI and startI are enabled. + */ + int get_swing_min(); + + /** + * Returns minimum power for turns when kI and startI are enabled. + */ + int get_turn_min(); + + /** + * 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 + */ + void set_slew_distance(int fwd, int rev); + + /** + * 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(int type, 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); + + /** + * Exit condition for turning. + */ + const int turn_exit = 1; + + /** + * Exit condition for swinging. + */ + const int swing_exit = 2; + + /** + * Exit condition for driving. + */ + const int drive_exit = 3; + + /** + * Returns current tick_per_inch() + */ + double get_tick_per_inch(); + + /** + * Returns current tick_per_inch() + */ + void modify_curve_with_controller(); + + // Slew + struct slew_ { + int sign = 0; + double error = 0; + double x_intercept = 0; + double y_intercept = 0; + double slope = 0; + double output = 0; + bool enabled = false; + double max_speed = 0; + }; + + slew_ left_slew; + slew_ right_slew; + + /** + * 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); + + /** + * Calculate slew. + * + * \param input + * slew_ enum + * \param current + * current sensor value + */ + double slew_calculate(slew_ &input, double current); + + private: // !Auton + bool drive_toggle = true; + bool print_toggle = true; + int swing_min = 0; + int turn_min = 0; + + /** + * Heading bool. + */ + bool heading_on = true; + + /** + * Active brake kp constant. + */ + double active_brake_kp = 0; + + /** + * Tick per inch calculation. + */ + double TICK_PER_REV; + double TICK_PER_INCH; + double CIRCUMFERENCE; + + double CARTRIDGE; + double RATIO; + double WHEEL_DIAMETER; + + /** + * Max speed for autonomous. + */ + int max_speed; + + /** + * Tasks + */ + void drive_pid_task(); + void swing_pid_task(); + void turn_pid_task(); + void ez_auto_task(); + + /** + * Constants for slew + */ + double SLEW_DISTANCE[2]; + double SLEW_MIN_POWER[2]; + + /** + * Starting value for left/right + */ + double l_start = 0; + double r_start = 0; + + /** + * Enable/disable modifying controller curve with controller. + */ + bool disable_controller = true; // True enables, false disables. + + /** + * Is tank drive running? + */ + bool is_tank; + +#define DRIVE_INTEGRATED 1 +#define DRIVE_ADI_ENCODER 2 +#define DRIVE_ROTATION 3 + + /** + * Is tracking? + */ + int is_tracker = DRIVE_INTEGRATED; + + /** + * Save input to sd card + */ + void save_l_curve_sd(); + void save_r_curve_sd(); + + /** + * Struct for buttons for increasing/decreasing curve with controller + */ + struct button_ { + bool lock = false; + bool release_reset = false; + int release_timer = 0; + int hold_timer = 0; + int increase_timer; + pros::controller_digital_e_t button; + }; + + button_ l_increase_; + button_ l_decrease_; + button_ r_increase_; + button_ r_decrease_; + + /** + * Function for button presses. + */ + void button_press(button_ *input_name, int button, std::function changeCurve, std::function save); + + /** + * The left and right curve scalers. + */ + double left_curve_scale; + double right_curve_scale; + + /** + * Increase and decrease left and right curve scale. + */ + void l_decrease(); + void l_increase(); + void r_decrease(); + void r_increase(); +}; diff --git a/CLOUDS/include/ary-lib/util.hpp b/CLOUDS/include/ary-lib/util.hpp new file mode 100644 index 0000000..05c1c4c --- /dev/null +++ b/CLOUDS/include/ary-lib/util.hpp @@ -0,0 +1,118 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include +#include +#include +#include + +#include "api.h" + +/** + * Controller. + */ +extern pros::Controller master; + +namespace ary { + +/** + * Prints our branding all over your pros terminal + */ +void printScr(); + +/** + * Prints to the brain screen in one string. Splits input between lines with + * '\n' or when text longer then 32 characters. + * + * @param text + * Input string. Use '\n' for a new line + * @param line + * Starting line to print on, defaults to 0 + */ +void print_to_screen(std::string text, int line = 0); + +///// +// +// Public Variables +// +///// + +enum e_curve_type { + DEFAULT = 0, + LOGARITHMIC = 1, + SQRT = 2, + SINE = 3, +}; + +/** + * Enum for split and single stick arcade. + */ +enum e_type { SINGLE = 0, + SPLIT = 1 }; + +/** + * Enum for split and single stick arcade. + */ +enum e_swing { LEFT_SWING = 0, + RIGHT_SWING = 1 }; + +/** + * Enum for PID::exit_condition outputs. + */ +enum exit_output { RUNNING = 1, + SMALL_EXIT = 2, + BIG_EXIT = 3, + VELOCITY_EXIT = 4, + mA_EXIT = 5, + ERROR_NO_CONSTANTS = 6 }; + +/** + * Enum for split and single stick arcade. + */ +enum e_mode { DISABLE = 0, + SWING = 1, + TURN = 2, + DRIVE = 3 }; + +/** + * Outputs string for exit_condition enum. + */ +std::string exit_to_string(exit_output input); + +namespace util { +extern bool AUTON_RAN; + +/** + * Returns 1 if input is positive and -1 if input is negative + */ +int signum(double input); + +/** + * Returns true if the input is < 0 + */ +bool is_reversed(double input); + +/** + * Returns input restricted to min-max threshold + */ +double clip_num(double input, double max, double min); + + +std::vector trapezoidalMotionProfile(double target, double maxVel, double accel, double decel); + +/** + * Is the SD card plugged in? + */ +const bool IS_SD_CARD = pros::usd::is_installed(); + +/** + * Delay time for tasks + */ +const int DELAY_TIME = 10; +} // namespace util +} // ary diff --git a/CLOUDS/src/ary-lib/PID.cpp b/CLOUDS/src/ary-lib/PID.cpp new file mode 100644 index 0000000..6fff97d --- /dev/null +++ b/CLOUDS/src/ary-lib/PID.cpp @@ -0,0 +1,194 @@ +#include "main.h" +#include "util.hpp" + +using namespace ary; +using namespace util; + +void PID::reset_variables() { + output = 0; + target = 0; + error = 0; + lastError = 0; + integral = 0; + time = 0; + prev_time = 0; +} + +PID::PID() { + reset_variables(); + set_constants(0, 0, 0, 0); +} + +PID::Constants PID::get_constants() { return constants; } + +// PID constructor with constants +PID::PID(double p, double i, double d, double start_i, std::string name) { + reset_variables(); + set_constants(p, i, d, start_i); + set_name(name); +} + +// Set PID constants +void PID::set_constants(double p, double i, double d, double p_start_i) { + constants.kp = p; + constants.ki = i; + constants.kd = d; + constants.start_i = p_start_i; +} + +// Set exit condition timeouts +void PID::set_exit_condition(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) { + exit.small_exit_time = p_small_exit_time; + exit.small_error = p_small_error; + exit.big_exit_time = p_big_exit_time; + exit.big_error = p_big_error; + exit.velocity_exit_time = p_velocity_exit_time; + exit.mA_timeout = p_mA_timeout; +} + +void PID::set_target(double input) { target = input; } +double PID::get_target() { return target; } + +double PID::calculate(double dist) { + error = target - dist; // Calculate the error + derivative = error - lastError; // Calculate the derivative term + + /* + If the integral constant is a non-zero value, accumulate the integral term with the calculated error. + If the sign of the error is not equal to the sign of the previously recorded error, abandon the integral term. + */ + if (constants.ki != 0) { + integral += error; + + if (signum(error) != signum(lastError)) + integral = 0; + } + + output = (error * constants.kp) + (integral * constants.ki) + (derivative * constants.kd); // Combine all of the terms to get an output speed + + lastError = error; // Set the last error to the previously calculated error. + + return output; +} + +void PID::reset_timers() { + i = 0; + k = 0; + j = 0; + l = 0; + is_mA = false; +} + +void PID::set_name(std::string p_name) { + name = p_name; + is_name = name == "" ? false : true; +} + +void PID::print_exit(ary::exit_output exit_type) { + std::cout << " "; + if (is_name) + std::cout << name << " PID " << exit_to_string(exit_type) << " Exit.\n"; + else + std::cout << exit_to_string(exit_type) << " Exit.\n"; +} + +exit_output PID::exit_condition(bool print) { + // If this function is called while all exit constants are 0, print an error + if (!(exit.small_error && exit.small_exit_time && exit.big_error && exit.big_exit_time && exit.velocity_exit_time && exit.mA_timeout)) { + print_exit(ERROR_NO_CONSTANTS); + return ERROR_NO_CONSTANTS; + } + + // If the robot gets within the target, make sure it's there for small_timeout amount of time + if (exit.small_error != 0) { + if (abs(error) < exit.small_error) { + j += util::DELAY_TIME; + i = 0; // While this is running, don't run big thresh + if (j > exit.small_exit_time) { + reset_timers(); + if (print) print_exit(SMALL_EXIT); + return SMALL_EXIT; + } + } else { + j = 0; + } + } + + // If the robot is close to the target, start a timer. If the robot doesn't get closer within + // a certain amount of time, exit and continue. This does not run while small_timeout is running + if (exit.big_error != 0 && exit.big_exit_time != 0) { // Check if this condition is enabled + if (abs(error) < exit.big_error) { + i += util::DELAY_TIME; + if (i > exit.big_exit_time) { + reset_timers(); + if (print) print_exit(BIG_EXIT); + return BIG_EXIT; + } + } else { + i = 0; + } + } + + // If the motor velocity is 0,the code will timeout and set interfered to true. + if (exit.velocity_exit_time != 0) { // Check if this condition is enabled + if (abs(derivative) <= 0.05) { + k += util::DELAY_TIME; + if (k > exit.velocity_exit_time) { + reset_timers(); + if (print) print_exit(VELOCITY_EXIT); + return VELOCITY_EXIT; + } + } else { + k = 0; + } + } + + return RUNNING; +} + +exit_output PID::exit_condition(pros::Motor sensor, bool print) { + // If the motors are pulling too many mA, the code will timeout and set interfered to true. + if (exit.mA_timeout != 0) { // Check if this condition is enabled + if (sensor.is_over_current()) { + l += util::DELAY_TIME; + if (l > exit.mA_timeout) { + reset_timers(); + if (print) print_exit(mA_EXIT); + return mA_EXIT; + } + } else { + l = 0; + } + } + + return exit_condition(print); +} + +exit_output PID::exit_condition(std::vector sensor, bool print) { + // If the motors are pulling too many mA, the code will timeout and set interfered to true. + if (exit.mA_timeout != 0) { // Check if this condition is enabled + for (auto i : sensor) { + // Check if 1 motor is pulling too many mA + if (i.is_over_current()) { + is_mA = true; + break; + } + // If all of the motors aren't drawing too many mA, keep bool false + else { + is_mA = false; + } + } + if (is_mA) { + l += util::DELAY_TIME; + if (l > exit.mA_timeout) { + reset_timers(); + if (print) print_exit(mA_EXIT); + return mA_EXIT; + } + } else { + l = 0; + } + } + + return exit_condition(print); +} \ No newline at end of file diff --git a/CLOUDS/src/ary-lib/auton.cpp b/CLOUDS/src/ary-lib/auton.cpp new file mode 100644 index 0000000..c0a70f3 --- /dev/null +++ b/CLOUDS/src/ary-lib/auton.cpp @@ -0,0 +1,16 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#include "main.h" + +Auton::Auton() { + Name = ""; + auton_call = nullptr; +} +Auton::Auton(std::string name, std::function callback) { + Name = name; + auton_call = callback; +} diff --git a/CLOUDS/src/ary-lib/auton_selector.cpp b/CLOUDS/src/ary-lib/auton_selector.cpp new file mode 100644 index 0000000..653a285 --- /dev/null +++ b/CLOUDS/src/ary-lib/auton_selector.cpp @@ -0,0 +1,38 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#include "main.h" + +AutonSelector::AutonSelector() { + auton_count = 0; + current_auton_page = 0; + Autons = {}; +} + +AutonSelector::AutonSelector(std::vector autons) { + auton_count = autons.size(); + current_auton_page = 0; + Autons = {}; + Autons.assign(autons.begin(), autons.end()); +} + +void AutonSelector::print_selected_auton() { + if (auton_count == 0) return; + for (int i = 0; i < 8; i++) + pros::lcd::clear_line(i); + ary::print_to_screen("Page " + std::to_string(current_auton_page + 1) + "\n" + Autons[current_auton_page].Name); +} + +void AutonSelector::call_selected_auton() { + if (auton_count == 0) return; + Autons[current_auton_page].auton_call(); +} + +void AutonSelector::add_autons(std::vector autons) { + auton_count += autons.size(); + current_auton_page = 0; + Autons.assign(autons.begin(), autons.end()); +} diff --git a/CLOUDS/src/ary-lib/autonselector.cpp b/CLOUDS/src/ary-lib/autonselector.cpp new file mode 100644 index 0000000..ed97a73 --- /dev/null +++ b/CLOUDS/src/ary-lib/autonselector.cpp @@ -0,0 +1,78 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#include + +#include "main.h" + +namespace ary::autonselector { +AutonSelector auton_selector{}; + +void page_up() { + if (auton_selector.current_auton_page == auton_selector.auton_count - 1) + auton_selector.current_auton_page = 0; + else + auton_selector.current_auton_page++; + //update_auto_sd(); + auton_selector.print_selected_auton(); +} + +void page_down() { + if (auton_selector.current_auton_page == 0) + auton_selector.current_auton_page = auton_selector.auton_count - 1; + else + auton_selector.current_auton_page--; + //update_auto_sd(); + auton_selector.print_selected_auton(); +} + +void initialize() { + pros::lcd::initialize(); + //ary::autonselector::init_auton_selector(); + + // Callbacks for auto selector + ary::autonselector::auton_selector.print_selected_auton(); + pros::lcd::register_btn0_cb(ary::autonselector::page_down); + pros::lcd::register_btn2_cb(ary::autonselector::page_up); +} + +void shutdown() { + pros::lcd::shutdown(); +} + +bool turn_off = false; + +pros::ADIDigitalIn* left_limit_switch = nullptr; +pros::ADIDigitalIn* right_limit_switch = nullptr; +pros::Task limit_switch_task(limitSwitchTask); +void limit_switch_lcd_initialize(pros::ADIDigitalIn* right_limit, pros::ADIDigitalIn* left_limit) { + if (!left_limit && !right_limit) { + delete left_limit_switch; + delete right_limit_switch; + if (pros::millis() <= 100) + turn_off = true; + return; + } + turn_off = false; + right_limit_switch = right_limit; + left_limit_switch = left_limit; + limit_switch_task.resume(); +} + +void limitSwitchTask() { + while (true) { + if (right_limit_switch && right_limit_switch->get_new_press()) + page_up(); + else if (left_limit_switch && left_limit_switch->get_new_press()) + page_down(); + + if (pros::millis() >= 500 && turn_off) + limit_switch_task.suspend(); + + pros::delay(50); + } +} +} \ No newline at end of file diff --git a/CLOUDS/src/ary-lib/drive/drive.cpp b/CLOUDS/src/ary-lib/drive/drive.cpp new file mode 100644 index 0000000..87c0bae --- /dev/null +++ b/CLOUDS/src/ary-lib/drive/drive.cpp @@ -0,0 +1,341 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#include "drive.hpp" + +#include + +#include "main.h" +#include "pros/llemu.hpp" +#include "pros/screen.hpp" + +using namespace ary; + +// Constructor for integrated encoders +Drive::Drive(std::vector left_motor_ports, std::vector right_motor_ports, + int imu_port, double wheel_diameter, double ticks, double ratio) + : imu(imu_port), + left_tracker(-1, -1, false), // Default value + right_tracker(-1, -1, false), // Default value + left_rotation(-1), + right_rotation(-1), + ez_auto([this] { this->ez_auto_task(); }) { + is_tracker = DRIVE_INTEGRATED; + + // Set ports to a global vector + for (auto i : left_motor_ports) { + pros::Motor temp(abs(i), util::is_reversed(i)); + left_motors.push_back(temp); + } + for (auto i : right_motor_ports) { + pros::Motor temp(abs(i), util::is_reversed(i)); + right_motors.push_back(temp); + } + + // Set constants for tick_per_inch calculation + WHEEL_DIAMETER = wheel_diameter; + RATIO = ratio; + CARTRIDGE = ticks; + TICK_PER_INCH = get_tick_per_inch(); + + set_defaults(); +} + +// Constructor for tracking wheels plugged into the brain +Drive::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) + : imu(imu_port), + left_tracker(abs(left_tracker_ports[0]), abs(left_tracker_ports[1]), util::is_reversed(left_tracker_ports[0])), + right_tracker(abs(right_tracker_ports[0]), abs(right_tracker_ports[1]), util::is_reversed(right_tracker_ports[0])), + left_rotation(-1), + right_rotation(-1), + ez_auto([this] { this->ez_auto_task(); }) { + is_tracker = DRIVE_ADI_ENCODER; + + // Set ports to a global vector + for (auto i : left_motor_ports) { + pros::Motor temp(abs(i), util::is_reversed(i)); + left_motors.push_back(temp); + } + for (auto i : right_motor_ports) { + pros::Motor temp(abs(i), util::is_reversed(i)); + right_motors.push_back(temp); + } + + // Set constants for tick_per_inch calculation + WHEEL_DIAMETER = wheel_diameter; + RATIO = ratio; + CARTRIDGE = ticks; + TICK_PER_INCH = get_tick_per_inch(); + + set_defaults(); +} + +// Constructor for tracking wheels plugged into a 3 wire expander +Drive::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) + : imu(imu_port), + left_tracker({expander_smart_port, abs(left_tracker_ports[0]), abs(left_tracker_ports[1])}, util::is_reversed(left_tracker_ports[0])), + right_tracker({expander_smart_port, abs(right_tracker_ports[0]), abs(right_tracker_ports[1])}, util::is_reversed(right_tracker_ports[0])), + left_rotation(-1), + right_rotation(-1), + ez_auto([this] { this->ez_auto_task(); }) { + is_tracker = DRIVE_ADI_ENCODER; + + // Set ports to a global vector + for (auto i : left_motor_ports) { + pros::Motor temp(abs(i), util::is_reversed(i)); + left_motors.push_back(temp); + } + for (auto i : right_motor_ports) { + pros::Motor temp(abs(i), util::is_reversed(i)); + right_motors.push_back(temp); + } + + // Set constants for tick_per_inch calculation + WHEEL_DIAMETER = wheel_diameter; + RATIO = ratio; + CARTRIDGE = ticks; + TICK_PER_INCH = get_tick_per_inch(); + + set_defaults(); +} + +// Constructor for rotation sensors +Drive::Drive(std::vector left_motor_ports, std::vector right_motor_ports, + int imu_port, double wheel_diameter, double ratio, + int left_rotation_port, int right_rotation_port) + : imu(imu_port), + left_tracker(-1, -1, false), // Default value + right_tracker(-1, -1, false), // Default value + left_rotation(abs(left_rotation_port)), + right_rotation(abs(right_rotation_port)), + ez_auto([this] { this->ez_auto_task(); }) { + is_tracker = DRIVE_ROTATION; + left_rotation.set_reversed(util::is_reversed(left_rotation_port)); + right_rotation.set_reversed(util::is_reversed(right_rotation_port)); + + // Set ports to a global vector + for (auto i : left_motor_ports) { + pros::Motor temp(abs(i), util::is_reversed(i)); + left_motors.push_back(temp); + } + for (auto i : right_motor_ports) { + pros::Motor temp(abs(i), util::is_reversed(i)); + right_motors.push_back(temp); + } + + // Set constants for tick_per_inch calculation + WHEEL_DIAMETER = wheel_diameter; + RATIO = ratio; + CARTRIDGE = 4096; + TICK_PER_INCH = get_tick_per_inch(); + + set_defaults(); +} + +void Drive::set_defaults() { + // PID Constants + headingPID = {11, 0, 20, 0}; + forward_drivePID = {0.45, 0, 5, 0}; + backward_drivePID = {0.45, 0, 5, 0}; + turnPID = {5, 0.003, 35, 15}; + swingPID = {7, 0, 45, 0}; + leftPID = {0.45, 0, 5, 0}; + rightPID = {0.45, 0, 5, 0}; + set_turn_min(30); + set_swing_min(30); + + // Slew constants + set_slew_min_power(80, 80); + set_slew_distance(7, 7); + + // Exit condition constants + set_exit_condition(turn_exit, 100, 3, 500, 7, 500, 500); + set_exit_condition(swing_exit, 100, 3, 500, 7, 500, 500); + set_exit_condition(drive_exit, 80, 50, 300, 150, 500, 500); + + // Modify joystick curve on controller (defaults to disabled) + toggle_modify_curve_with_controller(true); + + // Left / Right modify buttons + set_left_curve_buttons(pros::E_CONTROLLER_DIGITAL_LEFT, pros::E_CONTROLLER_DIGITAL_RIGHT); + set_right_curve_buttons(pros::E_CONTROLLER_DIGITAL_Y, pros::E_CONTROLLER_DIGITAL_A); + + set_curve_buttons(pros::E_CONTROLLER_DIGITAL_LEFT, pros::E_CONTROLLER_DIGITAL_RIGHT); + + // Enable auto printing and drive motors moving + toggle_auto_drive(true); + toggle_auto_print(true); + + // Disables limit switch for auto selector + autonselector::limit_switch_lcd_initialize(nullptr, nullptr); +} + +double Drive::get_tick_per_inch() { + CIRCUMFERENCE = WHEEL_DIAMETER * 3.141; + + if (is_tracker == DRIVE_ADI_ENCODER || is_tracker == DRIVE_ROTATION) + TICK_PER_REV = CARTRIDGE * RATIO; + else + TICK_PER_REV = (50.0 * (3600.0 / CARTRIDGE)) * RATIO; // with no cart, the encoder reads 50 counts per rotation + + TICK_PER_INCH = (TICK_PER_REV / CIRCUMFERENCE); + return TICK_PER_INCH; +} + +void Drive::set_pid_constants(PID* pid, double p, double i, double d, double p_start_i) { + pid->set_constants(p, i, d, p_start_i); +} + +void Drive::set_tank(int left, int right) { + if (pros::millis() < 1500) return; + + for (auto i : left_motors) { + if (!pto_check(i)) i.move_voltage(left * (12000.0 / 127.0)); // If the motor is in the pto list, don't do anything to the motor. + } + for (auto i : right_motors) { + if (!pto_check(i)) i.move_voltage(right * (12000.0 / 127.0)); // If the motor is in the pto list, don't do anything to the motor. + } +} + +void Drive::set_drive_current_limit(int mA) { + if (abs(mA) > 2500) { + mA = 2500; + } + CURRENT_MA = mA; + for (auto i : left_motors) { + if (!pto_check(i)) i.set_current_limit(abs(mA)); // If the motor is in the pto list, don't do anything to the motor. + } + for (auto i : right_motors) { + if (!pto_check(i)) i.set_current_limit(abs(mA)); // If the motor is in the pto list, don't do anything to the motor. + } +} + +// Motor telemetry +void Drive::reset_drive_sensor() { + left_motors.front().tare_position(); + right_motors.front().tare_position(); + if (is_tracker == DRIVE_ADI_ENCODER) { + left_tracker.reset(); + right_tracker.reset(); + return; + } else if (is_tracker == DRIVE_ROTATION) { + left_rotation.reset_position(); + right_rotation.reset_position(); + return; + } +} + +int Drive::right_sensor() { + if (is_tracker == DRIVE_ADI_ENCODER) + return right_tracker.get_value(); + else if (is_tracker == DRIVE_ROTATION) + return right_rotation.get_position(); + return right_motors.front().get_position(); +} +int Drive::right_velocity() { return right_motors.front().get_actual_velocity(); } +double Drive::right_mA() { return right_motors.front().get_current_draw(); } +bool Drive::right_over_current() { return right_motors.front().is_over_current(); } + +int Drive::left_sensor() { + if (is_tracker == DRIVE_ADI_ENCODER) + return left_tracker.get_value(); + else if (is_tracker == DRIVE_ROTATION) + return left_rotation.get_position(); + return left_motors.front().get_position(); +} +int Drive::left_velocity() { return left_motors.front().get_actual_velocity(); } +double Drive::left_mA() { return left_motors.front().get_current_draw(); } +bool Drive::left_over_current() { return left_motors.front().is_over_current(); } + +void Drive::reset_gyro(double new_heading) { imu.set_rotation(new_heading); } +double Drive::get_gyro() { return imu.get_rotation(); } + +void Drive::imu_loading_display(int iter) { + // If the lcd is already initialized, don't run this function + if (pros::lcd::is_initialized()) return; + + // Boarder + int boarder = 50; + + // Create the boarder + pros::screen::set_pen(COLOR_WHITE); + for (int i = 1; i < 3; i++) { + pros::screen::draw_rect(boarder + i, boarder + i, 480 - boarder - i, 240 - boarder - i); + } + + // While IMU is loading + if (iter < 2000) { + static int last_x1 = boarder; + pros::screen::set_pen(0x00FF6EC7); // EZ Pink + int x1 = (iter * ((480 - (boarder * 2)) / 2000.0)) + boarder; + pros::screen::fill_rect(last_x1, boarder, x1, 240 - boarder); + last_x1 = x1; + } + // Failsafe time + else { + static int last_x1 = boarder; + pros::screen::set_pen(COLOR_RED); + int x1 = ((iter - 2000) * ((480 - (boarder * 2)) / 1000.0)) + boarder; + pros::screen::fill_rect(last_x1, boarder, x1, 240 - boarder); + last_x1 = x1; + } +} + +void Drive::stop_drive(Drive& chassis) { + chassis.set_tank(0, 0); +} + +bool Drive::imu_calibrate(bool run_loading_animation) { + imu.reset(); + int iter = 0; + while (true) { + iter += util::DELAY_TIME; + + if (run_loading_animation) imu_loading_display(iter); + + if (iter >= 2000) { + if (!(imu.get_status() & pros::c::E_IMU_STATUS_CALIBRATING)) { + break; + } + if (iter >= 3000) { + printf("No IMU plugged in, (took %d ms to realize that)\n", iter); + return false; + } + } + pros::delay(util::DELAY_TIME); + } + master.rumble("."); + printf("IMU is done calibrating (took %d ms)\n", iter); + return true; +} + +// Brake modes +void Drive::set_drive_brake(pros::motor_brake_mode_e_t brake_type) { + CURRENT_BRAKE = brake_type; + for (auto i : left_motors) { + if (!pto_check(i)) i.set_brake_mode(brake_type); // If the motor is in the pto list, don't do anything to the motor. + } + for (auto i : right_motors) { + if (!pto_check(i)) i.set_brake_mode(brake_type); // If the motor is in the pto list, don't do anything to the motor. + } +} + +void Drive::init_curve_sd() { + // do absolutely nothing +} + +void Drive::initialize() { + init_curve_sd(); + imu_calibrate(); + reset_drive_sensor(); +} + +void Drive::toggle_auto_drive(bool toggle) { drive_toggle = toggle; } +void Drive::toggle_auto_print(bool toggle) { print_toggle = toggle; } \ No newline at end of file diff --git a/CLOUDS/src/ary-lib/drive/exit_conditions.cpp b/CLOUDS/src/ary-lib/drive/exit_conditions.cpp new file mode 100644 index 0000000..71fb3ae --- /dev/null +++ b/CLOUDS/src/ary-lib/drive/exit_conditions.cpp @@ -0,0 +1,185 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#include "ary-lib/util.hpp" +#include "main.h" + +using namespace ary; + +// Set exit condition timeouts +void Drive::set_exit_condition(int type, 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) { + if (type == drive_exit) { + leftPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); + rightPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); + } + + if (type == turn_exit) { + turnPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); + } + + if (type == swing_exit) { + swingPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); + } +} + +// User wrapper for exit condition +void Drive::wait_drive() { + // Let the PID run at least 1 iteration + pros::delay(util::DELAY_TIME); + + if (mode == DRIVE) { + exit_output left_exit = RUNNING; + exit_output right_exit = RUNNING; + while (left_exit == RUNNING || right_exit == RUNNING) { + left_exit = left_exit != RUNNING ? left_exit : leftPID.exit_condition(left_motors[0]); + right_exit = right_exit != RUNNING ? right_exit : rightPID.exit_condition(right_motors[0]); + pros::delay(util::DELAY_TIME); + } + if (print_toggle) std::cout << " Left: " << exit_to_string(left_exit) << " Exit. Right: " << exit_to_string(right_exit) << " Exit.\n"; + + if (left_exit == mA_EXIT || left_exit == VELOCITY_EXIT || right_exit == mA_EXIT || right_exit == VELOCITY_EXIT) { + interfered = true; + } + } + + // Turn Exit + else if (mode == TURN) { + exit_output turn_exit = RUNNING; + while (turn_exit == RUNNING) { + turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]}); + pros::delay(util::DELAY_TIME); + } + if (print_toggle) std::cout << " Turn: " << exit_to_string(turn_exit) << " Exit.\n"; + + if (turn_exit == mA_EXIT || turn_exit == VELOCITY_EXIT) { + interfered = true; + } + } + + // Swing Exit + else if (mode == SWING) { + exit_output swing_exit = RUNNING; + pros::Motor& sensor = current_swing == ary::LEFT_SWING ? left_motors[0] : right_motors[0]; + while (swing_exit == RUNNING) { + swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor); + pros::delay(util::DELAY_TIME); + } + if (print_toggle) std::cout << " Swing: " << exit_to_string(swing_exit) << " Exit.\n"; + + if (swing_exit == mA_EXIT || swing_exit == VELOCITY_EXIT) { + interfered = true; + } + } +} + +// Function to wait until a certain position is reached. Wrapper for exit condition. +void Drive::wait_until(double target) { + // If robot is driving... + if (mode == DRIVE) { + // Calculate error between current and target (target needs to be an in between position) + int l_tar = l_start + (target * TICK_PER_INCH); + int r_tar = r_start + (target * TICK_PER_INCH); + int l_error = l_tar - left_sensor(); + int r_error = r_tar - right_sensor(); + int l_sgn = util::signum(l_error); + int r_sgn = util::signum(r_error); + + exit_output left_exit = RUNNING; + exit_output right_exit = RUNNING; + + while (true) { + l_error = l_tar - left_sensor(); + r_error = r_tar - right_sensor(); + + // Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop + if (util::signum(l_error) == l_sgn || util::signum(r_error) == r_sgn) { + if (left_exit == RUNNING || right_exit == RUNNING) { + left_exit = left_exit != RUNNING ? left_exit : leftPID.exit_condition(left_motors[0]); + right_exit = right_exit != RUNNING ? right_exit : rightPID.exit_condition(right_motors[0]); + pros::delay(util::DELAY_TIME); + } else { + if (print_toggle) std::cout << " Left: " << exit_to_string(left_exit) << " Wait Until Exit. Right: " << exit_to_string(right_exit) << " Wait Until Exit.\n"; + + if (left_exit == mA_EXIT || left_exit == VELOCITY_EXIT || right_exit == mA_EXIT || right_exit == VELOCITY_EXIT) { + interfered = true; + } + return; + } + } + // Once we've past target, return + else if (util::signum(l_error) != l_sgn || util::signum(r_error) != r_sgn) { + if (print_toggle) std::cout << " Drive Wait Until Exit.\n"; + return; + } + + pros::delay(util::DELAY_TIME); + } + } + + // If robot is turning or swinging... + else if (mode == TURN || mode == SWING) { + // Calculate error between current and target (target needs to be an in between position) + int g_error = target - get_gyro(); + int g_sgn = util::signum(g_error); + + exit_output turn_exit = RUNNING; + exit_output swing_exit = RUNNING; + + pros::Motor& sensor = current_swing == ary::LEFT_SWING ? left_motors[0] : right_motors[0]; + + while (true) { + g_error = target - get_gyro(); + + // If turning... + if (mode == TURN) { + // Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop + if (util::signum(g_error) == g_sgn) { + if (turn_exit == RUNNING) { + turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]}); + pros::delay(util::DELAY_TIME); + } else { + if (print_toggle) std::cout << " Turn: " << exit_to_string(turn_exit) << " Wait Until Exit.\n"; + + if (turn_exit == mA_EXIT || turn_exit == VELOCITY_EXIT) { + interfered = true; + } + return; + } + } + // Once we've past target, return + else if (util::signum(g_error) != g_sgn) { + if (print_toggle) std::cout << " Turn Wait Until Exit.\n"; + return; + } + } + + // If swinging... + else { + // Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop + if (util::signum(g_error) == g_sgn) { + if (swing_exit == RUNNING) { + swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor); + pros::delay(util::DELAY_TIME); + } else { + if (print_toggle) std::cout << " Swing: " << exit_to_string(swing_exit) << " Wait Until Exit.\n"; + + if (swing_exit == mA_EXIT || swing_exit == VELOCITY_EXIT) { + interfered = true; + } + return; + } + } + // Once we've past target, return + else if (util::signum(g_error) != g_sgn) { + if (print_toggle) std::cout << " Swing Wait Until Exit.\n"; + return; + } + } + + pros::delay(util::DELAY_TIME); + } + } +} diff --git a/CLOUDS/src/ary-lib/drive/pid_tasks.cpp b/CLOUDS/src/ary-lib/drive/pid_tasks.cpp new file mode 100644 index 0000000..8322aa4 --- /dev/null +++ b/CLOUDS/src/ary-lib/drive/pid_tasks.cpp @@ -0,0 +1,98 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#include "main.h" +#include "pros/misc.hpp" + +using namespace ary; + +void Drive::ez_auto_task() { + while (true) { + // Autonomous PID + if (get_mode() == DRIVE) + drive_pid_task(); + else if (get_mode() == TURN) + turn_pid_task(); + else if (get_mode() == SWING) + swing_pid_task(); + + if (pros::competition::is_autonomous() && !util::AUTON_RAN) + util::AUTON_RAN = true; + else if (!pros::competition::is_autonomous()) + set_mode(DISABLE); + + pros::delay(util::DELAY_TIME); + } +} + +// Drive PID task +void Drive::drive_pid_task() { + // Compute PID + leftPID.calculate(left_sensor()); + rightPID.calculate(right_sensor()); + headingPID.calculate(get_gyro()); + + // Compute slew + double l_slew_out = slew_calculate(left_slew, left_sensor()); + double r_slew_out = slew_calculate(right_slew, right_sensor()); + + // Clip leftPID and rightPID to slew (if slew is disabled, it returns max_speed) + double l_drive_out = util::clip_num(leftPID.output, l_slew_out, -l_slew_out); + double r_drive_out = util::clip_num(rightPID.output, r_slew_out, -r_slew_out); + + // Toggle heading + double gyro_out = heading_on ? headingPID.output : 0; + + // Combine heading and drive + double l_out = l_drive_out + gyro_out; + double r_out = r_drive_out - gyro_out; + + // Set motors + if (drive_toggle) + set_tank(l_out, r_out); +} + +// Turn PID task +void Drive::turn_pid_task() { + // Compute PID + turnPID.calculate(get_gyro()); + + // Clip gyroPID to max speed + double gyro_out = util::clip_num(turnPID.output, max_speed, -max_speed); + + // Clip the speed of the turn when the robot is within StartI, only do this when target is larger then StartI + if (turnPID.constants.ki != 0 && (fabs(turnPID.get_target()) > turnPID.constants.start_i && fabs(turnPID.error) < turnPID.constants.start_i)) { + if (get_turn_min() != 0) + gyro_out = util::clip_num(gyro_out, get_turn_min(), -get_turn_min()); + } + + // Set motors + if (drive_toggle) + set_tank(gyro_out, -gyro_out); +} + +// Swing PID task +void Drive::swing_pid_task() { + // Compute PID + swingPID.calculate(get_gyro()); + + // Clip swingPID to max speed + double swing_out = util::clip_num(swingPID.output, max_speed, -max_speed); + + // Clip the speed of the turn when the robot is within StartI, only do this when target is larger then StartI + if (swingPID.constants.ki != 0 && (fabs(swingPID.get_target()) > swingPID.constants.start_i && fabs(swingPID.error) < swingPID.constants.start_i)) { + if (get_swing_min() != 0) + swing_out = util::clip_num(swing_out, get_swing_min(), -get_swing_min()); + } + + if (drive_toggle) { + // Check if left or right swing, then set motors accordingly + if (current_swing == LEFT_SWING) + set_tank(swing_out, 0); + else if (current_swing == RIGHT_SWING) + set_tank(0, -swing_out); + } +} diff --git a/CLOUDS/src/ary-lib/drive/pto.cpp b/CLOUDS/src/ary-lib/drive/pto.cpp new file mode 100644 index 0000000..6aa24fd --- /dev/null +++ b/CLOUDS/src/ary-lib/drive/pto.cpp @@ -0,0 +1,54 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#include +#include + +#include "drive.hpp" +#include "main.h" + +bool Drive::pto_check(pros::Motor check_if_pto) { + auto does_exist = std::find(pto_active.begin(), pto_active.end(), check_if_pto.get_port()); + if (does_exist != pto_active.end()) + return true; // Motor is in the list + return false; // Motor isn't in the list +} + +void Drive::pto_add(std::vector pto_list) { + for (auto i : pto_list) { + // Return if the motor is already in the list + if (pto_check(i)) return; + + // Return if the first index was used (this motor is used for velocity) + if (i.get_port() == left_motors[0].get_port() || i.get_port() == right_motors[0].get_port()) { + printf("You cannot PTO the first index!\n"); + return; + } + + pto_active.push_back(i.get_port()); + } +} + +void Drive::pto_remove(std::vector pto_list) { + for (auto i : pto_list) { + auto does_exist = std::find(pto_active.begin(), pto_active.end(), i.get_port()); + // Return if the motor isn't in the list + if (does_exist == pto_active.end()) return; + + // Find index of motor + int index = std::distance(pto_active.begin(), does_exist); + pto_active.erase(pto_active.begin() + index); + i.set_brake_mode(CURRENT_BRAKE); // Set the motor to the brake type of the drive + i.set_current_limit(CURRENT_MA); // Set the motor to the mA of the drive + } +} + +void Drive::pto_toggle(std::vector pto_list, bool toggle) { + if (toggle) + pto_add(pto_list); + else + pto_remove(pto_list); +} \ No newline at end of file diff --git a/CLOUDS/src/ary-lib/drive/set_pid.cpp b/CLOUDS/src/ary-lib/drive/set_pid.cpp new file mode 100644 index 0000000..cddc0a9 --- /dev/null +++ b/CLOUDS/src/ary-lib/drive/set_pid.cpp @@ -0,0 +1,136 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#include "main.h" + +// Updates max speed +void Drive::set_max_speed(int speed) { + max_speed = util::clip_num(abs(speed), 127, -127); +} + +void Drive::reset_pid_targets() { + headingPID.set_target(0); + leftPID.set_target(0); + rightPID.set_target(0); + forward_drivePID.set_target(0); + backward_drivePID.set_target(0); + turnPID.set_target(0); +} + +void Drive::set_angle(double angle) { + headingPID.set_target(angle); + reset_gyro(angle); +} + +void Drive::set_mode(e_mode p_mode) { + mode = p_mode; +} + +void Drive::set_turn_min(int min) { turn_min = abs(min); } +int Drive::get_turn_min() { return turn_min; } + +void Drive::set_swing_min(int min) { swing_min = abs(min); } +int Drive::get_swing_min() { return swing_min; } + +e_mode Drive::get_mode() { return mode; } + +// Set drive PID +void Drive::set_drive(double target, int speed, bool slew_on, bool toggle_heading) { + TICK_PER_INCH = get_tick_per_inch(); + + // Print targets + if (print_toggle) printf("Drive Started... Target Value: %f (%f ticks)", target, target * TICK_PER_INCH); + if (slew_on && print_toggle) printf(" with slew"); + if (print_toggle) printf("\n"); + + // Global setup + set_max_speed(speed); + heading_on = toggle_heading; + bool is_backwards = false; + l_start = left_sensor(); + r_start = right_sensor(); + + double l_target_encoder, r_target_encoder; + + // Figure actual target value + l_target_encoder = l_start + (target * TICK_PER_INCH); + r_target_encoder = r_start + (target * TICK_PER_INCH); + + // Figure out if going forward or backward + if (l_target_encoder < l_start && r_target_encoder < r_start) { + auto consts = backward_drivePID.get_constants(); + leftPID.set_constants(consts.kp, consts.ki, consts.kd, consts.start_i); + rightPID.set_constants(consts.kp, consts.ki, consts.kd, consts.start_i); + is_backwards = true; + } else { + auto consts = forward_drivePID.get_constants(); + leftPID.set_constants(consts.kp, consts.ki, consts.kd, consts.start_i); + rightPID.set_constants(consts.kp, consts.ki, consts.kd, consts.start_i); + is_backwards = false; + } + + // Set PID targets + leftPID.set_target(l_target_encoder); + rightPID.set_target(r_target_encoder); + + // Initialize slew + slew_initialize(left_slew, slew_on, max_speed, l_target_encoder, left_sensor(), l_start, is_backwards); + slew_initialize(right_slew, slew_on, max_speed, r_target_encoder, right_sensor(), r_start, is_backwards); + + // Run task + set_mode(DRIVE); +} + +// Set turn PID +void Drive::set_turn(double target, int speed) { + // Print targets + if (print_toggle) printf("Turn Started... Target Value: %f\n", target); + + // Set PID targets + turnPID.set_target(target); + headingPID.set_target(target); // Update heading target for next drive motion + set_max_speed(speed); + + // Run task + set_mode(TURN); +} + +// Set swing PID +void Drive::set_swing(e_swing type, double target, int speed) { + // Print targets + if (print_toggle) printf("Swing Started... Target Value: %f\n", target); + current_swing = type; + + // Set PID targets + swingPID.set_target(target); + headingPID.set_target(target); // Update heading target for next drive motion + set_max_speed(speed); + + // Run task + set_mode(SWING); +} + +void set_profiled_drive(Drive& chassis, double target, int endTimeout) { + //kv: rpm -> voltage + //sf: in/ms -> rpm + int sign = ary::util::signum(target); + target = fabs(target); + std::vector profile; + // std::cout << "reached 1" << std::endl; + if(sign > 0) profile = ary::util::trapezoidalMotionProfile(target, LINEAR_MAX_VEL, LINEAR_FWD_ACCEL, LINEAR_FWD_DECEL); + else profile = ary::util::trapezoidalMotionProfile(target, LINEAR_MAX_VEL, LINEAR_REV_ACCEL, LINEAR_REV_DECEL); + + for (int i = 0; i < profile.size(); i++) + { + chassis.set_tank(profile[i] * VELOCITY_TO_VOLTS_LIN * sign, profile[i] * VELOCITY_TO_VOLTS_LIN * sign); + pros::delay(10); + } + + chassis.set_tank(0, 0); // Stop the drive + chassis.set_drive_brake(MOTOR_BRAKE_BRAKE); + pros::delay(endTimeout); + +} diff --git a/CLOUDS/src/ary-lib/drive/slew.cpp b/CLOUDS/src/ary-lib/drive/slew.cpp new file mode 100644 index 0000000..23088a0 --- /dev/null +++ b/CLOUDS/src/ary-lib/drive/slew.cpp @@ -0,0 +1,51 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#include "main.h" + +using namespace ary; + +// Set minimum power +void Drive::set_slew_min_power(int fwd, int rev) { + SLEW_MIN_POWER[0] = abs(fwd); + SLEW_MIN_POWER[1] = abs(rev); +} + +// Set distance to slew for +void Drive::set_slew_distance(int fwd, int rev) { + SLEW_DISTANCE[0] = abs(fwd); + SLEW_DISTANCE[1] = abs(rev); +} + +// Initialize slew +void Drive::slew_initialize(slew_ &input, bool slew_on, double max_speed, double target, double current, double start, bool backwards) { + input.enabled = slew_on; + input.max_speed = max_speed; + + input.sign = util::signum(target - current); + input.x_intercept = start + ((SLEW_DISTANCE[backwards] * input.sign) * TICK_PER_INCH); + input.y_intercept = max_speed * input.sign; + input.slope = ((input.sign * SLEW_MIN_POWER[backwards]) - input.y_intercept) / (input.x_intercept - 0 - start); // y2-y1 / x2-x1 +} + +// Slew calculation +double Drive::slew_calculate(slew_ &input, double current) { + // 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 (util::signum(input.error) != input.sign) + input.enabled = false; + + // Return y=mx+b + else if (util::signum(input.error) == input.sign) + return ((input.slope * input.error) + input.y_intercept) * input.sign; + } + // When slew is completed, return max speed + return max_speed; +} diff --git a/CLOUDS/src/ary-lib/drive/user_input.cpp b/CLOUDS/src/ary-lib/drive/user_input.cpp new file mode 100644 index 0000000..43ec08a --- /dev/null +++ b/CLOUDS/src/ary-lib/drive/user_input.cpp @@ -0,0 +1,247 @@ +#include "main.h" + +double JOYSTICK_DRIVE_SCALE = 1; +double JOYSTICK_TURN_SCALE = 1; + +double cata_active_brake_kp = 0.1; + +void Drive::save_l_curve_sd() { + return; +} + +void Drive::save_r_curve_sd() { + return; +} + +// Set curve defaults +void Drive::set_curve_default(double left, double right) { + left_curve_scale = left; + right_curve_scale = right; + + save_l_curve_sd(); + save_r_curve_sd(); +} + +void Drive::set_left_curve_buttons(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase) { + l_increase_.button = increase; + l_decrease_.button = decrease; +} +void Drive::set_right_curve_buttons(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase) { + r_increase_.button = increase; + r_decrease_.button = decrease; +} + +void Drive::set_curve_buttons(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase) { + l_increase_.button = increase; + l_decrease_.button = decrease; + r_increase_.button = increase; + r_decrease_.button = decrease; +} + +// Increase / decrease left and right curves +void Drive::l_increase() { left_curve_scale += 0.1; } +void Drive::l_decrease() { + left_curve_scale -= 0.1; + left_curve_scale = left_curve_scale < 0 ? 0 : left_curve_scale; +} +void Drive::r_increase() { right_curve_scale += 0.1; } +void Drive::r_decrease() { + right_curve_scale -= 0.1; + right_curve_scale = right_curve_scale < 0 ? 0 : right_curve_scale; +} + +// Button press logic for increase/decrease curves +void Drive::button_press(button_* input_name, int button, std::function change_curve, std::function save) { + // If button is pressed, increase the curve and set toggles. + if (button && !input_name->lock) { + change_curve(); + input_name->lock = true; + input_name->release_reset = true; + } + + // If the button is still held, check if it's held for 500ms. + // Then, increase the curve every 100ms by 0.1 + else if (button && input_name->lock) { + input_name->hold_timer += ary::util::DELAY_TIME; + if (input_name->hold_timer > 500.0) { + input_name->increase_timer += ary::util::DELAY_TIME; + if (input_name->increase_timer > 100.0) { + change_curve(); + input_name->increase_timer = 0; + } + } + } + + // When button is released for 250ms, save the new curve value to the SD card + else if (!button) { + input_name->lock = false; + input_name->hold_timer = 0; + + if (input_name->release_reset) { + input_name->release_timer += ary::util::DELAY_TIME; + if (input_name->release_timer > 250.0) { + save(); + input_name->release_timer = 0; + input_name->release_reset = false; + } + } + } +} + +// Toggle modifying curves with controller +void Drive::toggle_modify_curve_with_controller(bool toggle) { disable_controller = toggle; } + +// Modify curves with button presses and display them to contrller +void Drive::modify_curve_with_controller() { + if (!disable_controller) return; // True enables, false disables. + + button_press(&l_increase_, master.get_digital(l_increase_.button), ([this] { this->l_increase(); }), ([this] { this->save_l_curve_sd(); })); + button_press(&l_decrease_, master.get_digital(l_decrease_.button), ([this] { this->l_decrease(); }), ([this] { this->save_l_curve_sd(); })); + if (!is_tank) { + button_press(&r_increase_, master.get_digital(r_increase_.button), ([this] { this->r_increase(); }), ([this] { this->save_r_curve_sd(); })); + button_press(&r_decrease_, master.get_digital(r_decrease_.button), ([this] { this->r_decrease(); }), ([this] { this->save_r_curve_sd(); })); + } + + auto sr = std::to_string(right_curve_scale); + auto sl = std::to_string(left_curve_scale); + if (!is_tank) + master.set_text(2, 0, sl + " " + sr); + else + master.set_text(2, 0, sl); +} + +// Left curve function +double Drive::left_curve_function(double x) { + if (left_curve_scale != 0) { + // if (CURVE_TYPE) + return (powf(2.718, -(left_curve_scale / 10)) + powf(2.718, (fabs(x) - 127) / 10) * (1 - powf(2.718, -(left_curve_scale / 10)))) * x; + // else + // return powf(2.718, ((abs(x)-127)*RIGHT_CURVE_SCALE)/100)*x; + } + return x; +} + +// Right curve fnuction +double Drive::right_curve_function(double x) { + if (right_curve_scale != 0) { + // if (CURVE_TYPE) + return (powf(2.718, -(right_curve_scale / 10)) + powf(2.718, (fabs(x) - 127) / 10) * (1 - powf(2.718, -(right_curve_scale / 10)))) * x; + // else + // return powf(2.718, ((abs(x)-127)*RIGHT_CURVE_SCALE)/100)*x; + } + return x; +} + +// Set active brake constant +void Drive::set_active_brake(double kp) { active_brake_kp = kp; } + +void Drive::set_cata_active_brake(double kp) { active_brake_kp = kp; } + +double Drive::get_cata_active_brake() { return cata_active_brake_kp; } + +// Set joystick threshold +void Drive::set_joystick_threshold(int threshold) { JOYSTICK_THRESHOLD = abs(threshold); } + +void Drive::reset_drive_sensors_opcontrol() { + if (util::AUTON_RAN) { + reset_drive_sensor(); + util::AUTON_RAN = false; + } +} + +void Drive::set_joystick_drivescale(double scaleval) { JOYSTICK_DRIVE_SCALE = scaleval; } + +void Drive::set_joystick_turnscale(double scaleval) { JOYSTICK_TURN_SCALE = scaleval; } + +void Drive::joy_thresh_opcontrol(int l_stick, int r_stick) { + // Threshold if joysticks don't come back to perfect 0 + if (abs(l_stick) > JOYSTICK_THRESHOLD || abs(r_stick) > JOYSTICK_THRESHOLD) { + set_tank(l_stick, r_stick); + if (active_brake_kp != 0) reset_drive_sensor(); + } + // When joys are released, run active brake (P) on drive + else { + set_tank((0 - left_sensor()) * active_brake_kp, (0 - right_sensor()) * active_brake_kp); + } +} + +// Tank control +void Drive::tank_control() { + is_tank = true; + reset_drive_sensors_opcontrol(); + + // Toggle for controller curve + modify_curve_with_controller(); + + // Put the joysticks through the curve function + int l_stick = left_curve_function(master.get_analog(ANALOG_LEFT_Y)); + int r_stick = left_curve_function(master.get_analog(ANALOG_RIGHT_Y)); + + // Set robot to l_stick and r_stick, check joystick threshold, set active brake + joy_thresh_opcontrol(l_stick, r_stick); +} + +// Arcade standarddouble JOYSTICK_DRIVE_SCALE = 1; + +// double JOYSTICK_DRIVE_SCALE = 1; +// double JOYSTICK_TURN_SCALE = 0.6; +void Drive::arcade_standard(e_type stick_type, e_curve_type curve_type) { + is_tank = false; + reset_drive_sensors_opcontrol(); + + // Toggle for controller curve + modify_curve_with_controller(); + + double fwd_stick, turn_stick; + // Check arcade type (split vs single, normal vs flipped) + if (stick_type == SPLIT) { + // Put the joysticks through the curve function + fwd_stick = left_curve_function(master.get_analog(ANALOG_LEFT_Y)); + turn_stick = right_curve_function(master.get_analog(ANALOG_RIGHT_X)); + } else if (stick_type == SINGLE) { + // Put the joysticks through the curve function + if (curve_type == DEFAULT) { + fwd_stick = left_curve_function(master.get_analog(ANALOG_LEFT_Y)); + turn_stick = right_curve_function(master.get_analog(ANALOG_LEFT_X)); + } else if (curve_type == LOGARITHMIC) { + //fwd_stick = (master.get_analog(ANALOG_LEFT_Y) >= 1) ? 10 + } else if (curve_type == SQRT) { + + } else if (curve_type == SINE) { + + } else { + std::cout << "bruh it no work" << std::endl; + } + + + fwd_stick *= JOYSTICK_DRIVE_SCALE; + turn_stick *= JOYSTICK_TURN_SCALE; + } + + // Set robot to l_stick and r_stick, check joystick threshold, set active brake + joy_thresh_opcontrol(fwd_stick + turn_stick, fwd_stick - turn_stick); +} + +// Arcade control flipped +void Drive::arcade_flipped(e_type stick_type) { + is_tank = false; + reset_drive_sensors_opcontrol(); + + // Toggle for controller curve + modify_curve_with_controller(); + + int turn_stick, fwd_stick; + // Check arcade type (split vs single, normal vs flipped) + if (stick_type == SPLIT) { + // Put the joysticks through the curve function + fwd_stick = right_curve_function(master.get_analog(ANALOG_RIGHT_Y)); + turn_stick = left_curve_function(master.get_analog(ANALOG_LEFT_X)); + } else if (stick_type == SINGLE) { + // Put the joysticks through the curve function + fwd_stick = right_curve_function(master.get_analog(ANALOG_RIGHT_Y)); + turn_stick = left_curve_function(master.get_analog(ANALOG_RIGHT_X)); + } + + // Set robot to l_stick and r_stick, check joystick threshold, set active brake + joy_thresh_opcontrol(fwd_stick + turn_stick, fwd_stick - turn_stick); +} diff --git a/CLOUDS/src/ary-lib/util.cpp b/CLOUDS/src/ary-lib/util.cpp new file mode 100644 index 0000000..812184a --- /dev/null +++ b/CLOUDS/src/ary-lib/util.cpp @@ -0,0 +1,184 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#include "main.h" + +pros::Controller master(pros::E_CONTROLLER_MASTER); + +namespace ary { +int mode = DISABLE; + +void printScr() { + std::cout << R"( + + + _____ ______ _____ _ _ + | ___|___ / |_ _| | | | | + | |__ / /_____| | ___ _ __ ___ _ __ | | __ _| |_ ___ + | __| / /______| |/ _ \ '_ ` _ \| '_ \| |/ _` | __/ _ \ + | |___./ /___ | | __/ | | | | | |_) | | (_| | || __/ + \____/\_____/ \_/\___|_| |_| |_| .__/|_|\__,_|\__\___| + | | + |_| +)" << '\n'; + + printf("Version: 2.1.1\n"); +} +std::string get_last_word(std::string text) { + std::string word = ""; + for (int i = text.length() - 1; i >= 0; i--) { + if (text[i] != ' ') { + word += text[i]; + } else { + std::reverse(word.begin(), word.end()); + return word; + } + } + std::reverse(word.begin(), word.end()); + return word; +} +std::string get_rest_of_the_word(std::string text, int position) { + std::string word = ""; + for (int i = position; i < text.length(); i++) { + if (text[i] != ' ' && text[i] != '\n') { + word += text[i]; + } else { + return word; + } + } + return word; +} +//All iance\n\nWE WIN THESE!!!!! +void print_to_screen(std::string text, int line) { + int CurrAutoLine = line; + std::vector texts = {}; + std::string temp = ""; + + for (int i = 0; i < text.length(); i++) { + if (text[i] != '\n' && temp.length() + 1 > 32) { + auto last_word = get_last_word(temp); + if (last_word == temp) { + texts.push_back(temp); + temp = text[i]; + } else { + int size = last_word.length(); + + auto rest_of_word = get_rest_of_the_word(text, i); + temp.erase(temp.length() - size, size); + texts.push_back(temp); + last_word += rest_of_word; + i += rest_of_word.length(); + temp = last_word; + if (i >= text.length() - 1) { + texts.push_back(temp); + break; + } + + } + } + if (i >= text.length() - 1) { + temp += text[i]; + texts.push_back(temp); + temp = ""; + break; + } else if (text[i] == '\n') { + texts.push_back(temp); + temp = ""; + } else { + temp += text[i]; + } + } + for (auto i : texts) { + if (CurrAutoLine > 7) { + pros::lcd::clear(); + pros::lcd::set_text(line, "Out of Bounds. Print Line is too far down"); + return; + } + pros::lcd::clear_line(CurrAutoLine); + pros::lcd::set_text(CurrAutoLine, i); + CurrAutoLine++; + } +} + +std::string exit_to_string(exit_output input) { + switch ((int)input) { + case RUNNING: + return "Running"; + case SMALL_EXIT: + return "Small"; + case BIG_EXIT: + return "Big"; + case VELOCITY_EXIT: + return "Velocity"; + case mA_EXIT: + return "mA"; + case ERROR_NO_CONSTANTS: + return "Error: Exit condition constants not set!"; + default: + return "Error: Out of bounds!"; + } + + return "Error: Out of bounds!"; +} +namespace util { +bool AUTON_RAN = true; + +bool is_reversed(double input) { + if (input < 0) return true; + return false; +} + +int signum(double input) { + if (input > 0) + return 1; + else if (input < 0) + return -1; + return 0; +} + +double clip_num(double input, double max, double min) { + if (input > max) + return max; + else if (input < min) + return min; + return input; +} + +std::vector trapezoidalMotionProfile(double dist, double maxVel, double accel, double decel) { + double max = std::min(std::sqrt((2 * accel * decel * dist) / accel + decel), maxVel); + double accelTime = max / accel; + double decelTime = max / decel; + double coastDist = (dist / max) - (max / (2 * accel)) - (max / (2 * decel)); + double coastTime = coastDist / max; + double totalTime = accelTime + decelTime + coastTime; + double vel = 0; + double diff; + std::vector profile; + + for (int i = 0; i < std::ceil(totalTime); i++) + { + if (i < std::floor(accelTime)) + { + profile.push_back(vel); + vel += accel; + } + + else if (i < coastTime + accelTime) + { + profile.push_back(max); + } + + else + { + profile.push_back(vel); + vel -= decel; + } + } + return profile; +} + +} // namespace util +} // namespace ary