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

lib or something

Browse Source
This commit is contained in:
ary 2023-09-04 13:22:06 -04:00
parent 4055edfeaf
commit e6d222cc31
20 changed files with 2797 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
RELENTLESS/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 compute(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 prev_error;
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
RELENTLESS/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
RELENTLESS/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
RELENTLESS/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
RELENTLESS/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();
}
}

825
RELENTLESS/include/ary-lib/drive/drive.hpp Normal file
View File

@ -0,0 +1,825 @@
/*
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"
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();
/**
* 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_pid(double target, int speed, bool slew_on = false, bool toggle_heading = true);
/**
* Sets the robot to turn using PID.
*
* \param target
* target value in degrees
* \param speed
* 0 to 127, max speed during motion
*/
void set_turn_pid(double target, int speed);
/**
* Turn using only the left or right side.
*
* \param type
* L_SWING or R_SWING
* \param target
* target value in degrees
* \param speed
* 0 to 127, max speed during motion
*/
void set_swing_pid(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();
};

114
RELENTLESS/include/ary-lib/util.hpp Normal file
View File

@ -0,0 +1,114 @@
/*
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 "api.h"
/**
* Controller.
*/
extern pros::Controller master;
namespace ary {
/**
* Prints our branding all over your pros terminal
*/
void print_ez_template();
/**
* 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 sgn(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);
/**
* 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

1
RELENTLESS/include/main.h
View File

@ -40,6 +40,7 @@
* You should add more #includes here
*/
#include "okapi/api.hpp"
#include "ary-lib/api.hpp"
//#include "pros/api_legacy.h"
/**

196
RELENTLESS/src/ary-lib/PID.cpp Normal file
View File

@ -0,0 +1,196 @@
/*
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 "util.hpp"
using namespace ary;
void PID::reset_variables() {
output = 0;
target = 0;
error = 0;
prev_error = 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::compute(double current) {
error = target - current;
derivative = error - prev_error;
if (constants.ki != 0) {
if (fabs(error) < constants.start_i)
integral += error;
if (util::sgn(error) != util::sgn(prev_error))
integral = 0;
}
output = (error * constants.kp) + (integral * constants.ki) + (derivative * constants.kd);
prev_error = 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
RELENTLESS/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
RELENTLESS/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
RELENTLESS/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);
}
}
}

333
RELENTLESS/src/ary-lib/drive/drive.cpp Normal file
View File

@ -0,0 +1,333 @@
/*
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;
}
}
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::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
RELENTLESS/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::sgn(l_error);
int r_sgn = util::sgn(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::sgn(l_error) == l_sgn || util::sgn(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::sgn(l_error) != l_sgn || util::sgn(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::sgn(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::sgn(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::sgn(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::sgn(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::sgn(g_error) != g_sgn) {
if (print_toggle) std::cout << " Swing Wait Until Exit.\n";
return;
}
}
pros::delay(util::DELAY_TIME);
}
}
}

98
RELENTLESS/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.compute(left_sensor());
rightPID.compute(right_sensor());
headingPID.compute(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.compute(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.compute(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
RELENTLESS/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);
}

114
RELENTLESS/src/ary-lib/drive/set_pid.cpp Normal file
View File

@ -0,0 +1,114 @@
/*
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_pid(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_pid(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_pid(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);
}

51
RELENTLESS/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::sgn(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::sgn(input.error) != input.sign)
input.enabled = false;
// Return y=mx+b
else if (util::sgn(input.error) == input.sign)
return ((input.slope * input.error) + input.y_intercept) * input.sign;
}
// When slew is completed, return max speed
return max_speed;
}

239
RELENTLESS/src/ary-lib/drive/user_input.cpp Normal file
View File

@ -0,0 +1,239 @@
#include "main.h"
double JOYSTICK_DRIVE_SCALE = 1;
double JOYSTICK_TURN_SCALE = 1;
double cata_active_brake_kp = 0.1;
// 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() {
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);
}

151
RELENTLESS/src/ary-lib/util.cpp Normal file
View File

@ -0,0 +1,151 @@
/*
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 print_ez_template() {
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 sgn(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;
}
} // namespace util
} // namespace ary