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

rewrite the entire bot - heaven on earth

Browse Source
This commit is contained in:
ary 2023-11-05 18:58:57 -05:00
parent 62359640ce
commit 2caf8a543a
45 changed files with 5097 additions and 382 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
HVN-ON-EARTH/LemLib@0.4.7.zip Normal file
View File

Binary file not shown.

BIN
HVN-ON-EARTH/gif-pros-asset@1.0.1.zip Normal file
View File

Binary file not shown.

22
HVN-ON-EARTH/include/Wings.h Normal file
View File

@ -0,0 +1,22 @@
#pragma once
#ifndef _Wings_h
#define _Wings_h_
#include "main.h"
class Wings {
public:
Wings();
void open();
void close();
void toggleLeft(int value);
void toggleRight(int value);
void openFor(double duration);
int getState();
private:
int left_wing_state;
int right_wing_state;
};
#endif

2
HVN-ON-EARTH/include/api.h
View File

@ -77,4 +77,6 @@
#include "pros/link.hpp"
#endif
#include "asset.h"
#endif // _PROS_API_H_

184
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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

88
HVN-ON-EARTH/include/asset.h Normal file
View File

@ -0,0 +1,88 @@
#pragma once
#ifndef _ASSET_H_
#define _ASSET_H_
#include "api.h"
#ifdef __cplusplus
extern "C" {
#endif
#ifndef V5_API_H_
typedef struct __attribute__((__packed__)) _v5_image {
uint16_t width;
uint16_t height;
uint32_t *data;
uint32_t *p;
} v5_image;
uint32_t vexImageBmpRead(const uint8_t *ibuf, v5_image *oBuf, uint32_t maxw, uint32_t maxh);
uint32_t vexImagePngRead(const uint8_t *ibuf, v5_image *oBuf, uint32_t maxw, uint32_t maxh, uint32_t ibuflen);
void vexDisplayCopyRect(int32_t x1, int32_t y1, int32_t x2, int32_t y2, uint32_t *pSrc, int32_t srcStride);
#endif
typedef struct __attribute__((__packed__)) _asset {
uint8_t *buf;
size_t size;
} asset;
#ifdef __cplusplus
}
#endif
#ifdef __cplusplus
#define ASSET(x) \
extern "C" { \
extern uint8_t _binary_static_##x##_start[], _binary_static_##x##_size[]; \
static asset x = {_binary_static_##x##_start, (size_t)_binary_static_##x##_size}; \
}
#else
#define ASSET(x) \
extern uint8_t _binary_static_##x##_start[], _binary_static_##x##_size[]; \
static asset x = {_binary_static_##x##_start, (size_t)_binary_static_##x##_size};
#endif
#define DECODE_PNG(var, asset, width, height) \
var.data = new uint32_t[width * height]; \
vexImagePngRead(asset.buf, &var, width, height, asset.size);
#define DECODE_BMP(var, asset, width, height) \
var.data = new uint32_t[width * height]; \
vexImageBmpRead(asset.buf, &var, width, height);
static void draw_image(int x, int y, v5_image *image) {
int x2 = x + image->width - 1;
int y2 = y + image->height - 1;
vexDisplayCopyRect(x, y, x2, y2, image->data, x2 - x + 1);
}
static void draw_image(lv_obj_t *canvas, int x, int y, v5_image *image) {
int start_x = x;
int start_y = y;
for (int i = 0; i < image->height; i++) {
for (int j = 0; j < image->width; j++) {
lv_color_t c = lv_color_hex(image->data[i * image->width + j]);
lv_color_t d = lv_canvas_get_px(canvas, start_x + j, start_y + i);
float a_float = c.alpha / 2.55 / 100;
float a_m_float = 1 - a_float;
d.red = d.red * a_m_float + c.red * a_float;
d.green = d.green * a_m_float + c.green * a_float;
d.blue = d.blue * a_m_float + c.blue * a_float;
lv_canvas_set_px(canvas, start_x + j, start_y + i, d);
}
}
}
#endif // _ASSET_H_

12
HVN-ON-EARTH/include/autons.hpp Normal file
View File

@ -0,0 +1,12 @@
#pragma once
#include "ary-lib/drive/drive.hpp"
void near_side();
void far_side();
void skills();
void test_seq();
void odom_test();
void default_constants();
void exit_condition_defaults();

69
HVN-ON-EARTH/include/gif-pros/gifclass.hpp Normal file
View File

@ -0,0 +1,69 @@
#pragma once
#include "main.h"
#include "gifdec.h"
/**
* MIT License
* Copyright (c) 2019 Theo Lemay
* https://github.com/theol0403/gif-pros
*/
class Gif {
public:
/**
* Construct the Gif class
* @param fname the gif filename on the SD card (prefixed with /usd/)
* @param parent the LVGL parent object
*/
Gif(const asset file, lv_obj_t* parent);
/**
* Destructs and cleans the Gif class
*/
~Gif();
/**
* Pauses the GIF task
*/
void pause();
/**
* Resumes the GIF task
*/
void resume();
/**
* Deletes GIF and frees all allocated memory
*/
void clean();
private:
gd_GIF* _gif = nullptr; // gif decoder object
void* _gifmem = nullptr; // gif file loaded from SD into memory
uint8_t* _buffer = nullptr; // decoder frame buffer
lv_color_t* _cbuf = nullptr; // canvas buffer
lv_obj_t* _canvas = nullptr; // canvas object
pros::task_t _task = nullptr; // render task
/**
* Cleans and frees all allocated memory
*/
void _cleanup();
/**
* Render cycle, blocks until loop count exceeds gif loop count flag (if any)
*/
void _render();
/**
* Calls _render()
* @param arg Gif*
*/
static void _render_task(void* arg);
};

58
HVN-ON-EARTH/include/gif-pros/gifdec.h Normal file
View File

@ -0,0 +1,58 @@
#ifndef GIFDEC_H
#define GIFDEC_H
#include <stdio.h>
#include <stdint.h>
#include <sys/types.h>
#ifdef __cplusplus
extern "C" {
#endif
#define BYTES_PER_PIXEL 4
typedef struct gd_Palette {
int size;
uint8_t colors[0x100 * 3];
} gd_Palette;
typedef struct gd_GCE {
uint16_t delay;
uint8_t tindex;
uint8_t disposal;
int input;
int transparency;
} gd_GCE;
typedef struct gd_GIF {
FILE *fp;
off_t anim_start;
uint16_t width, height;
uint16_t depth;
uint16_t loop_count;
gd_GCE gce;
gd_Palette *palette;
gd_Palette lct, gct;
void (*plain_text)(
struct gd_GIF *gif, uint16_t tx, uint16_t ty,
uint16_t tw, uint16_t th, uint8_t cw, uint8_t ch,
uint8_t fg, uint8_t bg
);
void (*comment)(struct gd_GIF *gif);
void (*application)(struct gd_GIF *gif, char id[8], char auth[3]);
uint16_t fx, fy, fw, fh;
uint8_t bgindex;
uint8_t *canvas, *frame;
} gd_GIF;
gd_GIF *gd_open_gif(FILE *fp);
int gd_get_frame(gd_GIF *gif);
void gd_render_frame(gd_GIF *gif, uint8_t *buffer);
void gd_rewind(gd_GIF *gif);
void gd_close_gif(gd_GIF *gif);
#ifdef __cplusplus
}
#endif
#endif /* GIFDEC_H */

66
HVN-ON-EARTH/include/globals.hpp Normal file
View File

@ -0,0 +1,66 @@
#pragma once
#include "main.h"
#include "Wings.h"
#define TRACK_WIDTH 11.5
#define PLACEHOLDER_TC_OFFSET 2.5
#define WHEEL_SIZE 2.75
#define DRIVE_RATIO 0.75
#define DRIVE_RPM 450
namespace globals {
extern pros::Controller master;
extern pros::Motor motor_tlf;
extern pros::Motor motor_tlb;
extern pros::Motor motor_blf;
extern pros::Motor motor_blb;
extern pros::Motor motor_trf;
extern pros::Motor motor_trb;
extern pros::Motor motor_brf;
extern pros::Motor motor_brb;
extern pros::Motor_Group left_drive;
extern pros::Motor_Group right_drive;
extern lemlib::Drivetrain_t dt_odom;
extern lemlib::OdomSensors_t chassis_sensors;
extern lemlib::ChassisController_t latController;
extern lemlib::ChassisController_t angController;
extern lemlib::Chassis chassis_odom;
extern pros::Rotation rot_vert;
extern pros::Rotation rot_horiz;
extern pros::Rotation enc_theta;
extern pros::ADIDigitalOut pto_piston;
extern pros::ADIDigitalOut left_wing_piston;
extern pros::ADIDigitalOut right_wing_piston;
extern pros::ADIDigitalOut intake_piston;
extern pros::ADIDigitalOut doinker_piston;
extern Wings wings;
extern lemlib::TrackingWheel vert_tracking_wheel;
extern lemlib::TrackingWheel horiz_tracking_wheel;
extern pros::Imu inertial_sensor;
extern Drive chassis;
extern pros::Motor& cata_left;
extern pros::Motor& cata_right;
enum e_controlsch {
RENU,
RIA,
CHRIS
};
}

17
HVN-ON-EARTH/include/lemlib/api.hpp Normal file
View File

@ -0,0 +1,17 @@
/**
* @file include/lemlib/api.hpp
* @author LemLib Team
* @brief LemLib API header file. Include this in your source files to use the library.
* @version 0.4.5
* @date 2023-01-27
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include "lemlib/util.hpp"
#include "lemlib/pid.hpp"
#include "lemlib/pose.hpp"
#include "lemlib/chassis/trackingWheel.hpp"
#include "lemlib/chassis/chassis.hpp"

173
HVN-ON-EARTH/include/lemlib/chassis/chassis.hpp Normal file
View File

@ -0,0 +1,173 @@
/**
* @file include/lemlib/chassis/chassis.hpp
* @author LemLib Team
* @brief Chassis class declarations
* @version 0.4.5
* @date 2023-01-23
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include "pros/motors.hpp"
#include "pros/imu.hpp"
#include "lemlib/chassis/trackingWheel.hpp"
#include "lemlib/pose.hpp"
namespace lemlib {
/**
* @brief Struct containing all the sensors used for odometry
*
* The sensors are stored in a struct so that they can be easily passed to the chassis class
* The variables are pointers so that they can be set to nullptr if they are not used
* Otherwise the chassis class would have to have a constructor for each possible combination of sensors
*
* @param vertical1 pointer to the first vertical tracking wheel
* @param vertical2 pointer to the second vertical tracking wheel
* @param horizontal1 pointer to the first horizontal tracking wheel
* @param horizontal2 pointer to the second horizontal tracking wheel
* @param imu pointer to the IMU
*/
typedef struct {
TrackingWheel* vertical1;
TrackingWheel* vertical2;
TrackingWheel* horizontal1;
TrackingWheel* horizontal2;
pros::Imu* imu;
} OdomSensors_t;
/**
* @brief Struct containing constants for a chassis controller
*
* The constants are stored in a struct so that they can be easily passed to the chassis class
* Set a constant to 0 and it will be ignored
*
* @param kP proportional constant for the chassis controller
* @param kD derivative constant for the chassis controller
* @param smallError the error at which the chassis controller will switch to a slower control loop
* @param smallErrorTimeout the time the chassis controller will wait before switching to a slower control loop
* @param largeError the error at which the chassis controller will switch to a faster control loop
* @param largeErrorTimeout the time the chassis controller will wait before switching to a faster control loop
* @param slew the maximum acceleration of the chassis controller
*/
typedef struct {
float kP;
float kD;
float smallError;
float smallErrorTimeout;
float largeError;
float largeErrorTimeout;
float slew;
} ChassisController_t;
/**
* @brief Struct containing constants for a drivetrain
*
* The constants are stored in a struct so that they can be easily passed to the chassis class
* Set a constant to 0 and it will be ignored
*
* @param leftMotors pointer to the left motors
* @param rightMotors pointer to the right motors
* @param trackWidth the track width of the robot
* @param wheelDiameter the diameter of the wheels (2.75, 3.25, 4, 4.125)
* @param rpm the rpm of the wheels
*/
typedef struct {
pros::Motor_Group* leftMotors;
pros::Motor_Group* rightMotors;
float trackWidth;
float wheelDiameter;
float rpm;
} Drivetrain_t;
/**
* @brief Chassis class
*
*/
class Chassis {
public:
/**
* @brief Construct a new Chassis
*
* @param drivetrain drivetrain to be used for the chassis
* @param lateralSettings settings for the lateral controller
* @param angularSettings settings for the angular controller
* @param sensors sensors to be used for odometry
*/
Chassis(Drivetrain_t drivetrain, ChassisController_t lateralSettings, ChassisController_t angularSettings,
OdomSensors_t sensors);
/**
* @brief Calibrate the chassis sensors
*
*/
void calibrate();
/**
* @brief Set the pose of the chassis
*
* @param x new x value
* @param y new y value
* @param theta new theta value
* @param radians true if theta is in radians, false if not. False by default
*/
void setPose(double x, double y, double theta, bool radians = false);
/**
* @brief Set the pose of the chassis
*
* @param pose the new pose
* @param radians whether pose theta is in radians (true) or not (false). false by default
*/
void setPose(Pose pose, bool radians = false);
/**
* @brief Get the pose of the chassis
*
* @param radians whether theta should be in radians (true) or degrees (false). false by default
* @return Pose
*/
Pose getPose(bool radians = false);
/**
* @brief Turn the chassis so it is facing the target point
*
* The PID logging id is "angularPID"
*
* @param x x location
* @param y y location
* @param timeout longest time the robot can spend moving
* @param reversed whether the robot should turn in the opposite direction. false by default
* @param maxSpeed the maximum speed the robot can turn at. Default is 200
* @param log whether the chassis should log the turnTo function. false by default
*/
void turnTo(float x, float y, int timeout, bool reversed = false, float maxSpeed = 127, bool log = false);
/**
* @brief Move the chassis towards the target point
*
* The PID logging ids are "angularPID" and "lateralPID"
*
* @param x x location
* @param y y location
* @param timeout longest time the robot can spend moving
* @param maxSpeed the maximum speed the robot can move at
* @param log whether the chassis should log the turnTo function. false by default
*/
void moveTo(float x, float y, int timeout, float maxSpeed = 200, bool log = false);
/**
* @brief Move the chassis along a path
*
* @param filePath file path to the path. No need to preface it with /usd/
* @param timeout the maximum time the robot can spend moving
* @param lookahead the lookahead distance. Units in inches. Larger values will make the robot move faster but
* will follow the path less accurately
* @param reverse whether the robot should follow the path in reverse. false by default
* @param maxSpeed the maximum speed the robot can move at
* @param log whether the chassis should log the path on a log file. false by default.
*/
void follow(const char* filePath, int timeout, float lookahead, bool reverse = false, float maxSpeed = 127,
bool log = false);
private:
ChassisController_t lateralSettings;
ChassisController_t angularSettings;
Drivetrain_t drivetrain;
OdomSensors_t odomSensors;
};
} // namespace lemlib

50
HVN-ON-EARTH/include/lemlib/chassis/odom.hpp Normal file
View File

@ -0,0 +1,50 @@
/**
* @file include/lemlib/chassis/odom.hpp
* @author LemLib Team
* @brief This is the header file for the odom.cpp file. Its not meant to be used directly, only through the chassis
* class
* @version 0.4.5
* @date 2023-01-23
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include "lemlib/chassis/chassis.hpp"
#include "lemlib/pose.hpp"
namespace lemlib {
/**
* @brief Set the sensors to be used for odometry
*
* @param sensors the sensors to be used
* @param drivetrain drivetrain to be used
*/
void setSensors(lemlib::OdomSensors_t sensors, lemlib::Drivetrain_t drivetrain);
/**
* @brief Get the pose of the robot
*
* @param radians true for theta in radians, false for degrees. False by default
* @return Pose
*/
Pose getPose(bool radians = false);
/**
* @brief Set the Pose of the robot
*
* @param pose the new pose
* @param radians true if theta is in radians, false if in degrees. False by default
*/
void setPose(Pose pose, bool radians = false);
/**
* @brief Update the pose of the robot
*
*/
void update();
/**
* @brief Initialize the odometry system
*
*/
void init();
} // namespace lemlib

