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

reimplement and clean superstructure

Browse Source
This commit is contained in:
ary 2023-12-01 09:06:13 -05:00
parent 8a8d40af6f
commit de8221fa33
2 changed files with 304 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
CLOUDS/include/superstructure.hpp Normal file
View File

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

239
CLOUDS/src/superstructure.cpp Normal file
View File

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