From 0429dd18ed363820ee47822b7a3747e48d674990 Mon Sep 17 00:00:00 2001 From: ary Date: Fri, 1 Dec 2023 21:17:23 -0500 Subject: [PATCH] testing pp on practice chassis --- RUN-THAT-BACK/src/main.cpp | 16 +++++++++------- RUN-THAT-BACK/static/testingpath.txt | 2 +- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/RUN-THAT-BACK/src/main.cpp b/RUN-THAT-BACK/src/main.cpp index d51bc6f..85335cf 100644 --- a/RUN-THAT-BACK/src/main.cpp +++ b/RUN-THAT-BACK/src/main.cpp @@ -7,12 +7,12 @@ pros::Controller controller(pros::E_CONTROLLER_MASTER); // drive motors -pros::Motor lF(-20, pros::E_MOTOR_GEARSET_06); // left front motor. port 8, reversed -pros::Motor lM(10, pros::E_MOTOR_GEARSET_06); // left middle motor. port 20, reversed -pros::Motor lB(-9, pros::E_MOTOR_GEARSET_06); // left back motor. port 19 -pros::Motor rF(12, pros::E_MOTOR_GEARSET_06); // right front motor. port 2 -pros::Motor rM(-17, pros::E_MOTOR_GEARSET_06); // right middle motor. port 11 -pros::Motor rB(-13, pros::E_MOTOR_GEARSET_06); // right back motor. port 13, reversed +pros::Motor lF(-9, pros::E_MOTOR_GEARSET_06); // left front motor. port 8, reversed +pros::Motor lM(-7, pros::E_MOTOR_GEARSET_06); // left middle motor. port 20, reversed +pros::Motor lB(8, pros::E_MOTOR_GEARSET_06); // left back motor. port 19 +pros::Motor rF(-4, pros::E_MOTOR_GEARSET_06); // right front motor. port 2 +pros::Motor rM(12, pros::E_MOTOR_GEARSET_06); // right middle motor. port 11 +pros::Motor rB(2, pros::E_MOTOR_GEARSET_06); // right back motor. port 13, reversed // motor groups pros::MotorGroup leftMotors({lF, lM, lB}); // left motor group @@ -31,7 +31,7 @@ lemlib::Drivetrain drivetrain(&leftMotors, // left motor group 10, // 10 inch track width lemlib::Omniwheel::NEW_275, // using new 3.25" omnis 450, // drivetrain rpm is 360 - 8 // chase power is 2. If we had traction wheels, it would have been 8 + 2 // chase power is 2. If we had traction wheels, it would have been 8 ); // lateral motion controller @@ -121,8 +121,10 @@ ASSET(testingpath_txt); * This is an example autonomous routine which demonstrates a lot of the features LemLib has to offer */ void autonomous() { + chassis.setPose(9.72, -59.062, 90, false); chassis.follow(testingpath_txt, 15, 4000); chassis.waitUntilDone(); + pros::lcd::print(4, "pure pursuit finished!"); // chassis.moveTo(20, 15, 90, 4000); diff --git a/RUN-THAT-BACK/static/testingpath.txt b/RUN-THAT-BACK/static/testingpath.txt index 87d4030..252c212 100644 --- a/RUN-THAT-BACK/static/testingpath.txt +++ b/RUN-THAT-BACK/static/testingpath.txt @@ -40,7 +40,7 @@ 9.424, -59.655, 0 -10.542, -58.49, 0 endData -127 +209.9 100 200 61.449, -28.859, 72.811, -67.13, 27.065, -60.552, 9.424, -59.655