From d1f2103d6f9378010e23b33f05990f5ba3e711d7 Mon Sep 17 00:00:00 2001
From: Robert Hedman <robert.hedman@mac.com>
Date: Sun, 26 Nov 2017 23:16:37 +0100
Subject: [PATCH] Functioning emulated lidar and serial communication threads
in one program.
---
Cargo.lock | 61 ++++++++++++++++++++
Cargo.toml | 1 +
src/main.rs | 160 ++++++++++++++++++++++++++++++++--------------------
3 files changed, 161 insertions(+), 61 deletions(-)
diff --git a/Cargo.lock b/Cargo.lock
index 0b7acd1..cde1a0a 100644
--- a/Cargo.lock
+++ b/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"
diff --git a/Cargo.toml b/Cargo.toml
index c8e2825..4dd9285 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -5,3 +5,4 @@ authors = ["Robert Hedman <robert.hedman@mac.com>"]
[dependencies]
image = "*"
+serial = "0.4"
diff --git a/src/main.rs b/src/main.rs
index 44a5cce..845340a 100644
--- a/src/main.rs
+++ b/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();
@@ -155,4 +171,26 @@ 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
--
GitLab