ank/cruisecontrol
1
Fork 0
Code Issues Activity

pid on camera motion

Browse source
This commit is contained in:
Andy Killorin 2024-09-09 21:44:48 -04:00
parent 9e1c0a8876
commit 3ff9d9e5f6
Signed by: ank
GPG key ID: B6241CA3B552BCA4
1 changed files with 24 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

29
simcontroller/src/main.rs
View file

@ -1,5 +1,5 @@
use camera::Camera; 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 webots::prelude::*;
use std::{f64::consts::TAU, time::Duration}; use std::{f64::consts::TAU, time::Duration};
mod camera; mod camera;
@ -17,6 +17,7 @@ struct MyRobot {
last_frame_descriptors: Option<Mat>, last_frame_descriptors: Option<Mat>,
last_frame_keypoints: Option<Vector<KeyPoint>>, last_frame_keypoints: Option<Vector<KeyPoint>>,
last_image: Option<Mat>, last_image: Option<Mat>,
direction: f32,
} }
impl Robot for MyRobot { impl Robot for MyRobot {
@ -52,6 +53,7 @@ impl Robot for MyRobot {
last_frame_descriptors: None, last_frame_descriptors: None,
last_frame_keypoints: None, last_frame_keypoints: None,
last_image: None, last_image: None,
direction: 0.,
} }
} }
@ -83,6 +85,20 @@ impl Robot for MyRobot {
let matches = matches.iter().filter(|m| m.distance < 105.).collect(); 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(); 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(); 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_descriptors = Some(descriptors);
self.last_frame_keypoints = Some(keypoints); self.last_frame_keypoints = Some(keypoints);
@ -91,11 +107,14 @@ impl Robot for MyRobot {
highgui::poll_key().unwrap(); highgui::poll_key().unwrap();
// initialize motor speeds at 50% of MAX_SPEED. // initialize motor speeds at 50% of MAX_SPEED.
let mut left_speed = 0.5 * MAX_SPEED; let error = 320. - self.direction as f64;
let mut right_speed = 0.5 * MAX_SPEED; 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.left_motor.set_velocity(left_speed.clamp(-10., 10.));
self.right_motor.set_velocity(right_speed); self.right_motor.set_velocity(right_speed.clamp(-10., 10.));
} }
} }