Functioning emulated lidar and serial communication threads in one program.

Cargo.lock

@@ -103,6 +103,14 @@ dependencies = [
  "adler32 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)",
 ]
 
+[[package]]
+name = "ioctl-rs"
+version = "0.1.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "libc 0.2.33 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
 [[package]]
 name = "jpeg-decoder"
 version = "0.1.13"
@@ -211,6 +219,7 @@ name = "rorientation"
 version = "0.1.0"
 dependencies = [
  "image 0.17.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "serial 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)",
 ]
 
 [[package]]
@@ -223,6 +232,52 @@ name = "scopeguard"
 version = "0.3.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 
+[[package]]
+name = "serial"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "serial-core 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "serial-unix 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "serial-windows 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "serial-core"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "libc 0.2.33 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "serial-unix"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "ioctl-rs 0.1.6 (registry+https://github.com/rust-lang/crates.io-index)",
+ "libc 0.2.33 (registry+https://github.com/rust-lang/crates.io-index)",
+ "serial-core 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "termios 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "serial-windows"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "libc 0.2.33 (registry+https://github.com/rust-lang/crates.io-index)",
+ "serial-core 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "termios"
+version = "0.2.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "libc 0.2.33 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
 [metadata]
 "checksum adler32 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)" = "6cbd0b9af8587c72beadc9f72d35b9fbb070982c9e6203e46e93f10df25f8f45"
 "checksum bitflags 0.7.0 (registry+https://github.com/rust-lang/crates.io-index)" = "aad18937a628ec6abcd26d1489012cc0e18c21798210f491af69ded9b881106d"
@@ -238,6 +293,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 "checksum gif 0.9.2 (registry+https://github.com/rust-lang/crates.io-index)" = "e2e41945ba23db3bf51b24756d73d81acb4f28d85c3dccc32c6fae904438c25f"
 "checksum image 0.17.0 (registry+https://github.com/rust-lang/crates.io-index)" = "d1576ffa01849c91b484b95c01d54dddc242b4d50923eaa2d4d74a58c4b9e8fd"
 "checksum inflate 0.3.3 (registry+https://github.com/rust-lang/crates.io-index)" = "10ec05638adf7c5c788bc0cfa608cd479a13572beda20feb4898fe1d85d2c64b"
+"checksum ioctl-rs 0.1.6 (registry+https://github.com/rust-lang/crates.io-index)" = "f7970510895cee30b3e9128319f2cefd4bde883a39f38baa279567ba3a7eb97d"
 "checksum jpeg-decoder 0.1.13 (registry+https://github.com/rust-lang/crates.io-index)" = "2805ccb10ffe4d10e06ef68a158ff94c255211ecbae848fbde2146b098f93ce7"
 "checksum lazy_static 0.2.10 (registry+https://github.com/rust-lang/crates.io-index)" = "236eb37a62591d4a41a89b7763d7de3e06ca02d5ab2815446a8bae5d2f8c2d57"
 "checksum libc 0.2.33 (registry+https://github.com/rust-lang/crates.io-index)" = "5ba3df4dcb460b9dfbd070d41c94c19209620c191b0340b929ce748a2bcd42d2"
@@ -253,3 +309,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 "checksum rayon-core 1.3.0 (registry+https://github.com/rust-lang/crates.io-index)" = "e64b609139d83da75902f88fd6c01820046840a18471e4dfcd5ac7c0f46bea53"
 "checksum scoped_threadpool 0.1.8 (registry+https://github.com/rust-lang/crates.io-index)" = "4ea459fe3ceff01e09534847c49860891d3ff1c12b4eb7731b67f2778fb60190"
 "checksum scopeguard 0.3.3 (registry+https://github.com/rust-lang/crates.io-index)" = "94258f53601af11e6a49f722422f6e3425c52b06245a5cf9bc09908b174f5e27"
