From b0add8506284fd368cd95391179a949ded22ae3d Mon Sep 17 00:00:00 2001 From: ary Date: Sat, 14 Oct 2023 17:55:42 -0400 Subject: [PATCH] testing sutff --- RELENTLESS-Lite/include/autons.hpp | 1 + RELENTLESS-Lite/include/globals.hpp | 5 +- RELENTLESS-Lite/src/autons.cpp | 136 ++++++++++++++++------------ RELENTLESS-Lite/src/globals.cpp | 11 ++- RELENTLESS-Lite/src/main.cpp | 53 +++++++---- 5 files changed, 129 insertions(+), 77 deletions(-) diff --git a/RELENTLESS-Lite/include/autons.hpp b/RELENTLESS-Lite/include/autons.hpp index 1f16632..a7ed948 100644 --- a/RELENTLESS-Lite/include/autons.hpp +++ b/RELENTLESS-Lite/include/autons.hpp @@ -4,6 +4,7 @@ void test_auton(); void test_second(); +void arc_testing(); void default_constants(); void exit_condition_defaults(); \ No newline at end of file diff --git a/RELENTLESS-Lite/include/globals.hpp b/RELENTLESS-Lite/include/globals.hpp index 08a6017..2ff5184 100644 --- a/RELENTLESS-Lite/include/globals.hpp +++ b/RELENTLESS-Lite/include/globals.hpp @@ -5,6 +5,8 @@ #define DRIVE_RPM 450 #define WHEEL_SIZE 2.75 +extern pros::Task swing_other; + namespace globals { extern pros::Controller master; @@ -19,7 +21,8 @@ namespace globals { extern pros::Motor_Group right_drive; extern lemlib::Drivetrain_t chassis_odom; - extern Drive chassis; + extern Drive chassisRight; + extern Drive chassisLeft; extern pros::Motor cata_left; extern pros::Motor cata_right; diff --git a/RELENTLESS-Lite/src/autons.cpp b/RELENTLESS-Lite/src/autons.cpp index f401ff0..c92ec5c 100644 --- a/RELENTLESS-Lite/src/autons.cpp +++ b/RELENTLESS-Lite/src/autons.cpp @@ -12,20 +12,39 @@ const int DRIVE_SPEED = 110; const int TURN_SPEED = 101.5; const int SWING_SPEED = 101.5; +void swingOther(void* p) { + chassisLeft.initialize(); + chassisLeft.set_swing_pid(LEFT_SWING, 180, SWING_SPEED); +} + +pros::Task swing_other(swingOther); + void default_constants() { - chassis.set_slew_min_power(80, 80); - chassis.set_slew_distance(7, 7); - chassis.set_pid_constants(&chassis.headingPID, 11, 0, 17.5, 0); - chassis.set_pid_constants(&chassis.forward_drivePID, 2, 0, 5, 0); - chassis.set_pid_constants(&chassis.backward_drivePID, 2, 0, 5, 0); - chassis.set_pid_constants(&chassis.turnPID, 8, 0.003, 27, 15); - chassis.set_pid_constants(&chassis.swingPID, 7, 0, 35, 0); + chassisRight.set_slew_min_power(80, 80); + chassisRight.set_slew_distance(7, 7); + chassisRight.set_pid_constants(&chassisRight.headingPID, 11, 0, 17.5, 0); + chassisRight.set_pid_constants(&chassisRight.forward_drivePID, 2, 0, 6.5, 0); + chassisRight.set_pid_constants(&chassisRight.backward_drivePID, 2, 0, 6.5, 0); + chassisRight.set_pid_constants(&chassisRight.turnPID, 8, 0.003, 27, 15); + chassisRight.set_pid_constants(&chassisRight.swingPID, 7, 0, 35, 0); + + chassisLeft.set_slew_min_power(80, 80); + chassisLeft.set_slew_distance(7, 7); + chassisLeft.set_pid_constants(&chassisRight.headingPID, 11, 0, 17.5, 0); + chassisLeft.set_pid_constants(&chassisRight.forward_drivePID, 2, 0, 6.5, 0); + chassisLeft.set_pid_constants(&chassisRight.backward_drivePID, 2, 0, 6.5, 0); + chassisLeft.set_pid_constants(&chassisRight.turnPID, 8, 0.003, 27, 15); + chassisLeft.set_pid_constants(&chassisRight.swingPID, 7, 0, 35, 0); } void exit_condition_defaults() { - chassis.set_exit_condition(chassis.turn_exit, 50, 2, 220, 3, 500, 500);//100, 3, 500, 7, 500, 500 //100, 2, 500, 4, 500, 500// 70, 1, 220, 3, 500, 500 - chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 500, 500);//100, 3, 500, 7, 500, 500 - chassis.set_exit_condition(chassis.drive_exit, 40, 80, 300, 150, 500, 500);//80, 50, 300, 150, 500, 500//20, 60, 300, 150, 500, 500 + chassisRight.set_exit_condition(chassisRight.turn_exit, 50, 2, 220, 3, 500, 500);//100, 3, 500, 7, 500, 500 //100, 2, 500, 4, 500, 500// 70, 1, 220, 3, 500, 500 + chassisRight.set_exit_condition(chassisRight.swing_exit, 100, 3, 500, 7, 500, 500);//100, 3, 500, 7, 500, 500 + chassisRight.set_exit_condition(chassisRight.drive_exit, 40, 80, 300, 150, 500, 500);//80, 50, 300, 150, 500, 500//20, 60, 300, 150, 500, 500 + + chassisLeft.set_exit_condition(chassisRight.turn_exit, 50, 2, 220, 3, 500, 500);//100, 3, 500, 7, 500, 500 //100, 2, 500, 4, 500, 500// 70, 1, 220, 3, 500, 500 + chassisLeft.set_exit_condition(chassisRight.swing_exit, 100, 3, 500, 7, 500, 500);//100, 3, 500, 7, 500, 500 + chassisLeft.set_exit_condition(chassisRight.drive_exit, 40, 80, 300, 150, 500, 500);//80, 50, 300, 150, 500, 500//20, 60, 300, 150, 500, 500 } // ** other stufff** // @@ -33,67 +52,72 @@ void exit_condition_defaults() { // ** AUTONS ** // void test_auton() { - chassis.set_drive_pid(48, DRIVE_SPEED, true, true); - chassis.wait_drive(); - chassis.set_turn_pid(-65, TURN_SPEED); - chassis.wait_drive(); - chassis.set_drive_pid(60, DRIVE_SPEED, true, true); - chassis.wait_drive(); - chassis.set_drive_pid(-10, DRIVE_SPEED, false, true); - chassis.wait_drive(); - chassis.set_turn_pid(57, TURN_SPEED); - chassis.wait_drive(); - chassis.set_drive_pid(43, DRIVE_SPEED, true, true); - chassis.wait_drive(); - chassis.set_swing_pid(LEFT_SWING, 80, SWING_SPEED); - chassis.wait_drive(); - chassis.set_drive_pid(2, DRIVE_SPEED, false, true); - chassis.wait_drive(); + chassisRight.set_drive_pid(48, DRIVE_SPEED, true, true); + chassisRight.wait_drive(); + chassisRight.set_turn_pid(-65, TURN_SPEED); + chassisRight.wait_drive(); + chassisRight.set_drive_pid(60, DRIVE_SPEED, true, true); + chassisRight.wait_drive(); + chassisRight.set_drive_pid(-10, DRIVE_SPEED, false, true); + chassisRight.wait_drive(); + chassisRight.set_turn_pid(57, TURN_SPEED); + chassisRight.wait_drive(); + chassisRight.set_drive_pid(43, DRIVE_SPEED, true, true); + chassisRight.wait_drive(); + chassisRight.set_swing_pid(LEFT_SWING, 80, SWING_SPEED); + chassisRight.wait_drive(); + chassisRight.set_drive_pid(2, DRIVE_SPEED, false, true); + chassisRight.wait_drive(); - chassis.set_drive_pid(-26, DRIVE_SPEED, true, true); - chassis.wait_drive(); + chassisRight.set_drive_pid(-26, DRIVE_SPEED, true, true); + chassisRight.wait_drive(); - chassis.set_turn_pid(-45, TURN_SPEED); - chassis.wait_drive(); + chassisRight.set_turn_pid(-45, TURN_SPEED); + chassisRight.wait_drive(); } void test_second() { - chassis.set_drive_pid(107, DRIVE_SPEED, true, true); - chassis.wait_drive(); + chassisRight.set_drive_pid(107, DRIVE_SPEED, true, true); + chassisRight.wait_drive(); - //chassis.set_swing_pid(LEFT_SWING, 100, SWING_SPEED); - //chassis.wait_drive(); + //chassisRight.set_swing_pid(LEFT_SWING, 100, SWING_SPEED); + //chassisRight.wait_drive(); - chassis.set_drive_pid(-38, DRIVE_SPEED, true, true); - chassis.wait_drive(); + chassisRight.set_drive_pid(-38, DRIVE_SPEED, true, true); + chassisRight.wait_drive(); - chassis.set_turn_pid(-99, TURN_SPEED); - chassis.wait_drive(); + chassisRight.set_turn_pid(-99, TURN_SPEED); + chassisRight.wait_drive(); - chassis.set_drive_pid(95, DRIVE_SPEED, true, true); - chassis.wait_drive(); + chassisRight.set_drive_pid(95, DRIVE_SPEED, true, true); + chassisRight.wait_drive(); - chassis.set_drive_pid(-10, DRIVE_SPEED, false, true); - chassis.wait_drive(); + chassisRight.set_drive_pid(-10, DRIVE_SPEED, false, true); + chassisRight.wait_drive(); - chassis.set_turn_pid(15, TURN_SPEED); - chassis.wait_drive(); + chassisRight.set_turn_pid(15, TURN_SPEED); + chassisRight.wait_drive(); - chassis.set_drive_pid(40, DRIVE_SPEED, true, true); - chassis.wait_drive(); + chassisRight.set_drive_pid(40, DRIVE_SPEED, true, true); + chassisRight.wait_drive(); - chassis.set_swing_pid(LEFT_SWING, 60, SWING_SPEED); - chassis.wait_drive(); + chassisRight.set_swing_pid(LEFT_SWING, 60, SWING_SPEED); + chassisRight.wait_drive(); - chassis.set_drive_pid(-48, DRIVE_SPEED, true, true); - chassis.wait_drive(); + chassisRight.set_drive_pid(-48, DRIVE_SPEED, true, true); + chassisRight.wait_drive(); - chassis.set_turn_pid(-90, TURN_SPEED); - chassis.wait_drive(); + chassisRight.set_turn_pid(-90, TURN_SPEED); + chassisRight.wait_drive(); - chassis.set_drive_pid(12, DRIVE_SPEED, false, true); - chassis.wait_drive(); + chassisRight.set_drive_pid(12, DRIVE_SPEED, false, true); + chassisRight.wait_drive(); - // chassis.set_drive_pid(2, DRIVE_SPEED, false, true); - // chassis.wait_drive(); + // chassisRight.set_drive_pid(2, DRIVE_SPEED, false, true); + // chassisRight.wait_drive(); +} + +void arc_testing() { + chassisRight.set_swing_pid(RIGHT_SWING, 180, SWING_SPEED); + swing_other.resume(); } \ No newline at end of file diff --git a/RELENTLESS-Lite/src/globals.cpp b/RELENTLESS-Lite/src/globals.cpp index 17193c8..1c8abdd 100644 --- a/RELENTLESS-Lite/src/globals.cpp +++ b/RELENTLESS-Lite/src/globals.cpp @@ -27,7 +27,16 @@ namespace globals { DRIVE_RPM }; - Drive chassis( + Drive chassisRight( + {-20, -17, 8}, + {12, 1, -4}, + 10, + 2.75, + 600, + 0.75 + ); + + Drive chassisLeft( {-20, -17, 8}, {12, 1, -4}, 10, diff --git a/RELENTLESS-Lite/src/main.cpp b/RELENTLESS-Lite/src/main.cpp index 158134e..cfac529 100644 --- a/RELENTLESS-Lite/src/main.cpp +++ b/RELENTLESS-Lite/src/main.cpp @@ -21,28 +21,38 @@ void initialize() { - */ - 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.5, 0.5); + chassisRight.set_curve_buttons(pros::E_CONTROLLER_DIGITAL_LEFT, pros::E_CONTROLLER_DIGITAL_RIGHT); + chassisRight.toggle_modify_curve_with_controller(true); + chassisRight.set_active_brake(0.1); + chassisRight.set_curve_default(0.5, 0.5); - chassis.set_joystick_drivescale(1.0); - chassis.set_joystick_turnscale(1.0); + chassisRight.set_joystick_drivescale(1.0); + chassisRight.set_joystick_turnscale(1.0); + + chassisLeft.set_curve_buttons(pros::E_CONTROLLER_DIGITAL_LEFT, pros::E_CONTROLLER_DIGITAL_RIGHT); + chassisLeft.toggle_modify_curve_with_controller(true); + chassisLeft.set_active_brake(0.1); + chassisLeft.set_curve_default(0.5, 0.5); + + chassisLeft.set_joystick_drivescale(1.0); + chassisLeft.set_joystick_turnscale(1.0); default_constants(); exit_condition_defaults(); ary::autonselector::auton_selector.add_autons({ Auton("Test Auton\n\nTesting Autonomous on field", test_auton), - Auton("Test Auton - The Sequel\n\nyo mama", test_second) // TODO: Change this before putting this in the notebook LOL + Auton("Test Auton - The Sequel\n\nyo mama", test_second), // TODO: Change this before putting this in the notebook LOL + Auton("CURVATURE, ARCING, AND EVERYTHING IN BETWEEN", arc_testing) }); - chassis.set_drive_brake(MOTOR_BRAKE_COAST); + chassisRight.set_drive_brake(MOTOR_BRAKE_COAST); + chassisLeft.set_drive_brake(MOTOR_BRAKE_COAST); //handle pto and stuff - //chassis.pto_add(); + //chassisRight.pto_add(); - chassis.initialize(); + chassisRight.initialize(); ary::autonselector::initialize(); } @@ -61,23 +71,28 @@ void competition_initialize() { } void autonomous() { - chassis.reset_pid_targets(); - chassis.reset_gyro(); - chassis.reset_drive_sensor(); - chassis.set_drive_brake(MOTOR_BRAKE_COAST); + chassisRight.reset_pid_targets(); + chassisRight.reset_gyro(); + chassisRight.reset_drive_sensor(); + chassisRight.set_drive_brake(MOTOR_BRAKE_COAST); + + chassisLeft.reset_pid_targets(); + chassisLeft.reset_gyro(); + chassisLeft.reset_drive_sensor(); + chassisLeft.set_drive_brake(MOTOR_BRAKE_COAST); default_constants(); ary::autonselector::auton_selector.call_selected_auton(); } void opcontrol() { - chassis.set_drive_brake(MOTOR_BRAKE_COAST); - chassis.set_active_brake(0); + chassisRight.set_drive_brake(MOTOR_BRAKE_COAST); + chassisRight.set_active_brake(0); while (true) { - //chassis.arcade_standard(ary::SdPLIT, ary::DEFAULT); - //chassis.arcade_standard(ary::SPLIT, ary::DEFAULT); - chassis.tank(); + //chassisRight.arcade_standard(ary::SdPLIT, ary::DEFAULT); + //chassisRight.arcade_standard(ary::SPLIT, ary::DEFAULT); + chassisRight.tank(); // if (globals::master.get_digital(DIGITAL_R1)) { // cata_left.move_voltage(12000);