Add odom resets

NoozAbooz · Dec 17, 2024 · 27314d3 · 27314d3
1 parent e1c3131
commit 27314d3
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 12 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/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
 	}
 }