diff --git a/RELENTLESS/include/autons.hpp b/RELENTLESS/include/autons.hpp index 0643ba8..223dd0d 100644 --- a/RELENTLESS/include/autons.hpp +++ b/RELENTLESS/include/autons.hpp @@ -5,6 +5,7 @@ void near_side(); void far_side(); void skills(); +void odom_test(); void default_constants(); void exit_condition_defaults(); \ No newline at end of file diff --git a/RELENTLESS/include/globals.hpp b/RELENTLESS/include/globals.hpp index 6abe932..7eb2c3d 100644 --- a/RELENTLESS/include/globals.hpp +++ b/RELENTLESS/include/globals.hpp @@ -26,7 +26,14 @@ namespace globals { extern pros::Motor_Group left_drive; extern pros::Motor_Group right_drive; - extern lemlib::Drivetrain_t chassis_odom; + extern lemlib::Drivetrain_t dt_odom; + + extern lemlib::OdomSensors_t chassis_sensors; + + extern lemlib::ChassisController_t latController; + extern lemlib::ChassisController_t angController; + + extern lemlib::Chassis chassis_odom; extern pros::Rotation rot_vert; extern pros::Rotation rot_horiz; @@ -46,6 +53,8 @@ namespace globals { extern lemlib::TrackingWheel vert_tracking_wheel; extern lemlib::TrackingWheel horiz_tracking_wheel; + extern pros::Imu inertial_sensor; + extern Drive chassis; extern pros::Motor& cata_left; diff --git a/RELENTLESS/include/superstructure.hpp b/RELENTLESS/include/superstructure.hpp index a421b6d..a1fed4a 100644 --- a/RELENTLESS/include/superstructure.hpp +++ b/RELENTLESS/include/superstructure.hpp @@ -22,6 +22,7 @@ namespace superstruct { //configs void chassisInit(); + void telemetry(); void opControlInit(); void configureExitConditions(); void configureConstants(); diff --git a/RELENTLESS/src/autons.cpp b/RELENTLESS/src/autons.cpp index 64dd365..c35868f 100644 --- a/RELENTLESS/src/autons.cpp +++ b/RELENTLESS/src/autons.cpp @@ -72,3 +72,8 @@ void far_side() { void skills() { } + +void odom_test() { + chassis_odom.moveTo(10, 10, 1000); + chassis_odom.turnTo(15, 15, 1000); +}; diff --git a/RELENTLESS/src/globals.cpp b/RELENTLESS/src/globals.cpp index 04e8c4a..e4c296e 100644 --- a/RELENTLESS/src/globals.cpp +++ b/RELENTLESS/src/globals.cpp @@ -38,6 +38,44 @@ namespace globals { Wings wings; + pros::Imu inertial_sensor(18); + + lemlib::Drivetrain_t dt_odom { + &left_drive, + &right_drive, + TRACK_WIDTH, + WHEEL_SIZE, + DRIVE_RPM + }; + + lemlib::OdomSensors_t chassis_sensors { + nullptr, + nullptr, + nullptr, + nullptr, + &inertial_sensor + }; + + lemlib::ChassisController_t latController { + 0.55, // kP + 5, // kD + 1, // smallErrorRange + 100, // smallErrorTimeout + 3, // largeErrorRange + 500, // largeErrorTimeout + 5 // slew rate + }; + + lemlib::ChassisController_t angController { + 6.5, // kP + 35, // kD + 1, // smallErrorRange + 100, // smallErrorTimeout + 3, // largeErrorRange + 500, // largeErrorTimeout + 40 // slew rate + }; + Drive chassis( {-2, -6, 12, 5}, {-16, 1, 4, -3}, @@ -45,7 +83,9 @@ namespace globals { WHEEL_SIZE, 600, DRIVE_RATIO - ); + ); + + lemlib::Chassis chassis_odom(dt_odom, latController, angController, chassis_sensors); pros::Motor& cata_left = chassis.left_motors[3]; pros::Motor& cata_right = chassis.right_motors[3]; diff --git a/RELENTLESS/src/main.cpp b/RELENTLESS/src/main.cpp index add0aa4..7358299 100644 --- a/RELENTLESS/src/main.cpp +++ b/RELENTLESS/src/main.cpp @@ -41,13 +41,16 @@ void initialize() { ary::autonselector::auton_selector.add_autons({ Auton("Near side (close to alliance goal) \n\nTo run when near alliance goal", near_side), Auton("Far side (far from alliance goal) \n\nTo run when far from alliance goal", far_side), - Auton("Skills \n\nPEAK!!!", skills) + Auton("Skills \n\nPEAK!!!", skills), + Auton("Testing Odometry \n\nBaltimore behavior", odom_test) }); motorsCoast(); // Allow the motors to coast initially chassis.initialize(); ary::autonselector::initialize(); + + pros::Task telemetryTask(telemetry); } void disabled() {} @@ -70,7 +73,7 @@ void opcontrol() { */ if (currentuser == RENU) { - chassis.tank_control(); + chassis.arcade_standard(ary::SPLIT, ary::DEFAULT); renu_control(); } else if (currentuser == RIA) { chassis.tank_control(); diff --git a/RELENTLESS/src/superstructure.cpp b/RELENTLESS/src/superstructure.cpp index 4e31eb3..c8a4615 100644 --- a/RELENTLESS/src/superstructure.cpp +++ b/RELENTLESS/src/superstructure.cpp @@ -36,6 +36,19 @@ namespace superstruct { /* 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_odom.calibrate(); + chassis_odom.setPose(0, 0, 0); + } + + void telemetry() { + while (true) { + lemlib::Pose bot_pose = chassis_odom.getPose(); + pros::lcd::print(0, "x: %f", bot_pose.x); // print the x position + pros::lcd::print(1, "y: %f", bot_pose.y); // print the y position + pros::lcd::print(2, "heading: %f", bot_pose.theta); // print the heading + pros::delay(10); + } } void opControlInit() {