Odometry Problems

So I’m the programmer for my team and I’m trying to implement odometry. The only problem is that it’s not working. We’re coding our robot in Rust using vex-rt, not C++, but does anyone see a major flaw in my logic or math?

Code:

``````fn trackpos(&'static self, startx: i32, starty: i32, imu: &mut MutexGuard<InertialSensor>) {
let mut dx = startx as f64; //change in x since last loop
let mut dy = starty as f64; //change in y since last loop
let mut c = 0.0;
let mut xpos = dx; //Absolute x-location
let mut ypos = dy; //Absolute y-location
let mut theta = imu.get_heading().expect("initializing theta in trackpos"); //the angle at which we are turned
let mut oldtheta = theta;
loop {
c = self.drive.lock().track().0 + self.drive.lock().track().1;
c /= 2.0;
theta = oldtheta - imu.get_heading().expect("calculating theta in trackpos");
/*
It's a right triangle so we can use SOH CAH TOA
*/
dx = c * cos(theta); //finding dx
dy = c * sin(theta); //finding dy
xpos += dx; //calculating new global x position
ypos += dy; //calculating new global y position
self.drive.lock().reset();
println!("xpos : {}, ypos: {}", xpos, ypos);
oldtheta = imu.get_heading().expect("calculating theta in trackpos");
}
}

Track function:

pub fn track(&self) -> (f64, f64) {
let mut lpos = self.lf.get_position().expect("lpos in track()");
let mut rpos = self.rf.get_position().expect("rpos in track()");
return (lpos, rpos);
}
``````

Side note: for those who aren’t familiar with Rust, don’t worry about .expect(), it’s used in error handling.
Also, drive is a mutex, and .reset() just tares the drive motors

2 Likes

Please format your code. You just have to stick ``` (it’s the button above `tab`) at the beginning and end, before the first line and after the last line.

1 Like