80
HVN-ON-EARTH/include/lemlib/chassis/trackingWheel.hpp Normal file
View File

@ -0,0 +1,80 @@
/**
* @file include/lemlib/chassis/trackingWheel.hpp
* @author LemLib Team
* @brief tracking wheel class declarations
* @version 0.4.5
* @date 2023-01-23
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include "pros/motors.hpp"
#include "pros/adi.hpp"
#include "pros/rotation.hpp"
namespace lemlib {
class TrackingWheel {
public:
/**
* @brief Create a new tracking wheel
*
* @param encoder the optical shaft encoder to use
* @param diameter diameter of the tracking wheel in inches
* @param distance distance between the tracking wheel and the center of rotation in inches
* @param gearRatio gear ratio of the tracking wheel, defaults to 1
*/
TrackingWheel(pros::ADIEncoder* encoder, float diameter, float distance, float gearRatio = 1);
/**
* @brief Create a new tracking wheel
*
* @param encoder the v5 rotation sensor to use
* @param diameter diameter of the tracking wheel in inches
* @param distance distance between the tracking wheel and the center of rotation in inches
* @param gearRatio gear ratio of the tracking wheel, defaults to 1
*/
TrackingWheel(pros::Rotation* encoder, float diameter, float distance, float gearRatio = 1);
/**
* @brief Create a new tracking wheel
*
* @param motors the motor group to use
* @param diameter diameter of the drivetrain wheels in inches
* @param distance half the track width of the drivetrain in inches
* @param rpm theoretical maximum rpm of the drivetrain wheels
*/
TrackingWheel(pros::Motor_Group* motors, float diameter, float distance, float rpm);
/**
* @brief Reset the tracking wheel position to 0
*
*/
void reset();
/**
* @brief Get the distance traveled by the tracking wheel
*
* @return float distance traveled in inches
*/
float getDistanceTraveled();
/**
* @brief Get the offset of the tracking wheel from the center of rotation
*
* @return float offset in inches
*/
float getOffset();
/**
* @brief Get the type of tracking wheel
*
* @return int - 1 if motor group, 0 otherwise
*/
int getType();
private:
float diameter;
float distance;
float rpm;
pros::ADIEncoder* encoder = nullptr;
pros::Rotation* rotation = nullptr;
pros::Motor_Group* motors = nullptr;
float gearRatio = 1;
};
} // namespace lemlib

