Skip to content
Snippets Groups Projects
Commit 0f04818d authored by Robert Hedman's avatar Robert Hedman
Browse files

many updates after failed merge

parent 5f9bc9ea
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 150 additions and 41 deletions
src/main.rs
+ 150
− 41
View file @ 0f04818d
...@@ -17,25 +17,125 @@ use serial::prelude::*; ...@@ -17,25 +17,125 @@ use serial::prelude::*;
//use std::str; //use std::str;
// constants
/////////////////////////////
/////// constants ///////
/////////////////////////////
//lidar
const LIDAR_ADDRESS: &str = "127.0.0.1:8080"; const LIDAR_ADDRESS: &str = "127.0.0.1:8080";
const COUNTER_SERIAL_PORT: &str = "/dev/cu.usbmodem1422"; const COUNTER_SERIAL_PORT: &str = "/dev/cu.usbmodem1422";
const LIDAR_BUF_SIZE: usize = 4096; 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. // each pixel represents 1dm*1dm, so 100*100mm.
const ROWS: usize = 1000; const ROWS: usize = 1000;
const COLS: usize = 1000; const COLS: usize = 1000;
// robot real stuff // robot real stuff
const AXEL_WIDTH: f32 = 175; // cm const AXEL_WIDTH: f32 = 175.0; // cm
const FREQ_TO_OMEGA: f32 = 0.0002; const FREQ_TO_OMEGA: f32 = 0.0002;
const WHEEL_RADIUS: f32 = 30.0; //cm 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() { 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]; //let mut vecMap = vec![vec![0; ROWS]; COLS];
...@@ -98,31 +198,28 @@ fn main() { ...@@ -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 // location tracker
let tracker = thread::spawn(move || { let tracker = thread::spawn(move || {
println!("Tracker started."); 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(); let mut port = serial::open(&COUNTER_SERIAL_PORT).unwrap();
interact(&mut port).unwrap(); interact(&mut port).unwrap();
//let mut l_tick: f32 = 0; //let mut l_tick: f32 = 0;
//let mtu r_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); // state: x, y, theta, xdot, ydot
println!("location initial position: {:?}", pos); 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 { loop {
// get encoder data from arduino and update local variables // get encoder data from arduino and update local variables
match port.read_exact(&mut serial_buf){ match port.read_exact(&mut serial_buf){
Ok(_) => { 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 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_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); 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);
pos.2 = update_state(&pos, l_tick, r_tick); update_state(&mut state, l_tick, r_tick);
print!("L: {}, ", pos.0); print!("L: {}, ", state.0);
println!("R: {}", pos.1); println!("R: {}", state.1);
}, },
Err(e) => { Err(e) => {
println!("Could not read all bytes: {:?}", e); println!("Could not read all bytes: {:?}", e);
...@@ -131,14 +228,24 @@ fn main() { ...@@ -131,14 +228,24 @@ fn main() {
// check if position is requested // check if position is requested
// a byte will be sent, codes are:
// 10: mapper, 11: controller
match request_location_rx.try_recv() { match request_location_rx.try_recv() {
Ok(msg) => { Ok(msg) => {
if msg == false { match msg {
0 => {
println!("tracker got stop signal."); println!("tracker got stop signal.");
break; break;
}; },
send_location_tx.send((pos)).unwrap(); 10 =>{
//pos.0 += 1; mapper_send_location_tx.send((state)).unwrap();
},
11 => {
controller_send_location_tx.send((state)).unwrap();
},
_ => {}, // nothing to send.
}
}, },
Err(_) => { Err(_) => {
//println!("Error: {}, on recieving side.", e); //println!("Error: {}, on recieving side.", e);
...@@ -147,7 +254,7 @@ fn main() { ...@@ -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() { ...@@ -158,7 +265,7 @@ fn main() {
println!("mapper started."); println!("mapper started.");
// create empty map // 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 // set up connection to lidar
let mut stream = TcpStream::connect(&LIDAR_ADDRESS).unwrap(); let mut stream = TcpStream::connect(&LIDAR_ADDRESS).unwrap();
...@@ -170,12 +277,10 @@ fn main() { ...@@ -170,12 +277,10 @@ fn main() {
loop { loop {
// request position data // request position data
println!("Mapper requesting pos data."); println!("Mapper requesting pos data.");
request_location_tx.send(true).unwrap(); mapper_request_location_tx.send(10 as u8).unwrap();
pos = send_location_rx.recv().unwrap(); pos = mapper_send_location_rx.recv().unwrap();
if pos.0 % 1000 == 0 {
println!("mapper pos: {:?} ",pos);
};
println!("Mapper is at length: {}", message_len);
// request lidar data // request lidar data
//thread::sleep(time::Duration::new(3,0)); //thread::sleep(time::Duration::new(3,0));
...@@ -203,7 +308,7 @@ fn main() { ...@@ -203,7 +308,7 @@ fn main() {
// reset everything for next message. // reset everything for next message.
recording = false; recording = false;
let message_len: usize = 0; message_len = 0;
break; break;
} }
else if recording { else if recording {
...@@ -220,12 +325,9 @@ fn main() { ...@@ -220,12 +325,9 @@ fn main() {
// update map // 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 thread::sleep(time::Duration::new(2,0*50*1000000)); // from ms to ns
}; };
...@@ -242,6 +344,8 @@ fn main() { ...@@ -242,6 +344,8 @@ fn main() {
server.join().unwrap(); server.join().unwrap();
println!("end of main"); println!("end of main");
controller.join().unwrap();
// pathfinder, takes a map and returns a list of nodes to aim for when driving // pathfinder, takes a map and returns a list of nodes to aim for when driving
// This will be a function for mapper to call. // This will be a function for mapper to call.
...@@ -271,13 +375,18 @@ const FREQ_TO_OMEGA: f32 = 0.0002; ...@@ -271,13 +375,18 @@ const FREQ_TO_OMEGA: f32 = 0.0002;
const WHEEL_RADIUS: f32 = 30.0; //cm const WHEEL_RADIUS: f32 = 30.0; //cm
*/ */
fn tick_to_speed(dir: u8, tick: i32) -> f32 { fn tick_to_speed(tick: i32) -> f32 {
tick/10.0 tick as f32 /10.0
} }
fn update_state(pos: &(f32,f32,f32), l_speed: f32, r_speed: f32) { fn update_state(pos: &mut (f32,f32,f32, f32, f32), l_speed: f32, r_speed: f32) {
pos.0 += 0.5*(l_speed + r_speed)*((pos.2).to_radians().sin()); let diffx = 0.5*(l_speed + r_speed)*((pos.2).to_radians().sin());
pos.1 += 0.5*(l_speed + r_speed)*((pos.2).to_radians().cos()); 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; 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]) { fn update_map_with_lidar(lidar_data: [u8; LIDAR_BUF_SIZE], message_len: usize, map: &mut [[u32; ROWS]; COLS]) {
map[0][0] += 1; map[0][0] += 1;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment