reimplement and clean superstructure
This commit is contained in:
ary
parent
8a8d40af6f
commit
de8221fa33
65
CLOUDS/include/superstructure.hpp
Normal file
65
CLOUDS/include/superstructure.hpp
Normal 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
239
CLOUDS/src/superstructure.cpp
Normal 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);
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user