Merge branch 'V2' of https://github.com/NoozAbooz/210K-HighStakes-2025 …

…into V2
NoozAbooz · Jan 5, 2025 · 967aa6a · 967aa6a
2 parents 7544f78 + 6744447
commit 967aa6a
Show file tree

Hide file tree

Showing 5 changed files with 36 additions and 30 deletions.
diff --git a/include/libKS/drivetrain/odom.hpp b/include/libKS/drivetrain/odom.hpp
@@ -12,5 +12,7 @@ namespace ks
     extern Position odom_pos;
 
     extern void odomThread();
-    extern void resetOdomPosition();
+    extern void setOdomPosition(double x_new, double y_new, double theta_new);
+    extern void initializeOdom();
+    extern pros::Task odom_task;
 }
diff --git a/src/auton/autonomous.cpp b/src/auton/autonomous.cpp
@@ -25,6 +25,22 @@ void testBM() {
     chassis.moveToPose(52, 6, 320, 1000, {.forwards = false});
 }
 
+rd::Selector gui_selector({
+    {"E 5 Ring", Five_Ring},
+    {"Q 4 Ring + Bar", four_ring_bar},
+    {"Q SAWP 2 Mogo", SAWP_4},
+    {"Skills", skills},
+});
+void autonomous() {
+    //initializeColourSort();
+    chassis.setPose(0, 0, 0);
+    field_status = "autonomous";
+    console.println("Running auton...");
+    gui_selector.run_auton();
+}
+
+
+/* Legacy Auton Routines */
 void four_ring_bar() {
     if (alliance == "red" || alliance == "na") {
         wallStake.move_voltage(10000); // Start
@@ -322,21 +338,4 @@ void skills() {
     pros::delay(100);
     intake.move_voltage(12000);
     pros::delay(500);
-}
-
-rd::Selector gui_selector({
-    {"E 5 Ring", Five_Ring},
-    {"Q 4 Ring + Bar", four_ring_bar},
-    {"Q SAWP 2 Mogo", SAWP_4},
-    {"Skills", skills},
-});
-void autonomous() {
-    //initializeColourSort();
-    chassis.setPose(0, 0, 0);
-    field_status = "autonomous";
-    console.println("Running auton...");
-    gui_selector.run_auton();
-}
-
-
-/* Legacy Auton Routines */
+}
diff --git a/src/init/initialize.cpp b/src/init/initialize.cpp
@@ -16,10 +16,7 @@ void initialize() {
     // pros::Task([] {
     //     chassis.calibrate();
     // });
-
-    pros::Task([] {
-        ks::odomThread();   
-    });
+    ks::initializeOdom();
 
     optical.set_led_pwm(75); // enable led on optical sensor for accuracy
     optical.set_integration_time(10); // refresh every 10ms

diff --git a/src/libKS/drivetrain/odom.cpp b/src/libKS/drivetrain/odom.cpp
@@ -80,19 +80,28 @@ double avg_heading;
 double deltaXLocal;
 double deltaYLocal;
 
-void ks::resetOdomPosition() {
-	x = 0;
-	y = 0;
+void ks::initializeOdom() {
+	pros::Task odom_task(ks::odomThread);
+}
+
+void ks::setOdomPosition(double x_new = 0, double y_new = 0, double theta_new = 0) {
+	odom_task.suspend();
+	x = x_new;
+	y = y_new;
 
 	verticalEncoder.reset_position();
 	horizontalEncoder.reset_position();
 
+	vertical_pos = 0;
+	horizontal_pos = 0;
+
 	prev_vertical_pos = 0;
 	prev_horizontal_pos = 0;
 	prev_heading = 0;
 
-	inertial1.set_heading(0);
-	inertial2.set_heading(0);
+	inertial1.set_heading(theta_new);
+	inertial2.set_heading(theta_new);
+	odom_task.resume();
 }
 
 void ks::odomThread() {
@@ -141,7 +150,6 @@ void ks::odomThread() {
        		theta += 360;
 		}
 
-		// funky stuff for alliance colours :(
 		chassis.setPose(x, y, get_imu_rotation());
         pros::delay(5);
     }

diff --git a/src/main.cpp b/src/main.cpp
@@ -31,7 +31,7 @@ void opcontrol() {
 
 		// Report temperature telemetry 😭
 		double drivetrainTemps = ks::vector_average(leftDrive.get_temperature_all());
-		controller.print(0, 0, "DT%.0lf|INT%.0lf|%.0lf  ", drivetrainTemps, intake.get_temperature(), chassis.getPose().theta);
+		controller.print(0, 0, "DT%.0lf|INT%.0lf|%.0lf  ", drivetrainTemps, intake.get_temperature(), ks::odom_pos.theta);
 		pros::delay(10); // Delay to save resources on brain
 	}
 }