some more work

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();

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;

RELENTLESS/include/superstructure.hpp

@@ -22,6 +22,7 @@
 namespace superstruct {
     //configs
     void chassisInit();
+    void telemetry();
     void opControlInit();
     void configureExitConditions();
     void configureConstants();

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);
+};

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},
@@ -47,6 +85,8 @@ namespace globals {
         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];
 }

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();

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() {