+"checksum serial 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)" = "a1237a96570fc377c13baa1b88c7589ab66edced652e43ffb17088f003db3e86"
+"checksum serial-core 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)" = "3f46209b345401737ae2125fe5b19a77acce90cd53e1658cda928e4fe9a64581"
+"checksum serial-unix 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)" = "f03fbca4c9d866e24a459cbca71283f545a37f8e3e002ad8c70593871453cab7"
+"checksum serial-windows 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)" = "15c6d3b776267a75d31bbdfd5d36c0ca051251caafc285827052bc53bcdc8162"
+"checksum termios 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)" = "d5d9cf598a6d7ce700a4e6a9199da127e6819a61e64b68609683cc9a01b5683a"

Cargo.toml

@@ -5,3 +5,4 @@ authors = ["Robert Hedman <robert.hedman@mac.com>"]
 
 [dependencies]
 image = "*"
+serial = "0.4"

src/main.rs

@@ -7,6 +7,13 @@ use std::io::prelude::*;
 use std::net::TcpStream;
 use std::net::TcpListener;
 
+// stuff for serial
+extern crate serial;
+use std::io;
+use std::time::Duration;
+use std::io::prelude::*;
+use serial::prelude::*;
+
 use std::str;
 
 // should the map matrix be two dim array of const size, vecs of dynamic, or what?
