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");
            vex_rt::rtos::Task::delay(core::time::Duration::from_millis(1));
        }
    }

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