sync

src/main.rs

@@ -21,20 +21,24 @@ use serial::prelude::*;
 
 const LIDAR_ADDRESS: &str = "127.0.0.1:8080";
 const COUNTER_SERIAL_PORT: &str = "/dev/cu.usbmodem1422";
+const LIDAR_BUF_SIZE: usize = 4096;
 
-// should the map matrix be two dim array of const size, vecs of dynamic, or what?
-const ROWS: usize = 50;
-const COLS: usize = 100;
+// each pixel represents 1dm*1dm, so 100*100mm.
+const ROWS: usize = 1000;
+const COLS: usize = 1000;
 
+#![feature(box_syntax)]
 fn main() {
 
+    let mut map= box ([[0u32; ROWS]; COLS]);
+
     //let mut vecMap = vec![vec![0; ROWS]; COLS];
 
 
-    let mut map = [[0u32; ROWS]; COLS];
-    map[0][0] = 3;
+    //let mut map = [[0u32; ROWS]; COLS];
+    //map[0][0] = 3;
 
-    println!("{}, {}", map[0][0], map[0][1]);
+    //println!("{}, {}", map[0][0], map[0][1]);
 
     // dummy thread for replacing lidar
     let server = thread::spawn(|| {
@@ -66,7 +70,7 @@ fn main() {
                                 println!("Server got: {:?}", String::from_utf8_lossy(&message[0..message_len]));
                                 //let written_size = stream.write(&[69 as u8, 96 as u8]);
                                 let written_size = stream.write_all(b"\x02This is a bunch of lidar data...\x03");
-                                println!("server wrote {:?} to stream.", written_size);
+                                println!("server wrote to stream.");
 
                                 // reset everything for next message.
                                 recording = false;
@@ -90,7 +94,7 @@ fn main() {
 
 
     let (request_location_tx, request_location_rx): (mpsc::Sender<bool>, mpsc::Receiver<bool>) = mpsc::channel();
-    let (send_location_tx, send_location_rx): (mpsc::Sender<(i32, i32)>, mpsc::Receiver<(i32, i32)>) = mpsc::channel();
+    let (send_location_tx, send_location_rx): (mpsc::Sender<(i32, i32, f32)>, mpsc::Receiver<(i32, i32, f32)>) = mpsc::channel();
 
     
     // location tracker
@@ -102,13 +106,15 @@ fn main() {
         interact(&mut port).unwrap();
 
         
-        let mut pos: (i32, i32) = (0,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);
                     print!("L: {}, ", pos.0);
                     println!("R: {}", pos.1);
                     },
@@ -125,7 +131,7 @@ fn main() {
                         println!("tracker got stop signal.");
                         break;
                     };
-                    send_location_tx.send(pos).unwrap();
+                    send_location_tx.send((pos)).unwrap();
                     //pos.0 += 1;
                 },
                 Err(_) => {
@@ -145,17 +151,19 @@ fn main() {
     let mapper = thread::spawn(move || {
         println!("mapper started.");
         // create empty map
-        let mut pos: (i32, i32); //= (0,0);
+        
+        let mut pos: (i32, i32, f32); //= (0,0);
 
         // set up connection to lidar
         let mut stream = TcpStream::connect(&LIDAR_ADDRESS).unwrap();
-        let mut lidar_buf: [u8; 4096] = [0u8; 4096];
-        let mut message_len = 0;
+        let mut lidar_buf: [u8; LIDAR_BUF_SIZE] = [0u8; LIDAR_BUF_SIZE];
+        let mut message_len: usize = 0;
         let mut recording = false;
 
         // main loop for this thread
         loop {
             // request position data
+            println!("Mapper requesting pos data.");
             request_location_tx.send(true).unwrap();
             pos = send_location_rx.recv().unwrap();
             if pos.0 % 1000 == 0 {
@@ -165,6 +173,7 @@ fn main() {
 
             // request lidar data
             //thread::sleep(time::Duration::new(3,0));    
+            println!("mapper requesting lidar scan.");
             let _ = stream.write_all(b"\x02sRN LMDscandata\x03"); // tell lidar to send one measurment
             for r in stream.try_clone().expect("stream clone failed").bytes() { // iterate over answer
                 match r {
@@ -183,9 +192,12 @@ fn main() {
                             //let written_size = stream.write(&[69 as u8, 96 as u8]);
                             //println!("server wrote {:?} to stream.", written_size);
 
+                            // update map using lidar data
+                            update_map_with_lidar(lidar_buf, message_len, &mut map);
+
                             // reset everything for next message.
                             recording = false;
-                            message_len = 0;
+                            let message_len: usize = 0;
                             break;
                         }
                         else if recording {
@@ -251,3 +263,9 @@ fn interact<T: SerialPort>(port: &mut T) -> io::Result<()> {
 fn freq_to_distance(freq: i32) -> i32 {
     freq/10
 }
+fn dist_to_angle(pos: &(i32,i32,f32)) -> f32 {
+    0.0
+}
+fn update_map_with_lidar(lidar_data: [u8; LIDAR_BUF_SIZE], message_len: usize, map: &mut [[u32; ROWS]; COLS]) {
+    map[0][0] += 1;
+}
\ No newline at end of file