@@ -16,6 +23,30 @@ const ROWS: usize = 50;
 const COLS: usize = 100;
 
 fn main() {
+    // dummy thread for replacing lidar
+    let server = thread::spawn(|| {
+        let listener = TcpListener::bind("127.0.0.1:8080").unwrap();
+        let mut stream;
+        let mut message =  [0u8; 2];
+        match listener.accept() {
+            Ok((socket, addr)) => {
+                println!("server: new client: {:?}", addr); stream=socket;
+                thread::sleep(time::Duration::new(1,0));
+                loop {
+                    match stream.read_exact(&mut message) {
+                        Ok(msg) => {println!("server got: {} {}", message[0], message[1]);
+                        let written_size = stream.write(&[69 as u8, 96 as u8]);
+                        println!("server wrote {:?} to stream.", written_size);
+                        },
+                        Err(e) => {println!("Some error in readToString in swerver: {}", e)},
+                    }
+                }
+            },
+            Err(e) => println!("couldn't get client: {:?}", e),
+        }
+    });
+
+
     let mut map = [[0u32; ROWS]; COLS];
     map[0][0] = 3;
 
@@ -27,18 +58,32 @@ fn main() {
     // location tracker
     let tracker = thread::spawn(move || {
         println!("Tracker started.");
+        
+        let mut serialBuf: [u8; 8] = [0; 8];
+        let device = String::from("/dev/cu.usbmodem41");
+        let mut port = serial::open(&device).unwrap();
+        interact(&mut port).unwrap();
+
+        let mut L: i32 = 0;
+        let mut R: i32 = 0;
+        
         let mut pos: (i32, i32) = (0,0);
         loop {
-            // get encoder data from arduino
-            let mut t: i32 = 0;
-            for _ in 0..1000 {
-                t = t+1;
-            };
-            for _ in 0..1000 {
-                t = t-1;
-            };
+            // get encoder data from arduino and update local variables
+            match port.read_exact(&mut serialBuf){
+                Ok(_) => {
+                    L = freq_to_distance( ( ((serialBuf[0] as u32) << 24) + ((serialBuf[1] as u32) << 16) + ((serialBuf[2] as u32) << 8) + (serialBuf[3] as u32) ) as i32);
+                    R = freq_to_distance( ( ((serialBuf[4] as u32) << 24) + ((serialBuf[5] as u32) << 16) + ((serialBuf[6] as u32) << 8) + (serialBuf[7] as u32) ) as i32);
+                    print!("L: {}, ", L);
+                    println!("R: {}", R);
+                    pos.0 += L;
+                    pos.1 += R;
+                    },
+                Err(e) => {
+                    println!("Could not read all bytes: {:?}", e);
+                },
+            }
 
-            // update local variables
 
             // check if position is requested
             match request_location_rx.try_recv() {
@@ -60,11 +105,20 @@ fn main() {
         println!("tracker exiting with local pos: {:?}", pos);
    
     });
+
+
+
     // mapper (gets lidar data, requests position and updates its map.)
     let mapper = thread::spawn(move || {
         println!("mapper started.");
         // create empty map
         let mut pos: (i32, i32); //= (0,0);
+
+        // set up connection to lidar
+        let mut stream = TcpStream::connect("127.0.0.1:8080").unwrap();
+        let mut lidarBuf: [u8; 2] = [0; 2];
+
+        // main loop for this thread
         loop {
             // request position data
             request_location_tx.send(true).unwrap();
@@ -75,15 +129,28 @@ fn main() {
             
 
             // request lidar data
+            thread::sleep(time::Duration::new(3,0));     
+            let stream_write_response = stream.write(&[65 as u8, 66 as u8]);
+            println!("mapper wrote {:?} to stream.", stream_write_response);
+
+            match stream.read_exact(&mut lidarBuf){
+                Ok(_) => {
+                    println!("mapper got {}, {}, from lidar.", lidarBuf[0], lidarBuf[1]);
+                    },
+                Err(e) => {
+                    println!("Could not read all bytes: {:?}", e);
+                },
+            }
 
             // update map
 
+
             if pos.0 == 2000 {
                 println!("mapper done.");
                 request_location_tx.send(false).unwrap();
                 break;
             }
-            //thread::sleep(time::Duration::new(0,50*1000000)); // from ms to ns
+            thread::sleep(time::Duration::new(1,0*50*1000000)); // from ms to ns
         };
 
         
@@ -91,57 +158,6 @@ fn main() {
     });
 
 
-    let server = thread::spawn(|| {
-        let listener = TcpListener::bind("127.0.0.1:8080").unwrap();
-        let mut stream;
-        let mut message =  [0u8; 128];
-        match listener.accept() {
-            Ok((socket, addr)) => {
-                println!("server: new client: {:?}", addr); stream=socket;
-                thread::sleep(time::Duration::new(1,0));
-                loop {
-                    match stream.read(&mut message) {
-                        Ok(msg) => {println!("server got: {:?}, local message: {}", msg, str::from_utf8(&message).unwrap());
-                        let written_size = stream.write_all("Server says hi.\n".as_bytes());
-                        println!("server wrote_all {:?} to stream.", written_size);
-                        },
-                        Err(e) => {println!("Some error in readToString in swerver: {}", e)},
-                    }
-                }
-            },
-            Err(e) => println!("couldn't get client: {:?}", e),
-        }
-    });
-
-    thread::sleep(time::Duration::new(3,0));
-    let mut stream = TcpStream::connect("127.0.0.1:8080").unwrap();
-    //thread::sleep(time::Duration::new(0,900*1000000));
-    //let written_size = stream.write(&['y' as u8,'o' as u8, '!' as u8]);
-    //println!("main wrote {:?} bytes to stream.", written_size);
-
-    let written_size = stream.write_all("Main says hi.".as_bytes());
-    println!("main wrote_all {:?} to stream.", written_size);
-
-    let mut stream_copy = stream.try_clone().unwrap();
-    let mut c: u8 = 0;
-    for b in stream.bytes() {
-        match b {
-            Ok(byte) => {print!("{}", byte as char);
-            if c == 14 {
-                c = 0;
-                let _ = stream_copy.write_all("Hi Server, Me Main, got your message!".as_bytes());
-            }
-            else {
-                c += 1;
-            }
-            
-            },
-            Err(_) => {},
-        }
-    }
-    
-
-
     println!("Main wating for join.");
     mapper.join().unwrap();
     tracker.join().unwrap();
@@ -156,3 +172,25 @@ fn main() {
     //
 
 }
+
+
+
+
+fn interact<T: SerialPort>(port: &mut T) -> io::Result<()> {
+    try!(port.reconfigure(&|settings| {
+        try!(settings.set_baud_rate(serial::Baud9600));
+        settings.set_char_size(serial::Bits8);
+        settings.set_parity(serial::ParityNone);
+        settings.set_stop_bits(serial::Stop1);
+        settings.set_flow_control(serial::FlowNone);
+        Ok(())
+    }));
+ try!(port.set_timeout(Duration::from_millis(10000)));
+
+    Ok(())
+}
+
+
+fn freq_to_distance(freq: i32) -> i32 {
+    freq/10
+}
\ No newline at end of file