From 3ff9d9e5f633c9ac70ad2e8bc1bbb3d82f925d0b Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Mon, 9 Sep 2024 21:44:48 -0400
Subject: [PATCH] pid on camera motion

---
 simcontroller/src/main.rs | 29 ++++++++++++++++++++++++-----
 1 file changed, 24 insertions(+), 5 deletions(-)

diff --git a/simcontroller/src/main.rs b/simcontroller/src/main.rs
index 886c727..374620a 100644
--- a/simcontroller/src/main.rs
+++ b/simcontroller/src/main.rs
@@ -1,5 +1,5 @@
 use camera::Camera;
-use opencv::{core::{KeyPoint, Mat, Ptr, VecN, Vector, CV_8UC4}, features2d::{draw_matches_def, BFMatcher, ORB}, highgui, imgcodecs::ImreadModes, imgproc, prelude::{DescriptorMatcherTrait, DescriptorMatcherTraitConst, Feature2DTrait}};
+use opencv::{core::{KeyPoint, KeyPointTraitConst, Mat, Ptr, VecN, Vector, CV_8UC4}, features2d::{draw_matches_def, BFMatcher, ORB}, highgui, imgcodecs::ImreadModes, imgproc, prelude::{DescriptorMatcherTrait, DescriptorMatcherTraitConst, Feature2DTrait}};
 use webots::prelude::*;
 use std::{f64::consts::TAU, time::Duration};
 mod camera;
@@ -17,6 +17,7 @@ struct MyRobot {
     last_frame_descriptors: Option<Mat>,
     last_frame_keypoints: Option<Vector<KeyPoint>>,
     last_image: Option<Mat>,
+    direction: f32,
 }
 
 impl Robot for MyRobot {
@@ -52,6 +53,7 @@ impl Robot for MyRobot {
             last_frame_descriptors: None,
             last_frame_keypoints: None,
             last_image: None,
+            direction: 0.,
         }
     }
 
@@ -83,6 +85,20 @@ impl Robot for MyRobot {
                 let matches = matches.iter().filter(|m| m.distance < 105.).collect();
                 draw_matches_def(self.last_image.as_ref().unwrap(), self.last_frame_keypoints.as_ref().unwrap(), &img , &keypoints, &matches, &mut debugout).unwrap();
                 highgui::imshow("matches", &debugout).unwrap();
+
+                let mut horiz = Vec::new();
+                for hit in matches {
+                    if hit.distance > 45. {continue;}
+                    let last = self.last_frame_keypoints.as_ref().unwrap().get(hit.query_idx as usize).unwrap();
+                    let current = keypoints.get(hit.train_idx as usize).unwrap();
+
+                    horiz.push(last.pt().x - current.pt().x);
+                }
+                if horiz.len() > 0 {
+                    let avg = horiz.iter().sum::<f32>()/horiz.len() as f32;
+
+                    self.direction += avg;
+                }
             }
             self.last_frame_descriptors = Some(descriptors);
             self.last_frame_keypoints = Some(keypoints);
@@ -91,11 +107,14 @@ impl Robot for MyRobot {
         highgui::poll_key().unwrap();
 
         // initialize motor speeds at 50% of MAX_SPEED.
-        let mut left_speed = 0.5 * MAX_SPEED;
-        let mut right_speed = 0.5 * MAX_SPEED;
+        let error = 320. - self.direction as f64;
+        let error = error * 0.05;
+        dbg!(self.direction);
+        let mut left_speed = error * MAX_SPEED;
+        let mut right_speed = -error * MAX_SPEED;
 
-        self.left_motor.set_velocity(left_speed);
-        self.right_motor.set_velocity(right_speed);
+        self.left_motor.set_velocity(left_speed.clamp(-10., 10.));
+        self.right_motor.set_velocity(right_speed.clamp(-10., 10.));
     }
 }