From de8221fa33f0d165685b1c47f62c5fb4477f9c5b Mon Sep 17 00:00:00 2001 From: ary Date: Fri, 1 Dec 2023 09:06:13 -0500 Subject: [PATCH] reimplement and clean superstructure --- CLOUDS/include/superstructure.hpp | 65 ++++++++ CLOUDS/src/superstructure.cpp | 239 ++++++++++++++++++++++++++++++ 2 files changed, 304 insertions(+) create mode 100644 CLOUDS/include/superstructure.hpp create mode 100644 CLOUDS/src/superstructure.cpp diff --git a/CLOUDS/include/superstructure.hpp b/CLOUDS/include/superstructure.hpp new file mode 100644 index 0000000..ed5f373 --- /dev/null +++ b/CLOUDS/include/superstructure.hpp @@ -0,0 +1,65 @@ +#include "main.h" + +#define DRIVE_SPEED 120 +#define TURN_SPEED 105 +#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_INTAKE DIGITAL_L1 +#define RENU_INTAKE_CONTROL_OUTTAKE DIGITAL_B +#define RENU_LEFT_WING_CONTROL DIGITAL_LEFT +#define RENU_RIGHT_WING_CONTROL DIGITAL_RIGHT +#define RENU_WING_CONTROL DIGITAL_L2 +#define RENU_CLIMB_CONTROL DIGITAL_A + +// 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 opControlInit(); + void configureExitConditions(); + void configureConstants(); + void autonomousResets(); + void motorsCoast(); + void motorsHold(); + void motorsBrake(); + + void disableActiveBrake(); + + /* Motion */ + 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 */ + void togglePto(bool toggle); + void runCata(double inpt); + void runAntiBlock(double inpt); + void subsysControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton, pros::controller_digital_e_t intakeButton, pros::controller_digital_e_t outtakeButton); + void wingsControl(pros::controller_digital_e_t wingControlButton); + void climbControl(pros::controller_digital_e_t climbButton); + + /* Controls */ + void renu_control(); + void ria_control(); + void chris_control(); + +} \ No newline at end of file diff --git a/CLOUDS/src/superstructure.cpp b/CLOUDS/src/superstructure.cpp new file mode 100644 index 0000000..bdeff46 --- /dev/null +++ b/CLOUDS/src/superstructure.cpp @@ -0,0 +1,239 @@ +#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); + } + + 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.5, 0, 5, 0); + chassis.set_pid_constants(&chassis.backward_drivePID, 0.5, 0, 5, 0); + chassis.set_pid_constants(&chassis.turnPID, 6.25, 0.003, 57, 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_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_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_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_turn(theta, TURN_SPEED * turnScale); + chassis.wait_drive(); + } + + // Turns the chassis, runs other commands immediately after call + void turnAsync(double theta) { + 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_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_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 togglePto(bool toggle) { + ptoEnabled = toggle; + chassis.pto_toggle({intake_mtr, cata_mtr}, toggle); // Configure the listed PTO motors to whatever value toggle is. + pto_piston.set_value(toggle); + + if (toggle) { + intake_mtr.set_brake_mode(MOTOR_BRAKE_COAST); + cata_mtr.set_brake_mode(MOTOR_BRAKE_COAST); + } + } + + void runCata(double inpt) { + if (!ptoEnabled) return; + cata_mtr = inpt; + } + + void runIntake(double inpt) { + if (!ptoEnabled) return; + intake_mtr = inpt; + } + + int lock = 0; + void subsysControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton, pros::controller_digital_e_t intakeButton, pros::controller_digital_e_t outtakeButton) { + 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); + } + + if (globals::master.get_digital(intakeButton)) { + runIntake(-12000); + } else if (globals::master.get_digital(outtakeButton)) { + runIntake(12000); + } else { + runIntake(-25); + } + } + + int climb_state = 0; + void climbControl(pros::controller_digital_e_t climbButton) { + if (globals::master.get_digital_new_press(climbButton)) { + if (climb_state == 0) { + climb_piston.set_value(1); + climb_state = 1; + } else if (climb_state == 1) { + climb_piston.set_value(0); + climb_state = 0; + } + } + } + + void wingsControl(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); + } + + /* + Controls -> For whoever is controlling the robot + */ + void renu_control() { + subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE); + wingsControl(RENU_WING_CONTROL); + climbControl(RENU_CLIMB_CONTROL); + } + + void ria_control() { + subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE); + wingsControl(RENU_WING_CONTROL); + climbControl(RENU_CLIMB_CONTROL); + } + + void chris_control() { + subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE); + wingsControl(RENU_WING_CONTROL); + } +} \ No newline at end of file