That-One-Nerd/ary-over-under
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add ary lib

Browse Source
This commit is contained in:
ary 2023-12-01 08:31:01 -05:00
parent 855a171d9c
commit 1387d2a289
19 changed files with 2900 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
CLOUDS/include/ary-lib/PID.hpp Normal file
View File

@ -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<pros::Motor> 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);
};

14
CLOUDS/include/ary-lib/api.hpp Normal file
View File

@ -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"

19
CLOUDS/include/ary-lib/auton.hpp Normal file
View File

@ -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 <functional>
#include <iostream>
class Auton {
public:
Auton();
Auton(std::string, std::function<void()>);
std::string Name;
std::function<void()> auton_call;
private:
};

23
CLOUDS/include/ary-lib/auton_selector.hpp Normal file
View File

@ -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 <tuple>
#include "ary-lib/auton.hpp"
using namespace std;
class AutonSelector {
public:
std::vector<Auton> Autons;
int current_auton_page;
int auton_count;
AutonSelector();
AutonSelector(std::vector<Auton> autons);
void call_selected_auton();
void print_selected_auton();
void add_autons(std::vector<Auton> autons);
};

64
CLOUDS/include/ary-lib/autonselector.hpp Normal file
View File

@ -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();
}
}

856
CLOUDS/include/ary-lib/drive/drive.hpp Normal file
View File

@ -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 <functional>
#include <iostream>
#include <tuple>
#include <cmath>
#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<pros::Motor> left_motors;
/**
* Vector of pros motors for the right chassis.
*/
std::vector<pros::Motor> right_motors;
/**
* Vector of pros motors that are disconnected from the drive.
*/
std::vector<int> 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<int> left_motor_ports, std::vector<int> 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<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector<int> left_tracker_ports, std::vector<int> 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<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector<int> left_tracker_ports, std::vector<int> 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<int> left_motor_ports, std::vector<int> 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<pros::Motor> 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<pros::Motor> 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<pros::Motor> 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<void()> changeCurve, std::function<void()> 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();
};

118
CLOUDS/include/ary-lib/util.hpp Normal file
View File

@ -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 <bits/stdc++.h>
#include <stdio.h>
#include <string.h>
#include <vector>
#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<double> 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

194
CLOUDS/src/ary-lib/PID.cpp Normal file
View File

@ -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<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
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);
}

16
CLOUDS/src/ary-lib/auton.cpp Normal file
View File

@ -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<void()> callback) {
Name = name;
auton_call = callback;
}

38
CLOUDS/src/ary-lib/auton_selector.cpp Normal file
View File

@ -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<Auton> 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<Auton> autons) {
auton_count += autons.size();
current_auton_page = 0;
Autons.assign(autons.begin(), autons.end());
}

78
CLOUDS/src/ary-lib/autonselector.cpp Normal file
View File

@ -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 <filesystem>
#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);
}
}
}

341
CLOUDS/src/ary-lib/drive/drive.cpp Normal file
View File

@ -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 <list>
#include "main.h"
#include "pros/llemu.hpp"
#include "pros/screen.hpp"
using namespace ary;
// Constructor for integrated encoders
Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> 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<int> left_motor_ports, std::vector<int> right_motor_ports,
int imu_port, double wheel_diameter, double ticks, double ratio,
std::vector<int> left_tracker_ports, std::vector<int> 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<int> left_motor_ports, std::vector<int> right_motor_ports,
int imu_port, double wheel_diameter, double ticks, double ratio,
std::vector<int> left_tracker_ports, std::vector<int> 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<int> left_motor_ports, std::vector<int> 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; }

185
CLOUDS/src/ary-lib/drive/exit_conditions.cpp Normal file
View File

@ -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);
}
}
}

98
CLOUDS/src/ary-lib/drive/pid_tasks.cpp Normal file
View File

@ -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);
}
}

54
CLOUDS/src/ary-lib/drive/pto.cpp Normal file
View File

@ -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 <algorithm>
#include <vector>
#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<pros::Motor> 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<pros::Motor> 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<pros::Motor> pto_list, bool toggle) {
if (toggle)
pto_add(pto_list);
else
pto_remove(pto_list);
}

136
CLOUDS/src/ary-lib/drive/set_pid.cpp Normal file
View File

@ -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<double> 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);
}

51
CLOUDS/src/ary-lib/drive/slew.cpp Normal file
View File

@ -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;
}

247
CLOUDS/src/ary-lib/drive/user_input.cpp Normal file
View File

@ -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<void()> change_curve, std::function<void()> 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);
}

184
CLOUDS/src/ary-lib/util.cpp Normal file
View File

@ -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<string> 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<double> 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<double> 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