ank/cruisecontrol
1
Fork 0
Code Issues Activity

track velocity instead of rotation (still scuffed)

Browse source
This commit is contained in:
Andy Killorin 2024-09-09 22:39:39 -04:00
parent 3ff9d9e5f6
commit c6341820ca
Signed by: ank
GPG key ID: B6241CA3B552BCA4
1 changed files with 26 additions and 6 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

32
simcontroller/src/main.rs
View file

@ -18,6 +18,7 @@ struct MyRobot {
last_frame_keypoints: Option<Vector<KeyPoint>>,
last_image: Option<Mat>,
direction: f32,
rotational_velocity: f32,
}
impl Robot for MyRobot {
@ -54,6 +55,7 @@ impl Robot for MyRobot {
last_frame_keypoints: None,
last_image: None,
direction: 0.,
rotational_velocity: 0.,
}
}
@ -80,7 +82,10 @@ impl Robot for MyRobot {
if let Some(mut last_frame_descriptors) = self.last_frame_descriptors.as_ref() {
self.matcher.train_match_def(&mut last_frame_descriptors, &mut descriptors, &mut matches).unwrap();
if let Err(e) = self.matcher.train_match_def(&mut last_frame_descriptors, &mut descriptors, &mut matches) {
eprintln!("no features to match");
}
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();
@ -92,13 +97,19 @@ impl Robot for MyRobot {
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);
let dx = last.pt().x - current.pt().x;
horiz.push(dx);
}
if horiz.len() > 0 {
let avg = horiz.iter().sum::<f32>()/horiz.len() as f32;
self.direction += avg;
let lerp = 0.1 + horiz.len() as f32 * 0.05;
let lerp = lerp.max(0.65);
self.rotational_velocity = self.rotational_velocity * (1.-lerp) + avg * lerp;
//self.direction += avg;
}
self.direction += self.rotational_velocity;
}
self.last_frame_descriptors = Some(descriptors);
self.last_frame_keypoints = Some(keypoints);
@ -106,10 +117,19 @@ impl Robot for MyRobot {
}
highgui::poll_key().unwrap();
// initialize motor speeds at 50% of MAX_SPEED.
let error = 320. - self.direction as f64;
let error = error * 0.05;
const DEG_TO_PX: f64 = 320. / 62.2;
let goal = if _time.elapsed.as_secs() % 8 < 4 {
90. * DEG_TO_PX
} else {
90. * DEG_TO_PX
};
let error = goal - self.direction as f64;
let mut error = error * 0.02;
if error.abs() < 0.5 {
error = 0.;
}
dbg!(self.direction);
dbg!(self.rotational_velocity);
let mut left_speed = error * MAX_SPEED;
let mut right_speed = -error * MAX_SPEED;