139
HVN-ON-EARTH/include/lemlib/logger.hpp Normal file
View File

@ -0,0 +1,139 @@
/**
* @file include/lemlib/logger.hpp
* @author LemLib Team
* @brief A Logger for LemLib.
* @version 0.4.5
* @date 2023-02-12
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
namespace lemlib {
static bool debug = false;
static bool verbose = false;
namespace logger {
/**
* @brief A level enumeration.
*
* Debug: Only enabled if lemlib::logger::debug is true
* Info: General information
* Warn: Warnings, usually not critical/doesn't affect the robot
* Error: Errors, usually critical and affects the robot
* Fatal: Fatal errors, crashes the program
*
* @note The log level is inclusive. For example, if the log level is set to
*/
enum class Level { DEBUG, INFO, WARN, ERROR, FATAL };
static Level lowestLevel = Level::INFO;
/**
* @brief Whether or not to log debug messages.
*
* @return true if debug is enabled
*/
bool isDebug();
/**
* @brief Sets lemlib::debug
*
* @param debug the new value
*/
void setDebug(bool debug);
/**
* @brief Whether or not to log info messages.
*
* If false, only log messages with a level of lemlib::logger::Level::WARN
* or higher will be logged
*/
bool isVerbose();
/**
* @brief Sets lemlib::verbose
*
* @param verbose the new value
*/
void setVerbose(bool verbose);
/**
* @brief The current lowest log level.
*
* @return the lowest loggable level
*/
Level getLowestLevel();
/**
* @brief Sets the lowest loggable level
*
* @param level the new lowest loggable level
*/
void setLowestLevel(Level level);
/**
* @brief Logs a message with an exception
*
* @param level the level of the message
* @param message the message
* @param exception the exception
*/
void log(Level level, const char* message, const char* exception);
/**
* @brief Logs a message
*
* @param level the level of the message
* @param message the message
*/
void log(Level level, const char* message);
/**
* @brief Logs a debug message
*
* @param message
*/
void debug(const char* message);
/**
* @brief Logs an info message
*
* @param message
*/
void info(const char* message);
/**
* @brief Logs a warning message
*
* @param message
*/
void warn(const char* message);
/**
* @brief Logs an error message
*
* @param message
* @param exception
*/
void error(const char* message, const char* exception);
/**
* @brief Logs an error message
*
* @param message
*/
void error(const char* message);
/**
* @brief Logs a fatal message
*
* @param message
* @param exception
*/
void fatal(const char* message, const char* exception);
/**
* @brief Logs a fatal message
*
* @param message
*/
void fatal(const char* message);
} // namespace logger
} // namespace lemlib

125
HVN-ON-EARTH/include/lemlib/pid.hpp Normal file
View File

