diff --git a/PROJECT-FUZZY-WUZZY/include/superstructure.hpp b/PROJECT-FUZZY-WUZZY/include/superstructure.hpp index 7b9637e..8f4d5a2 100644 --- a/PROJECT-FUZZY-WUZZY/include/superstructure.hpp +++ b/PROJECT-FUZZY-WUZZY/include/superstructure.hpp @@ -1 +1,8 @@ -#pragma once \ No newline at end of file +#pragma once + +#include "main.h" + +namespace superstruct { + void initialize_superstruct(); + void chassis_telemetry(); +} \ No newline at end of file diff --git a/PROJECT-FUZZY-WUZZY/src/main.cpp b/PROJECT-FUZZY-WUZZY/src/main.cpp index 4f22665..72cc95a 100644 --- a/PROJECT-FUZZY-WUZZY/src/main.cpp +++ b/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() {} diff --git a/PROJECT-FUZZY-WUZZY/src/superstructure.cpp b/PROJECT-FUZZY-WUZZY/src/superstructure.cpp index e69de29..4abf047 100644 --- a/PROJECT-FUZZY-WUZZY/src/superstructure.cpp +++ b/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); + } + } +} \ No newline at end of file