implement base structure

PROJECT-FUZZY-WUZZY/include/superstructure.hpp

@@ -1 +1,8 @@
 #pragma once
+
+#include "main.h"
+
+namespace superstruct {
+    void initialize_superstruct();
+    void chassis_telemetry();
+}

PROJECT-FUZZY-WUZZY/src/main.cpp

@@ -1,17 +1,10 @@
 #include "main.h"
 
-// void on_center_button() {
-// 	static bool pressed = false;
-// 	pressed = !pressed;
-// 	if (pressed) {
-// 		pros::lcd::set_text(2, "I was pressed!");
-// 	} else {
-// 		pros::lcd::clear_line(2);
-// 	}
-// }
+pros::Task telemetry(superstruct::chassis_telemetry);
 
 void initialize() {
 	pros::lcd::initialize();
+	superstruct::initialize_superstruct();
 	ary::printScr();
 	pros::delay(500);
 
@@ -25,9 +18,6 @@ void initialize() {
 	});
 
 	ary::autonselector::initialize();
-
-
-	//pros::lcd::register_btn1_cb(on_center_button);
 }
 
 void disabled() {}

PROJECT-FUZZY-WUZZY/src/superstructure.cpp

@@ -0,0 +1,23 @@
+#include "main.h"
+#include "superstructure.hpp"
+
+namespace superstruct {
+    void initialize_superstruct() {
+        globals::chassis.calibrate();
+    }
+
+    void chassis_telemetry() {
+        pros::delay(2500); // give time for the chassis to calibrate
+        lemlib::Pose pose(0, 0, 0);
+        while (true) {
+            // print robot location to the brain screen
+            pros::lcd::print(0, "X: %f", globals::chassis.getPose().x); // x
+            pros::lcd::print(1, "Y: %f", globals::chassis.getPose().y); //s y
+            pros::lcd::print(2, "Theta: %f", globals::chassis.getPose().theta); // heading
+            // log position telemetry
+            lemlib::telemetrySink()->info("Chassis pose: {}", globals::chassis.getPose());
+            // delay to save resources
+            pros::delay(50);
+        }
+    }
+}