@ -0,0 +1,125 @@
/**
* @file include/lemlib/pid.hpp
* @author Lemlib Team
* @brief FAPID class header
* @version 0.4.5
* @date 2023-01-15
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include <string>
#include "pros/rtos.hpp"
namespace lemlib {
/**
* @brief Feedforward, Acceleration, Proportional, Integral, Derivative PID controller
*
* The controller does not loop on its own. It must be called in a loop.
* For example: while(!controller.settled) { controller.update(input, output); }
*
*/
class FAPID {
public:
/**
* @brief Construct a new FAPID
*
* @param kF feedfoward gain, multiplied by target and added to output. Set 0 if disabled
* @param kA acceleration gain, limits the change in output. Set 0 if disabled
* @param kP proportional gain, multiplied by error and added to output
* @param kI integral gain, multiplied by total error and added to output
* @param kD derivative gain, multiplied by change in error and added to output
* @param name name of the FAPID. Used for logging
*/
FAPID(float kF, float kA, float kP, float kI, float kD, std::string name);
/**
* @brief Set gains
*
* @param kF feedfoward gain, multiplied by target and added to output. Set 0 if disabled
* @param kA acceleration gain, limits the change in output. Set 0 if disabled
* @param kP proportional gain, multiplied by error and added to output
* @param kI integral gain, multiplied by total error and added to output
* @param kD derivative gain, multiplied by change in error and added to output
*/
void setGains(float kF, float kA, float kP, float kI, float kD);
/**
* @brief Set the exit conditions
*
* @param largeError range where error is considered large
* @param smallError range where error is considered small
* @param largeTime time in milliseconds t
* @param smallTime
* @param maxTime
*/
void setExit(float largeError, float smallError, int largeTime, int smallTime, int maxTime);
/**
* @brief Update the FAPID
*
* @param target the target value
* @param position the current value
* @param log whether to check the most recent terminal input for user input. Default is false because logging
* multiple PIDs could slow down the program.
* @return float - output
*/
float update(float target, float position, bool log = false);
/**
* @brief Reset the FAPID
*/
void reset();
/**
* @brief Check if the FAPID has settled
*
* If the exit conditions have not been set, this function will always return false
*
* @return true - the FAPID has settled
* @return false - the FAPID has not settled
*/
bool settled();
/**
* @brief initialize the FAPID logging system
*
* if this function is called, std::cin will be used to interact with the FAPID
*
* the user can interact with the FAPID through the terminal
* the user can access gains and other variables with the following format:
* <name>.<variable> to get the value of the variable
* <name>.<variable>_<value> to set the value of the variable
* for example:
* pid.kP_0.5 will set the kP value to 0.5
* list of variables thats value can be set:
* kF, kA, kP, kI, kD
* list of variables that can be accessed:
* kF, kA, kP, kI, kD, totalError
* list of functions that can be called:
* reset()
*/
static void init();
private:
float kF;
float kA;
float kP;
float kI;
float kD;
float largeError;
float smallError;
int largeTime = 0;
int smallTime = 0;
int maxTime = -1; // -1 means no max time set, run forever
int largeTimeCounter = 0;
int smallTimeCounter = 0;
int startTime = 0;
float prevError = 0;
float totalError = 0;
float prevOutput = 0;
void log();
std::string name;
static std::string input;
static pros::Task* logTask;
static pros::Mutex logMutex;
};
} // namespace lemlib

95
HVN-ON-EARTH/include/lemlib/pose.hpp Normal file
View File

@ -0,0 +1,95 @@
/**
* @file include/lemlib/pose.hpp
* @author LemLib Team
* @brief Pose class declarations
* @version 0.4.5
* @date 2023-01-23
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
namespace lemlib {
class Pose {
public:
/** @brief x value*/
float x;
/** @brief y value*/
float y;
/** @brief theta value*/
float theta;
/**
* @brief Create a new pose
*
* @param x component
* @param y component
* @param theta heading. Defaults to 0
*/
Pose(float x, float y, float theta = 0);
/**
* @brief Add a pose to this pose
*
* @param other other pose
* @return Pose
*/
Pose operator+(const Pose& other);
/**
* @brief Subtract a pose from this pose
*
* @param other other pose
* @return Pose
*/
Pose operator-(const Pose& other);
/**
* @brief Multiply a pose by this pose
*
* @param other other pose
* @return Pose
*/
float operator*(const Pose& other);
/**
* @brief Multiply a pose by a float
*
* @param other float
* @return Pose
*/
Pose operator*(const float& other);
/**
* @brief Divide a pose by a float
*
* @param other float
* @return Pose
*/
Pose operator/(const float& other);
/**
* @brief Linearly interpolate between two poses
*
* @param other the other pose
* @param t t value
* @return Pose
*/
Pose lerp(Pose other, float t);
/**
* @brief Get the distance between two poses
*
* @param other the other pose
* @return float
*/
float distance(Pose other);
/**
* @brief Get the angle between two poses
*
* @param other the other pose
* @return float in radians
*/
float angle(Pose other);
/**
* @brief Rotate a pose by an angle
*
* @param angle angle in radians
* @return Pose
*/
Pose rotate(float angle);
};
} // namespace lemlib

77
HVN-ON-EARTH/include/lemlib/util.hpp Normal file
View File

@ -0,0 +1,77 @@
/**
* @file include/lemlib/util.hpp
* @author LemLib Team
* @brief Utility functions declarations
* @version 0.4.5
* @date 2023-01-15
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include <vector>
namespace lemlib {
/**
* @brief Slew rate limiter
*
* @param target target value
* @param current current value
* @param maxChange maximum change. No maximum if set to 0
* @return float - the limited value
*/
float slew(float target, float current, float maxChange);
/**
* @brief Convert radians to degrees
*
* @param rad radians
* @return float degrees
*/
float radToDeg(float rad);
/**
* @brief Convert degrees to radians
*
* @param deg degrees
* @return float radians
*/
float degToRad(float deg);
/**
* @brief Calculate the error between 2 angles. Useful when calculating the error between 2 headings
*
* @param angle1
* @param angle2
* @param radians true if angle is in radians, false if not. False by default
* @return float wrapped angle
*/
float angleError(float angle1, float angle2, bool radians = false);
/**
* @brief Return the sign of a number
*
* @param x the number to get the sign of
* @return float - -1 if negative, 1 if positive
*/
float sgn(float x);
/**
* @brief Return the average of a vector of numbers
*
* @param values
* @return float
*/
float avg(std::vector<float> values);
/**
* @brief Return the average of a vector of numbers
*
* @param values
* @return double
*/
double avg(std::vector<double> values);
} // namespace lemlib

10
HVN-ON-EARTH/include/main.h
View File

@ -39,6 +39,16 @@
/**
* You should add more #includes here
*/
#include "ary-lib/api.hpp"
#include "lemlib/api.hpp"
#include "globals.hpp"
#include "superstructure.hpp"
#include "autons.hpp"
#include "wings.h"
#include "asset.h"
#include "gif-pros/gifclass.hpp"
//#include "okapi/api.hpp"
//#include "pros/api_legacy.h"

68
HVN-ON-EARTH/include/superstructure.hpp Normal file
View File

