diff --git a/CLOUDS/include/superstructure.hpp b/CLOUDS/include/superstructure.hpp index dde2c79..57ea7db 100644 --- a/CLOUDS/include/superstructure.hpp +++ b/CLOUDS/include/superstructure.hpp @@ -3,6 +3,7 @@ #define DRIVE_SPEED 127 #define TURN_SPEED 120 #define SWING_SPEED 120 + // R1 -> WINGS, L1 -> CATA, L2 -> PTO, R2 -> INTAKE // Renu's control preferences #define RENU_PTO_TOGGLE DIGITAL_R1 @@ -30,6 +31,7 @@ namespace superstruct { void motorsCoast(); void motorsHold(); void motorsBrake(); + void controllerTelemtry(); void disableActiveBrake(); @@ -51,7 +53,7 @@ namespace superstruct { /* Structure */ void togglePto(bool toggle); void runCata(double inpt); - void runIntake(double inpt); + void setIntakeSpeed(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 actuateClimb(); diff --git a/CLOUDS/src/superstructure.cpp b/CLOUDS/src/superstructure.cpp index 868339f..5a3da00 100644 --- a/CLOUDS/src/superstructure.cpp +++ b/CLOUDS/src/superstructure.cpp @@ -16,15 +16,19 @@ double speedScale = 1.0; double turnScale = 1.0; double swingScale = 1.0; -namespace superstruct { - void chassisInit() { +double intakeSpeed = 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); @@ -32,26 +36,29 @@ namespace superstruct { chassis.set_active_brake(0.1); chassis.set_curve_default(1.075, 1.075); - /* Adjust the adjust the factor by which the drive velocity is adjusted */ + /* 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.set_joystick_turnscale(1.0); } - void opControlInit() { + void opControlInit() + { motorsCoast(); - disableActiveBrake(); + disableActiveBrake(); } // Adjust exit conditions to allow for quick movements - void configureExitConditions() { - chassis.set_exit_condition(chassis.turn_exit, 50, 2, 220, 3, 150, 500); - chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 150, 500); - chassis.set_exit_condition(chassis.drive_exit, 40, 80, 300, 150, 150, 500); + 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(100, 100); + 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); @@ -61,69 +68,103 @@ namespace superstruct { } // Prepare the bot for the autonomous period of a match - void autonomousResets() { + void autonomousResets() + { chassis.reset_pid_targets(); - chassis.reset_gyro(); - chassis.reset_drive_sensor(); + chassis.reset_gyro(); + chassis.reset_drive_sensor(); configureConstants(); configureExitConditions(); motorsBrake(); - } - void motorsCoast() { + void motorsCoast() + { chassis.set_drive_brake(MOTOR_BRAKE_COAST); } - void motorsHold() { + void motorsHold() + { chassis.set_drive_brake(MOTOR_BRAKE_HOLD); } - void motorsBrake() { + 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() { + 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); + void handleIntake() { + while (true) { + intake_mtr.move_voltage(intakeSpeed); + } } - // 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); + pros::Task intakeManager(handleIntake); + + // void controllerTelemetry() + // { + // while (1) + // { + // for (int i = 0; i < 4; i++) + // { + // double localavg_one = 0; + // } + + // for (int i = 0; i < 4; i++) + // { + // double + // } + // } + // } + + // 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); + 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) { + 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) { + 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) { + 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) { + void rightSwing(double theta) + { chassis.set_swing(RIGHT_SWING, theta, SWING_SPEED * swingScale); } @@ -131,66 +172,89 @@ namespace superstruct { Each of the scale values must be clamed between 0.1 - 1 (10% to 100%) to avoid saturation of motors. */ - void setDriveScale(double val) { + void setDriveScale(double val) + { speedScale = std::clamp(val, 0.1, 1.0); } - void setTurnScale(double val) { + void setTurnScale(double val) + { turnScale = std::clamp(val, 0.1, 1.0); } - void setSwingScale(double val) { - swingScale = 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) { + 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) { + 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; + void runCata(double inpt) + { + if (!ptoEnabled) + return; cata_mtr.move_voltage(inpt); } - void runIntake(double inpt) { - if (!ptoEnabled) return; - intake_mtr.move_voltage(inpt); + void setIntakeSpeed(double inpt) + { + if (!ptoEnabled) + intakeSpeed = 0; + intakeSpeed = 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 + 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)) { + } + 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(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(0); + if (globals::master.get_digital(intakeButton)) + { + setIntakeSpeed(-12000); + } + else if (globals::master.get_digital(outtakeButton)) + { + setIntakeSpeed(12000); + } + else + { + setIntakeSpeed(-1000); } } - void wingsControl(pros::controller_digital_e_t wingControlButton) { - if (globals::master.get_digital_new_press(wingControlButton)) { + 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 @@ -198,13 +262,16 @@ namespace superstruct { } } - void actuateClimb() { + void actuateClimb() + { climb_piston_one.set_value(1); climb_piston_two.set_value(1); } - void climbControl(pros::controller_digital_e_t but1, pros::controller_digital_e_t but2) { - if (globals::master.get_digital(but1) && globals::master.get_digital(but2)) { + void climbControl(pros::controller_digital_e_t but1, pros::controller_digital_e_t but2) + { + if (globals::master.get_digital(but1) && globals::master.get_digital(but2)) + { actuateClimb(); } } @@ -212,19 +279,22 @@ namespace superstruct { /* 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_ONE, RENU_CLIMB_CONTROL_TWO); - } - - void ria_control() { + 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_ONE, RENU_CLIMB_CONTROL_TWO); } - void chris_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_ONE, RENU_CLIMB_CONTROL_TWO); + } + + void chris_control() + { subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE); wingsControl(RENU_WING_CONTROL); climbControl(RENU_CLIMB_CONTROL_ONE, RENU_CLIMB_CONTROL_TWO);