diff --git a/src/main.rs b/src/main.rs
index 111be79b15c5ce6ac4c6ea5d36fbe77d0a0cd436..af0b939a0dc2ab4e005c260d39f70bff265ea4e2 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -17,25 +17,125 @@ use serial::prelude::*;
//use std::str;
-// constants
+/////////////////////////////
+/////// constants ///////
+/////////////////////////////
+
+//lidar
const LIDAR_ADDRESS: &str = "127.0.0.1:8080";
const COUNTER_SERIAL_PORT: &str = "/dev/cu.usbmodem1422";
const LIDAR_BUF_SIZE: usize = 4096;
+//controller
+const REMOTE_OPERATOR_ADDRESS: &str = "10.9.0.3:30000"; // remember to double check port!
+const CONTROLLER_SAMPLING_TIME: u32 = 200; //ms
+
// each pixel represents 1dm*1dm, so 100*100mm.
const ROWS: usize = 1000;
const COLS: usize = 1000;
// robot real stuff
-const AXEL_WIDTH: f32 = 175; // cm
+const AXEL_WIDTH: f32 = 175.0; // cm
const FREQ_TO_OMEGA: f32 = 0.0002;
const WHEEL_RADIUS: f32 = 30.0; //cm
-#![feature(box_syntax)]
+/////////////////////////////
+/////// structs ///////
+/////////////////////////////
+struct stateStruct {
+ x: f32,
+ y: f32,
+ theta: f32,
+ xdot: f32,
+ ydot: f32,
+}
+
+//#![feature(box_syntax)]
fn main() {
- let mut map= box ([[0u32; ROWS]; COLS]);
+ let mut map = Box::new([[0u32; ROWS]; COLS]);
+
+ // communicators between threads
+ let (mapper_request_location_tx, request_location_rx): (mpsc::Sender<u8>, mpsc::Receiver<u8>) = mpsc::channel();
+ let controller_request_location_tx = mapper_request_location_tx.clone();
+
+ let (controller_send_location_tx, controller_send_location_rx): (mpsc::Sender<(f32, f32, f32, f32, f32)>, mpsc::Receiver<(f32, f32, f32, f32, f32)>) = mpsc::channel();
+ let (mapper_send_location_tx, mapper_send_location_rx): (mpsc::Sender<(f32, f32, f32, f32, f32)>, mpsc::Receiver<(f32, f32, f32, f32, f32)>) = mpsc::channel();
+
+
+ let controller = thread::spawn(move || {
+ let listener = TcpListener::bind(REMOTE_OPERATOR_ADDRESS).unwrap();
+ let mut stream;
+ let mut state: (f32, f32, f32, f32, f32) = (COLS as f32/2.0,ROWS as f32/2.0, 0.0, 0.0, 0.0);
+ match listener.accept() {
+ Ok((socket, addr)) => {
+ println!("controller: new client: {:?}", addr); stream=socket;
+
+ // the message format is defined as: (as incoming bytes)
+ // [SOM, L_dir, L_speed, R_dir, R_speed, EOM]
+ // where: L_dir = 0 meanst stop, 1 forward and 2 rev, same for R
+ // speed is a byte from 0-200 determining throttle position (201 - 255 are reserved.)
+ // special bytes:
+ // SOM (start of message): 250, EOM (end of message): 251
+ const message_length: usize = 6;
+
+ let mut message: [u8; message_length] = [0; message_length];
+ let mut received_bytes = 0;
+ let mut recording = false;
+
+ loop {
+ // get controller input (We expect to set soloMode here.)
+ for r in stream.try_clone().expect("stream clone failed").bytes() {
+ match r {
+ Ok(b) => { // TODO Add case if EOM is missing
+ if b == 250 && !recording {
+ //println!("Server found start of message");
+ message[received_bytes] = b;
+ recording = true;
+ received_bytes+=1;
+ }
+ else if b == 251 {
+ message[received_bytes] = b;
+ received_bytes+=1; // since length now is total bytes, not index
+ //println!("Server found end of message");
+ println!("Cotroller got: {:?}", String::from_utf8_lossy(&message[0..received_bytes]));
+ //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.");
+
+ // reset everything for next message.
+ recording = false;
+ received_bytes = 0;
+ break;
+ }
+ else if recording {
+ message[received_bytes] = b;
+ received_bytes+=1;
+ }
+
+ },
+ Err(_) => {}, //TODO
+ }
+ }
+
+
+ // get motors current speeds
+ println!("Controller requesting pos data.");
+ controller_request_location_tx.send(10 as u8).unwrap();
+ state = controller_send_location_rx.recv().unwrap();
+
+ // PID controller
+
+ // send voltage to motors.
+
+ }
+
+
+ },
+ Err(e) => println!("couldn't get client: {:?}", e), // TODO: what if controller braks? Thread ends?
+ }
+ });
//let mut vecMap = vec![vec![0; ROWS]; COLS];
@@ -98,31 +198,28 @@ 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, f32)>, mpsc::Receiver<(i32, i32, f32)>) = mpsc::channel();
-
-
// location tracker
let tracker = thread::spawn(move || {
println!("Tracker started.");
- let mut serial_buf: [u8; 10] = [0; 10];
+ let mut serial_buf: [u8; 8] = [0; 8];
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);
+ // state: x, y, theta, xdot, ydot
+ let mut state: (f32, f32, f32, f32, f32) = (COLS as f32/2.0,ROWS as f32/2.0, 0.0, 0.0, 0.0);
+ println!("location initial position: {:?}", state);
loop {
// get encoder data from arduino and update local variables
match port.read_exact(&mut serial_buf){
Ok(_) => {
- let l_speed: f32 = tick_to_speed( serial_buf[0], ( ((serial_buf[1] as u32) << 24) + ((serial_buf[2] as u32) << 16) + ((serial_buf[3] as u32) << 8) + (serial_buf[4] as u32) ) as i32);
- let r_speed: f32 = tick_to_speed( serial_buf[5], ( ((serial_buf[6] as u32) << 24) + ((serial_buf[7] as u32) << 16) + ((serial_buf[8] as u32) << 8) + (serial_buf[9] as u32) ) as i32);
- pos.2 = update_state(&pos, l_tick, r_tick);
- print!("L: {}, ", pos.0);
- println!("R: {}", pos.1);
+ let l_tick: 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_tick: 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);
+ update_state(&mut state, l_tick, r_tick);
+ print!("L: {}, ", state.0);
+ println!("R: {}", state.1);
},
Err(e) => {
println!("Could not read all bytes: {:?}", e);
@@ -131,14 +228,24 @@ fn main() {
// check if position is requested
+ // a byte will be sent, codes are:
+ // 10: mapper, 11: controller
match request_location_rx.try_recv() {
Ok(msg) => {
- if msg == false {
- println!("tracker got stop signal.");
- break;
- };
- send_location_tx.send((pos)).unwrap();
- //pos.0 += 1;
+ match msg {
+ 0 => {
+ println!("tracker got stop signal.");
+ break;
+ },
+ 10 =>{
+ mapper_send_location_tx.send((state)).unwrap();
+ },
+ 11 => {
+ controller_send_location_tx.send((state)).unwrap();
+ },
+ _ => {}, // nothing to send.
+ }
+
},
Err(_) => {
//println!("Error: {}, on recieving side.", e);
@@ -147,7 +254,7 @@ fn main() {
},
};
};
- println!("tracker exiting with local pos: {:?}", pos);
+ println!("tracker exiting with local pos: {:?}", state);
});
@@ -158,7 +265,7 @@ fn main() {
println!("mapper started.");
// create empty map
- let mut pos: (i32, i32, f32); //= (0,0);
+ let mut pos: (f32, f32, f32, f32, f32); //= (0,0);
// set up connection to lidar
let mut stream = TcpStream::connect(&LIDAR_ADDRESS).unwrap();
@@ -170,12 +277,10 @@ fn main() {
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 {
- println!("mapper pos: {:?} ",pos);
- };
+ mapper_request_location_tx.send(10 as u8).unwrap();
+ pos = mapper_send_location_rx.recv().unwrap();
+ println!("Mapper is at length: {}", message_len);
// request lidar data
//thread::sleep(time::Duration::new(3,0));
@@ -203,7 +308,7 @@ fn main() {
// reset everything for next message.
recording = false;
- let message_len: usize = 0;
+ message_len = 0;
break;
}
else if recording {
@@ -220,12 +325,9 @@ fn main() {
// update map
+ // to shutdown tracker:
+ // request_location_tx.send(false).unwrap();
- if pos.0 == 2000 {
- println!("mapper done.");
- request_location_tx.send(false).unwrap();
- break;
- }
thread::sleep(time::Duration::new(2,0*50*1000000)); // from ms to ns
};
@@ -242,6 +344,8 @@ fn main() {
server.join().unwrap();
println!("end of main");
+ controller.join().unwrap();
+
// pathfinder, takes a map and returns a list of nodes to aim for when driving
// This will be a function for mapper to call.
@@ -271,14 +375,19 @@ const FREQ_TO_OMEGA: f32 = 0.0002;
const WHEEL_RADIUS: f32 = 30.0; //cm
*/
-fn tick_to_speed(dir: u8, tick: i32) -> f32 {
- tick/10.0
+fn tick_to_speed(tick: i32) -> f32 {
+ tick as f32 /10.0
}
-fn update_state(pos: &(f32,f32,f32), l_speed: f32, r_speed: f32) {
- pos.0 += 0.5*(l_speed + r_speed)*((pos.2).to_radians().sin());
- pos.1 += 0.5*(l_speed + r_speed)*((pos.2).to_radians().cos());
+fn update_state(pos: &mut (f32,f32,f32, f32, f32), l_speed: f32, r_speed: f32) {
+ let diffx = 0.5*(l_speed + r_speed)*((pos.2).to_radians().sin());
+ let diffy = 0.5*(l_speed + r_speed)*((pos.2).to_radians().cos());
+ pos.3 = (diffx - pos.0) / (CONTROLLER_SAMPLING_TIME as f32 / 1000.0); // sampl time is in ms.
+ pos.4 = (diffy - pos.1) / (CONTROLLER_SAMPLING_TIME as f32 / 1000.0);
+ pos.0 += diffx;
+ pos.1 += diffy;
pos.2 += (r_speed - l_speed)/AXEL_WIDTH;
+
}
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
+}