@ -0,0 +1,68 @@
#include "main.h"
#define DRIVE_SPEED 110
#define TURN_SPEED 90
#define SWING_SPEED 90
// R1 -> WINGS, L1 -> CATA, L2 -> PTO, R2 -> INTAKE
// Renu's control preferences
#define RENU_PTO_TOGGLE DIGITAL_R2
#define RENU_CATA_CONTROL DIGITAL_R1
#define RENU_INTAKE_CONTROL DIGITAL_L1
#define RENU_LEFT_WING_CONTROL DIGITAL_LEFT
#define RENU_RIGHT_WING_CONTROL DIGITAL_RIGHT
#define RENU_WING_CONTROL DIGITAL_L2
// Ria's control preferences
#define RIA_PTO_TOGGLE DIGITAL_LEFT
#define RIA_CATA_CONTROL DIGITAL_A
#define RIA_INTAKE_CONTROL DIGITAL_R1
#define RIA_WINGS_CONTROL DIGITAL_L1
namespace superstruct {
//configs
void chassisInit();
void telemetry();
void opControlInit();
void configureExitConditions();
void configureConstants();
void autonomousResets();
void motorsCoast();
void motorsHold();
void motorsBrake();
void disableActiveBrake();
// Movement Methods
void driveAsync(double dist, bool useHeadingCorrection);
void driveSync(double dist, bool useHeadingCorrection);
void driveWithMD(double dist, bool useHeadingCorrection, double waitUntilDist);
void turnSync(double theta);
void turnAsync(double theta);
void leftSwing(double theta);
void rightSwing(double theta);
void setDriveScale(double val);
void setTurnScale(double val);
void setSwingScale(double val);
//- Structure methods
void intakeControl(pros::controller_digital_e_t intakeButton);
void togglePto();
void runCata();
void cataControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton);
void wingsControl();
void wingsControlSingle(pros::controller_digital_e_t wingControlButton);
void wingsControlComplex(pros::controller_analog_e_t leftWingButton, pros::controller_analog_e_t rightWingButton, pros::controller_analog_e_t wingButton);
//More methods
void toggleIntake(bool val);
void toggleRemovalMech(bool val);
void renu_control();
void ria_control();
void chris_control();
}

159
HVN-ON-EARTH/project.pros
View File

