small const and fn updates

src/main.rs

@@ -27,6 +27,11 @@ const LIDAR_BUF_SIZE: usize = 4096;
 const ROWS: usize = 1000;
 const COLS: usize = 1000;
 
+// robot real stuff
+const AXEL_WIDTH: f32 = 175; // cm
+const FREQ_TO_OMEGA: f32 = 0.0002;
+const WHEEL_RADIUS: f32 =  30.0; //cm
+
 #![feature(box_syntax)]
 fn main() {
 
@@ -105,16 +110,17 @@ fn main() {
         let mut port = serial::open(&COUNTER_SERIAL_PORT).unwrap();
         interact(&mut port).unwrap();
 
-        
+        //let mut l_tick: f32 = 0;
+        //let mtu r_tick: f32 = 0;
         let mut pos: (i32, i32, f32) = (COLS as i32/2,ROWS as i32/2, 0.0);
         println!("location initial position: {:?}", pos);
         loop {
             // get encoder data from arduino and update local variables
             match port.read_exact(&mut serial_buf){
                 Ok(_) => {
-                    pos.0 += freq_to_distance( ( ((serial_buf[0] as u32) << 24) + ((serial_buf[1] as u32) << 16) + ((serial_buf[2] as u32) << 8) + (serial_buf[3] as u32) ) as i32);
-                    pos.1 += freq_to_distance( ( ((serial_buf[4] as u32) << 24) + ((serial_buf[5] as u32) << 16) + ((serial_buf[6] as u32) << 8) + (serial_buf[7] as u32) ) as i32);
-                    pos.2 = dist_to_angle(&pos);
+                    let l_speed: f32 = tick_to_speed( ( ((serial_buf[0] as u32) << 24) + ((serial_buf[1] as u32) << 16) + ((serial_buf[2] as u32) << 8) + (serial_buf[3] as u32) ) as i32);
+                    let r_speed: f32 = tick_to_speed( ( ((serial_buf[4] as u32) << 24) + ((serial_buf[5] as u32) << 16) + ((serial_buf[6] as u32) << 8) + (serial_buf[7] as u32) ) as i32);
+                    pos.2 = update_state(&pos, l_tick, r_tick);
                     print!("L: {}, ", pos.0);
                     println!("R: {}", pos.1);
                     },
@@ -259,12 +265,17 @@ fn interact<T: SerialPort>(port: &mut T) -> io::Result<()> {
     Ok(())
 }
 
+/*
+const AXEL_WIDTH: f32 = 175; // cm
+const FREQ_TO_OMEGA: f32 = 0.0002;
+const WHEEL_RADIUS: f32 =  30.0; //cm
+*/
 
-fn freq_to_distance(freq: i32) -> i32 {
-    freq/10
+fn tick_to_speed(tick: i32) -> f32 { 
+    tick/10.0
 }
-fn dist_to_angle(pos: &(i32,i32,f32)) -> f32 {
-    0.0
+fn update_state(pos: &(f32,f32,f32), l_speed: f32, r_speed: f32) {
+    pos.0 += 0.5;
 }
 fn update_map_with_lidar(lidar_data: [u8; LIDAR_BUF_SIZE], message_len: usize, map: &mut [[u32; ROWS]; COLS]) {
     map[0][0] += 1;