@ -4,6 +4,165 @@
"project_name": "HVN-ON-EARTH",
"target": "v5",
"templates": {
"LemLib": {
"location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\LemLib@0.4.7",
"metadata": {
"origin": "local"
},
"name": "LemLib",
"py/object": "pros.conductor.templates.local_template.LocalTemplate",
"supported_kernels": "^3.8.0",
"system_files": [
"firmware\\LemLib.a",
"include\\lemlib\\chassis\\odom.hpp",
"include\\lemlib\\pose.hpp",
"include\\lemlib\\util.hpp",
"include\\lemlib\\chassis\\trackingWheel.hpp",
"include\\lemlib\\pid.hpp",
"include\\lemlib\\api.hpp",
"include\\lemlib\\logger.hpp",
"include\\lemlib\\chassis\\chassis.hpp"
],
"target": "v5",
"user_files": [],
"version": "0.4.7"
},
"gif-pros-asset": {
"location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\gif-pros-asset@1.0.1",
"metadata": {
"origin": "local"
},
"name": "gif-pros-asset",
"py/object": "pros.conductor.templates.local_template.LocalTemplate",
"supported_kernels": "^3.8.0",
"system_files": [
"include\\pros\\link.h",
"include\\display\\lv_misc\\lv_ll.h",
"include\\display\\lv_objx\\lv_cb.h",
"include\\display\\lv_objx\\lv_slider.h",
"include\\display\\lv_objx\\lv_ddlist.h",
"include\\display\\lv_themes\\lv_theme_material.h",
"include\\display\\lvgl.h",
"include\\display\\lv_misc\\lv_math.h",
"include\\pros\\llemu.hpp",
"include\\display\\lv_misc\\lv_color.h",
"include\\display\\lv_draw\\lv_draw_rbasic.h",
"include\\pros\\optical.hpp",
"include\\display\\lv_core\\lv_lang.h",
"include\\pros\\llemu.h",
"include\\gif-pros\\gifdec.h",
"include\\api.h",
"include\\pros\\colors.h",
"include\\display\\lv_misc\\lv_area.h",
"include\\pros\\misc.hpp",
"include\\display\\lv_draw\\lv_draw_arc.h",
"include\\pros\\apix.h",
"include\\display\\lv_themes\\lv_theme_nemo.h",
"include\\display\\lv_themes\\lv_theme_zen.h",
"include\\display\\lv_draw\\lv_draw_img.h",
"include\\display\\lv_misc\\lv_anim.h",
"include\\pros\\screen.hpp",
"include\\display\\lv_hal\\lv_hal_indev.h",
"include\\display\\lv_draw\\lv_draw_rect.h",
"include\\display\\lv_misc\\lv_ufs.h",
"include\\display\\lv_objx\\lv_arc.h",
"include\\display\\lv_objx\\lv_calendar.h",
"include\\gif-pros\\gifclass.hpp",
"include\\display\\lv_conf.h",
"include\\pros\\error.h",
"include\\display\\lv_fonts\\lv_font_builtin.h",
"include\\pros\\vision.hpp",
"include\\pros\\ext_adi.h",
"include\\display\\lv_misc\\lv_task.h",
"include\\pros\\rotation.h",
"include\\pros\\colors.hpp",
"include\\display\\lv_version.h",
"include\\display\\lv_objx\\lv_cont.h",
"include\\display\\lv_themes\\lv_theme_mono.h",
"include\\pros\\gps.h",
"include\\pros\\motors.h",
"include\\display\\lv_core\\lv_indev.h",
"include\\display\\lv_objx\\lv_sw.h",
"include\\display\\lv_objx\\lv_ta.h",
"include\\display\\lv_misc\\lv_fs.h",
"include\\display\\lv_hal\\lv_hal.h",
"include\\display\\lv_objx\\lv_img.h",
"include\\display\\lv_objx\\lv_chart.h",
"include\\display\\lv_objx\\lv_btnm.h",
"include\\display\\lv_objx\\lv_spinbox.h",
"include\\pros\\api_legacy.h",
"include\\display\\lv_objx\\lv_btn.h",
"include\\display\\lv_objx\\lv_win.h",
"include\\display\\lv_objx\\lv_imgbtn.h",
"include\\display\\lv_objx\\lv_bar.h",
"include\\display\\lv_objx\\lv_objx_templ.h",
"include\\display\\lv_objx\\lv_line.h",
"include\\pros\\screen.h",
"include\\display\\lv_objx\\lv_label.h",
"include\\pros\\optical.h",
"include\\display\\lv_draw\\lv_draw_label.h",
"include\\pros\\imu.hpp",
"include\\pros\\link.hpp",
"include\\pros\\misc.h",
"include\\display\\lv_themes\\lv_theme_alien.h",
"include\\display\\lv_conf_checker.h",
"include\\display\\lv_core\\lv_obj.h",
"include\\pros\\distance.hpp",
"include\\display\\lv_draw\\lv_draw_triangle.h",
"include\\display\\lv_objx\\lv_preload.h",
"include\\display\\lv_objx\\lv_kb.h",
"include\\display\\lv_core\\lv_vdb.h",
"include\\display\\lv_misc\\lv_gc.h",
"include\\display\\lv_themes\\lv_theme_templ.h",
"include\\display\\lv_themes\\lv_theme_night.h",
"include\\display\\lv_misc\\lv_mem.h",
"include\\display\\lv_objx\\lv_tabview.h",
"firmware\\gif-pros-asset.a",
"include\\display\\lv_misc\\lv_symbol_def.h",
"include\\pros\\serial.hpp",
"include\\display\\lv_misc\\lv_circ.h",
"include\\display\\lv_hal\\lv_hal_tick.h",
"include\\display\\lv_draw\\lv_draw.h",
"include\\pros\\gps.hpp",
"include\\pros\\adi.h",
"include\\pros\\adi.hpp",
"include\\pros\\rotation.hpp",
"include\\display\\lv_objx\\lv_canvas.h",
"include\\pros\\motors.hpp",
"include\\display\\lv_misc\\lv_templ.h",
"include\\display\\lv_misc\\lv_font.h",
"include\\display\\lv_draw\\lv_draw_vbasic.h",
"include\\main.h",
"include\\display\\lv_draw\\lv_draw_line.h",
"include\\asset.h",
"include\\display\\lv_objx\\lv_tileview.h",
"include\\display\\lv_misc\\lv_log.h",
"include\\display\\lv_core\\lv_group.h",
"include\\display\\lv_objx\\lv_led.h",
"include\\display\\lv_core\\lv_style.h",
"include\\pros\\vision.h",
"include\\display\\lv_objx\\lv_mbox.h",
"include\\pros\\rtos.h",
"include\\display\\lv_themes\\lv_theme_default.h",
"include\\pros\\serial.h",
"include\\display\\lv_core\\lv_refr.h",
"include\\pros\\rtos.hpp",
"include\\display\\lv_objx\\lv_gauge.h",
"include\\pros\\distance.h",
"include\\display\\lv_hal\\lv_hal_disp.h",
"include\\display\\lv_objx\\lv_table.h",
"include\\display\\lv_objx\\lv_lmeter.h",
"include\\pros\\imu.h",
"include\\display\\lv_misc\\lv_txt.h",
"include\\display\\lv_objx\\lv_page.h",
"include\\display\\lv_objx\\lv_roller.h",
"include\\display\\lv_themes\\lv_theme.h",
"include\\display\\lv_objx\\lv_list.h"
],
"target": "v5",
"user_files": [],
"version": "1.0.1"
},
"kernel": {
"location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\kernel@3.8.0",
"metadata": {

76
HVN-ON-EARTH/src/Wings.cpp Normal file
View File

@ -0,0 +1,76 @@
/*
Wings.cpp
Controls all of the logic for the wings in autonomous and driver control
*/
#include "main.h"
#include "Wings.h"
using namespace globals;
Wings::Wings() {
/*
The state of each wing needs to be manually tracked due to privitization errors.
We cannot read the values of the pistons directly, in order to work around this we need to track the state manually.
*/
left_wing_state = 0;
right_wing_state = 0;
close(); // Force close the wings for safety
};
void Wings::open() {
left_wing_piston.set_value(1);
left_wing_state = 1;
right_wing_piston.set_value(1);
right_wing_state = 1;
};
void Wings::close() {
left_wing_piston.set_value(0);
left_wing_state = 0;
right_wing_piston.set_value(0);
right_wing_state = 0;
};
void Wings::toggleLeft(int value) {
left_wing_piston.set_value(value);
left_wing_state = value;
};
void Wings::toggleRight(int value) {
right_wing_piston.set_value(value);
right_wing_state = value;
};
/*
Opens the wings for a certain amount of time
openFor(double duration) -> duration in seconds, converted to milliseconds
*/
void Wings::openFor(double duration) {
open();
pros::delay(duration * 1000);
close();
};
/*
returns int
0 -> Both wings closed
1 -> Right wing open
2 -> Left wing open
3 -> Both wings open
*/
int Wings::getState() {
if (left_wing_state == 0 && right_wing_state == 0) {
return 0;
} else if (left_wing_state == 0 && right_wing_state == 1) {
return 1;
} else if (left_wing_state == 1 && right_wing_state == 0) {
return 2;
} else if (left_wing_state == 1 && right_wing_state == 1) {
return 3;
} else {
return 0;
}
}

194
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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
HVN-ON-EARTH/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

83
HVN-ON-EARTH/src/autons.cpp Normal file
View File

@ -0,0 +1,83 @@
#include "main.h"
#include <stdlib.h>
using namespace globals;
using namespace superstruct;
/*
TODO:
- AUTONS
- SameBarQual
- OppBarQual
- SameBarElims
- OppBar Elims
*/
/*
chassis.wait_drive() -> Waits for the drivetrain to complete whatever movement is happening
chassis.wait_until(double measurement) -> Waits for the drivetrain to reach a certain measurement before performing the next task.
*/
void near_side() {
// Score the triball preload
toggleIntake(true);
driveSync(100, true);
turnSync(90);
toggleIntake(false);
pros::delay(50);
driveSync(22, true);
driveSync(-22, true);
turnSync(-120);
driveSync(48.5, true);
pros::delay(150);
toggleIntake(true);
pros::delay(100);
driveSync(-35, true);
turnSync(90);
pros::delay(100);
toggleIntake(false);
driveSync(55, true);
driveSync(-20, true);
//driveSync(100, true);
}
void far_side() {
toggleIntake(true);
driveSync(100, true);
turnSync(-90);
toggleIntake(false);
pros::delay(250);
driveSync(31, true);
driveSync(-30, true);
turnSync(-160);
driveSync(125, true);
pros::delay(1000);
driveSync(-35, true);
turnSync(55);
driveSync(70, true);
}
void skills() {
}
void test_seq() {
driveSync(10, true);
turnSync(90);
turnSync(90);
}
void odom_test() {
chassis_odom.moveTo(10, 10, 1000);
chassis_odom.turnTo(15, 15, 1000);
};

95
HVN-ON-EARTH/src/globals.cpp Normal file
View File

@ -0,0 +1,95 @@
/*
globals.cpp
Contains all devices and electronics used for the robot
Holds other mechanicsm classes.
*/
#include "globals.hpp"
using namespace ary;
namespace globals {
// Chassis
pros::Controller master(CONTROLLER_MASTER);
/*
{-2, -6, 12, 5}, -> left
{-16, 1, 4, -3}, -> right
*/
pros::Motor motor_tlf(12, MOTOR_GEARSET_06, false);
pros::Motor motor_blb(6, MOTOR_GEARSET_06, true);
pros::Motor motor_blf(2, MOTOR_GEARSET_06, true);
pros::Motor motor_tlb(5, MOTOR_GEARSET_06, false);
pros::Motor motor_trf(16, MOTOR_GEARSET_06, true);
pros::Motor motor_trb(3, MOTOR_GEARSET_06, true);
pros::Motor motor_brf(1, MOTOR_GEARSET_06, false);
pros::Motor motor_brb(4, MOTOR_GEARSET_06, false);
pros::Motor_Group left_drive({ motor_tlf, motor_tlb, motor_blf, motor_blb });
pros::Motor_Group right_drive({ motor_trf, motor_trb, motor_brf, motor_brb });
// Electronics / Pneumatics / Sensors
pros::ADIDigitalOut pto_piston('A');
pros::ADIDigitalOut left_wing_piston('E');
pros::ADIDigitalOut right_wing_piston('F');
pros::ADIDigitalOut intake_piston('D');
//pros::ADIDigitalOut doinker_piston('E');
Wings wings;
pros::Imu inertial_sensor(18);
lemlib::Drivetrain_t dt_odom {
&left_drive,
&right_drive,
TRACK_WIDTH,
WHEEL_SIZE,
DRIVE_RPM
};
lemlib::OdomSensors_t chassis_sensors {
nullptr,
nullptr,
nullptr,
nullptr,
&inertial_sensor
};
lemlib::ChassisController_t latController {
0.55, // kP
5, // kD
1, // smallErrorRange
100, // smallErrorTimeout
3, // largeErrorRange
500, // largeErrorTimeout
5 // slew rate
};
lemlib::ChassisController_t angController {
6.5, // kP
35, // kD
1, // smallErrorRange
100, // smallErrorTimeout
3, // largeErrorRange
500, // largeErrorTimeout
40 // slew rate
};
Drive chassis(
{-2, -6, 12, 5},
{-16, 1, 4, -3},
18,
WHEEL_SIZE,
600,
DRIVE_RATIO
);
lemlib::Chassis chassis_odom(dt_odom, latController, angController, chassis_sensors);
pros::Motor& cata_left = chassis.left_motors[3];
pros::Motor& cata_right = chassis.right_motors[3];
}

138
HVN-ON-EARTH/src/main.cpp
View File

@ -1,20 +1,17 @@
/*
main.cpp
The entry point for the bot
*/
#include "main.h"
/**
* A callback function for LLEMU's center button.
*
* When this callback is fired, it will toggle line 2 of the LCD text between
* "I was pressed!" and nothing.
*/
void on_center_button() {
static bool pressed = false;
pressed = !pressed;
if (pressed) {
pros::lcd::set_text(2, "I was pressed!");
} else {
pros::lcd::clear_line(2);
}
}
ASSET(chip_gif)
using namespace globals;
using namespace superstruct;
e_controlsch currentuser = RENU;
/**
* Runs initialization code. This occurs as soon as the program is started.
@ -23,71 +20,72 @@ void on_center_button() {
* to keep execution time for this mode under a few seconds.
*/
void initialize() {
pros::lcd::initialize();
pros::lcd::set_text(1, "Hello PROS User!");
ary::printScr();
pros::lcd::register_btn1_cb(on_center_button);
pros::delay(500); // Give time for legacy ports configure
/*
Construct the chassis and it's constants
chassisInit() -> Adjusts drive curves, active brake constants, among other things
configureConstants() -> Sets PID constants on the chassis
configureExitConditions() -> Sets default values for PID Exit conditions
*/
chassisInit();
configureConstants();
configureExitConditions();
/*
Add autonomous paths to the auto selection
*/
ary::autonselector::auton_selector.add_autons({
Auton("Near side (close to alliance goal) \n\nTo run when near alliance goal", near_side),
Auton("Far side (far from alliance goal) \n\nTo run when far from alliance goal", far_side),
Auton("Skills \n\nPEAK!!!", skills),
Auton("TESTING TURNS", test_seq),
Auton("Testing Odometry \n\nBaltimore behavior", odom_test)
});
motorsCoast(); // Allow the motors to coast initially
chassis.initialize();
ary::autonselector::initialize();
//pros::lcd::register_btn1_cb(centerButtonCallback);
pros::Task telemetryTask(telemetry);
}
/**
* Runs while the robot is in the disabled state of Field Management System or
* the VEX Competition Switch, following either autonomous or opcontrol. When
* the robot is enabled, this task will exit.
*/
void disabled() {}
/**
* Runs after initialize(), and before autonomous when connected to the Field
* Management System or the VEX Competition Switch. This is intended for
* competition-specific initialization routines, such as an autonomous selector
* on the LCD.
*
* This task will exit when the robot is enabled and autonomous or opcontrol
* starts.
*/
void competition_initialize() {}
/**
* Runs the user autonomous code. This function will be started in its own task
* with the default priority and stack size whenever the robot is enabled via
* the Field Management System or the VEX Competition Switch in the autonomous
* mode. Alternatively, this function may be called in initialize or opcontrol
* for non-competition testing purposes.
*
* If the robot is disabled or communications is lost, the autonomous task
* will be stopped. Re-enabling the robot will restart the task, not re-start it
* from where it left off.
*/
void autonomous() {}
void autonomous() {
autonomousResets();
ary::autonselector::auton_selector.call_selected_auton();
}
/**
* Runs the operator control code. This function will be started in its own task
* with the default priority and stack size whenever the robot is enabled via
* the Field Management System or the VEX Competition Switch in the operator
* control mode.
*
* If no competition control is connected, this function will run immediately
* following initialize().
*
* If the robot is disabled or communications is lost, the
* operator control task will be stopped. Re-enabling the robot will restart the
* task, not resume it from where it left off.
*/
void opcontrol() {
pros::Controller master(pros::E_CONTROLLER_MASTER);
pros::Motor left_mtr(1);
pros::Motor right_mtr(2);
Gif gif(chip_gif, lv_scr_act());
opControlInit(); // Configure the chassis for driver control
while (true) {
pros::lcd::print(0, "%d %d %d", (pros::lcd::read_buttons() & LCD_BTN_LEFT) >> 2,
(pros::lcd::read_buttons() & LCD_BTN_CENTER) >> 1,
(pros::lcd::read_buttons() & LCD_BTN_RIGHT) >> 0);
int left = master.get_analog(ANALOG_LEFT_Y);
int right = master.get_analog(ANALOG_RIGHT_Y);
/*
Handle controls for whoever is driving the bot at the moment, each available user a respective set of controls'
Available Options:
RENU, RIA, CHRIS
*/
if (currentuser == RENU) {
chassis.tank_control();
renu_control();
} else if (currentuser == RIA) {
chassis.tank_control();
renu_control();
} else if (currentuser == CHRIS) {
chassis.arcade_standard(ary::SINGLE, ary::DEFAULT);
chris_control();
} else {
renu_control();
}
left_mtr = left;
right_mtr = right;
pros::delay(20);
pros::delay(ary::util::DELAY_TIME);
}
}

253
HVN-ON-EARTH/src/superstructure.cpp Normal file
View File

@ -0,0 +1,253 @@
#include "superstructure.hpp"
using namespace ary;
using namespace globals;
bool ptoEnabled = false;
bool wingsOpen = false;
bool intakeEngaged = false;
/*
SCALE SPEEDS: Determines what percentage speeds of autonomous movements should move at
speedScale -> The scale of how fast the drivetrain goes forward and backwards
turnScale -> The scale of how fast the drivetrain turns
swingScale -> The scale of fast one side of the chassis moves
*/
double speedScale = 1.0;
double turnScale = 1.0;
double swingScale = 1.0;
namespace superstruct {
void chassisInit() {
/*
When the robot first starts up we want to do a couple things:
- Adjust the drivetrain curve bottons so it does not interfere with any of the driver controls.
- Enable the joystick curve
- Enable active break on the drive
- Active break is a P controller applied to the drivetrain in order to help it maintain it's position, resisting external forces.
-
*/
chassis.set_curve_buttons(pros::E_CONTROLLER_DIGITAL_LEFT, pros::E_CONTROLLER_DIGITAL_RIGHT);
chassis.toggle_modify_curve_with_controller(true);
chassis.set_active_brake(0.1);
chassis.set_curve_default(0.375, 0.375);
/* Adjust the adjust the factor by which the drive velocity is adjusted */
chassis.set_joystick_drivescale(1.0);
chassis.set_joystick_turnscale(1.0);
chassis_odom.calibrate();
chassis_odom.setPose(0, 0, 0);
}
void telemetry() {
while (true) {
lemlib::Pose bot_pose = chassis_odom.getPose();
pros::lcd::print(0, "x: %f", bot_pose.x); // print the x position
pros::lcd::print(1, "y: %f", bot_pose.y); // print the y position
pros::lcd::print(2, "heading: %f", bot_pose.theta); // print the heading
pros::delay(10);
}
}
void opControlInit() {
motorsCoast();
disableActiveBrake();
}
// Adjust exit conditions to allow for quick movements
void configureExitConditions() {
chassis.set_exit_condition(chassis.turn_exit, 50, 2, 220, 3, 500, 500);
chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 500, 500);
chassis.set_exit_condition(chassis.drive_exit, 40, 80, 300, 150, 500, 500);
}
// Adjust PID constants for accurate movements
void configureConstants() {
chassis.set_slew_min_power(80, 80);
chassis.set_slew_distance(7, 7);
chassis.set_pid_constants(&chassis.headingPID, 16, 0, 32, 0);
chassis.set_pid_constants(&chassis.forward_drivePID, 0.55, 0, 5, 0);
chassis.set_pid_constants(&chassis.backward_drivePID, 0.55, 0, 5, 0);
chassis.set_pid_constants(&chassis.turnPID, 6.5, 0.003, 35, 15);
chassis.set_pid_constants(&chassis.swingPID, 8.5, 0, 50, 0);
}
// Prepare the bot for the autonomous period of a match
void autonomousResets() {
chassis.reset_pid_targets();
chassis.reset_gyro();
chassis.reset_drive_sensor();
configureConstants();
configureExitConditions();
motorsBrake();
}
void motorsCoast() {
chassis.set_drive_brake(MOTOR_BRAKE_COAST);
}
void motorsHold() {
chassis.set_drive_brake(MOTOR_BRAKE_HOLD);
}
void motorsBrake() {
chassis.set_drive_brake(MOTOR_BRAKE_BRAKE);
}
// The chassis will not apply a constant voltage to prevent it from being moved
void disableActiveBrake() {
chassis.set_active_brake(0.0);
}
// Drives forward, runs next commands WITHOUT waiting for the drive to complete
void driveAsync(double dist, bool useHeadingCorrection) {
//chassis.set_mode(ary::DRIVE);
chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection);
}
// Drives forward, runs next commands AFTER waiting for the drive to complete
void driveSync(double dist, bool useHeadingCorrection) {
//chassis.set_mode(ary::DRIVE);
chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection);
chassis.wait_drive();
}
// Drives forward, runs next commands AFTER reaching a certain measurement/error along the path
void driveWithMD(double dist, bool useHeadingCorrection, double waitUntilDist) {
//chassis.set_mode(ary::DRIVE);
chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection);
chassis.wait_until(waitUntilDist);
}
// Turns the chassis, runs other commands after it has run.
void turnSync(double theta) {
//chassis.set_mode(ary::TURN);
chassis.set_turn(theta, TURN_SPEED * turnScale);
chassis.wait_drive();
}
// Turns the chassis, runs other commands immediately after call
void turnAsync(double theta) {
//chassis.set_mode(ary::TURN);
chassis.set_turn(theta, TURN_SPEED * turnScale);
}
// Moves only the right side of the chassis so it can make a left turn
void leftSwing(double theta) {
//chassis.set_mode(SWING);
chassis.set_swing(LEFT_SWING, theta, SWING_SPEED * swingScale);
}
// Moves only the left side of the chassis so it can make a right turn
void rightSwing(double theta) {
//chassis.set_mode(SWING);
chassis.set_swing(RIGHT_SWING, theta, SWING_SPEED * swingScale);
}
/*
Each of the scale values must be clamed between 0.1 - 1 (10% to 100%) to avoid saturation of motors.
*/
void setDriveScale(double val) {
speedScale = std::clamp(val, 0.1, 1.0);
}
void setTurnScale(double val) {
turnScale = std::clamp(val, 0.1, 1.0);
}
void setSwingScale(double val) {
swingScale = std::clamp(val, 0.1, 1.0); //
}
// Structure methods
void intakeControl(pros::controller_digital_e_t intakeButton) {
if (globals::master.get_digital_new_press(intakeButton)) {
if (intakeEngaged == false) {
intake_piston.set_value(1);
intakeEngaged = true;
} else if (intakeEngaged == true) {
intake_piston.set_value(0);
intakeEngaged = false;
}
}
}
void togglePto(bool toggle) {
ptoEnabled = toggle;
chassis.pto_toggle({cata_left, cata_right}, toggle); // Configure the listed PTO motors to whatever value toggle is.
pto_piston.set_value(toggle);
if (toggle) {
cata_left.set_brake_mode(MOTOR_BRAKE_COAST);
cata_right.set_brake_mode(MOTOR_BRAKE_COAST);
}
}
void runCata(double inpt) {
if (!ptoEnabled) return;
cata_left = inpt;
cata_right = inpt;
}
int lock = 0;
void cataControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton) {
if (globals::master.get_digital(ptoToggleButton) && lock == 0) { // If the PTO button has been pressed and the PTO is not engaged
togglePto(!ptoEnabled); // Toggle the PTO so that cataput is useable
lock = 1;
} else if(!globals::master.get_digital(ptoToggleButton)) {
lock = 0;
}
if (globals::master.get_digital(cataRunButton)) {
runCata(-12000);
} else {
runCata(0);
}
}
void wingsControlSingle(pros::controller_digital_e_t wingControlButton) {
if (globals::master.get_digital_new_press(wingControlButton)) {
if (wings.getState() == 0) // A value of 0 indicates that both wings are closed
wings.open();
else if (wings.getState() == 3) // A value of 3 indicates that both wings are open
wings.close();
}
}
void toggleIntake(bool val) {
int valTo = (val == true) ? 1 : 0;
intake_piston.set_value(valTo);
}
// void toggleRemovalMech(bool val) {
// int valTo = (val == true) ? 1 : 0;
// doinker_piston.set_value(valTo);
// }
/*
Handle respective controls
*/
void renu_control() {
cataControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL);
wingsControlSingle(RENU_WING_CONTROL);
intakeControl(RENU_INTAKE_CONTROL);
}
void ria_control() {
cataControl(RIA_PTO_TOGGLE, RIA_CATA_CONTROL);
wingsControlSingle(RIA_WINGS_CONTROL);
intakeControl(RIA_INTAKE_CONTROL);
}
void chris_control() {
cataControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL);
wingsControlSingle(RENU_WING_CONTROL);
intakeControl(RENU_INTAKE_CONTROL);
}
}

BIN
HVN-ON-EARTH/static/clear.gif Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 880 KiB