Commit da985306 authored by Robert Hedman's avatar Robert Hedman

Added code fro arduinos, added naming of threads.

parent 47238a29
......@@ -2,70 +2,39 @@
#include <Servo.h>
#define BAUDRATE 57600 // Serial baudrate
#define SIZE 12 // Size of incoming data in bytes
#define SIZE 6 // Size of incoming data in bytes
// DC motor pins
#define E1 5 //M1 Speed Control
#define E2 6 //M2 Speed Control
Servo motor1;
Servo motor2;
// Servo pins
#define sNum 6
#define mNum 4
int sPins[] = { 2, // arm
3, // claw
8, // bottom
9, // top
10, // cagelock
11 // droplock
};
int sInit[] = { 90,
90,
90,
90,
90,
90
};
#define ML 5 //M1 Speed Control
#define MR 6 //M2 Speed Control
// Byte array to hold incoming serial data
byte bytes[SIZE];
Servo motorL;
Servo motorR;
// Data arrays to hold motor values
int mData[mNum];
int sData[sNum];
// Serial data in will be 6 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
// Servos
Servo servos[sNum];
// Byte array to hold incoming serial data
byte bytes[SIZE];
short motors[SIZE-2];
// Timer for communications problems check
elapsedMillis timer0;
void setup(void)
{
// Initialize DC motor
for (int i = 0; i < mNum; i++)
mData[i] = 0;
// Initialize servos
for (int i = 0; i < sNum; i++) {
sData[i] = sInit[i];
servos[i].attach(sPins[i]);
servos[i].write(sData[i]);
}
// Initialize DC motor pins
for (int i = 4; i <= 7; i++)
pinMode(i, OUTPUT);
pinMode(ML, OUTPUT);
pinMode(MR, OUTPUT);
motorL.attach(ML);
motorR.attach(MR);
// Write initial DC motor values
//digitalWrite(E1, LOW);
//digitalWrite(E2, LOW);
motor2.attach(E2);
motor1.attach(E1);
for (int i = 0; i < SIZE-2; i++) {
motors[i] = 0;
}
// Initialize serial
Serial.begin(BAUDRATE);
......@@ -79,18 +48,12 @@ void loop(void)
bytes[i] = 0;
}
// Check for connection problems
// Check for connection problems, if nothing in for more than .1 secs stop motors
while (Serial.available() < SIZE) {
if (timer0 > 100) {
timer0 = 0;
mData[0] = 0;
mData[2] = 0;
//digitalWrite(E1, mData[0]);
//digitalWrite(E2, mData[2]);
motor1.writeMicroseconds(1500);
motor2.writeMicroseconds(1500);
motorL.writeMicroseconds(1500);
motorR.writeMicroseconds(1500);
}
delay(10);
......@@ -103,64 +66,45 @@ void loop(void)
}
}
// 0 meanst stop, 1 forward and 2 rev
// SOM, L_dir, L_speed, R_dir, R_speed, EOM
// If serial data looks ok, fill data array
if (bytes[0] == 'b' && bytes[SIZE - 1] == 'e') {
for (int i = 0; i < mNum; i++)
mData[i] = bytes[i + 1];
for (int i = 0; i < sNum; i++)
sData[i] = bytes[i + mNum + 1];
Serial.print("OK\n");
// Serial.print("mData: ");
// // Print contents of mData to serial
// for(int i = 0; i < 4; i++) {
// Serial.print(mData[i]);
// Serial.print(" ");
// }
// Serial.print(" sData: ");
// // Print contents of sData to serial
// for(int i = 0; i < sNum; i++) {
// Serial.print(sData[i]);
// Serial.print(" ");
// }
// Serial.praint("\n");
} else {
Serial.print("Corrupt data!\n");
}
if (bytes[0] == 250 && bytes[SIZE - 1] == 251) {
if (bytes[1] == 0) {
motors[0] = 0;
motors[1] = 1;
} else if (bytes[1] == 1) {
motors[0] = 1;
motors[1] = round(2.5*bytes[2]);
} else {
motors[0] = -1;
motors[1] = round(2.5*bytes[2]);
}
if (bytes[3] == 0) {
motors[2] = 0;
motors[3] = 1;
} else if (bytes[3] == 1) {
motors[2] = 1;
motors[3] = round(2.5*bytes[4]);
} else {
motors[2] = -1;
motors[3] = round(2.5*bytes[4]);
}
motorL.writeMicroseconds(1500+motors[0]*motors[1]);
motorR.writeMicroseconds(1500+motors[2]*motors[3]);
// Write DC motor values
//analogWrite(E1, mData[0]);
if (mData[1] == 1) {
//digitalWrite(M1, HIGH);
mData[1] = 1;
} else if (mData[1] == 0) {
//digitalWrite(M1, LOW);
mData[1] = -1;
}
if (mData[3] == 1) {
//digitalWrite(M2, HIGH);
mData[3] = 1;
} else if (mData[3] == 0) {
//digitalWrite(M2, LOW);
mData[3] = -1;
} else {
motorL.writeMicroseconds(1500);
motorR.writeMicroseconds(1500);
}
//analogWrite(E2, mData[2]);
motor1.writeMicroseconds(1500+mData[1]*mData[0]*2);
motor2.writeMicroseconds(1500+mData[3]*mData[2]*2);
//delay(6);
for (int i = 0; i < sNum; i++)
servos[i].write(sData[i]);
while (Serial.available() > 0) {
char t = Serial.read();
}
timer0 = 0;
delay(10);
......
......@@ -26,8 +26,8 @@ const LIDAR_ADDRESS: &str = "127.0.0.1:8080";
const LIDAR_BUF_SIZE: usize = 4096;
//Arduinos
const COUNTER_SERIAL_PORT: &str = "/dev/cu.usbmodem1422";
const DRIVER_SERIAL_PORT: &str = "/dev/cu.usbmodem1423";
const COUNTER_SERIAL_PORT: &str = "/dev/cu.usbmodem1412";
const DRIVER_SERIAL_PORT: &str = "/dev/cu.usbmodem1421";
//controller
const REMOTE_OPERATOR_ADDRESS: &str = "10.9.0.3:30000"; // remember to double check port!
......@@ -45,7 +45,7 @@ const WHEEL_RADIUS: f32 = 30.0; //cm
/////////////////////////////
/////// structs ///////
/////////////////////////////
struct stateStruct {
struct RobotState {
x: f32,
y: f32,
theta: f32,
......@@ -66,12 +66,35 @@ fn main() {
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 builder = thread::Builder::new().name("controller".into());
let controller = builder.spawn(move || {
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);
let left_motor = Pin::new(127); // number depends on chip, etc.
let right_motor = Pin::new(128);
let mut state: (f32, f32, f32, f32, f32) = (COLS as f32/2.0,ROWS as f32/2.0, 0.0, 0.0, 0.0);
let mut port: serial::SystemPort;
loop{
match serial::open(&DRIVER_SERIAL_PORT) {
Ok(p) => {port = p; break;},
Err(e) => {
println!("Controller: Could not connect to arduino driver: {}", e);
thread::sleep(time::Duration::new(1,0));
},
}
}
interact(&mut port).unwrap();
let listener: std::net::TcpListener;
loop {
match TcpListener::bind(REMOTE_OPERATOR_ADDRESS) {
Ok(lis) => {listener = lis; break;},
Err(e) => {
println!("Controller: No connection from operator yet. {}", e);
thread::sleep(time::Duration::new(1,0));
},
}
}
match listener.accept() {
Ok((socket, addr)) => {
println!("controller: new client: {:?}", addr); stream=socket;
......@@ -121,7 +144,7 @@ fn main() {
},
Err(_) => {}, //TODO
}
if received_bytes > message_len && recording { // EOM is missing!
if received_bytes > message_length && recording { // EOM is missing!
received_bytes = 0;
recording = false;
}
......@@ -136,6 +159,7 @@ fn main() {
// PID controller
// send voltage to motors.
port.write(&message[0..message_length]).unwrap();
}
......@@ -143,7 +167,7 @@ fn main() {
},
Err(e) => println!("couldn't get client: {:?}", e), // TODO: what if controller braks? Thread ends?
}
});
}).unwrap();
//let mut vecMap = vec![vec![0; ROWS]; COLS];
......@@ -154,7 +178,8 @@ fn main() {
//println!("{}, {}", map[0][0], map[0][1]);
// dummy thread for replacing lidar
let server = thread::spawn(|| {
let builder = thread::Builder::new().name("DummyLidarThread".into());
let server = builder.spawn(|| {
let listener = TcpListener::bind(LIDAR_ADDRESS).unwrap();
let mut stream;
match listener.accept() {
......@@ -203,15 +228,25 @@ fn main() {
},
Err(e) => println!("couldn't get client: {:?}", e),
}
});
}).unwrap();
// location tracker
let tracker = thread::spawn(move || {
let builder = thread::Builder::new().name("Location Tracker Thread".into());
let tracker = builder.spawn(move || {
println!("Tracker started.");
let mut serial_buf: [u8; 8] = [0; 8];
let mut port = serial::open(&COUNTER_SERIAL_PORT).unwrap();
let mut port: serial::SystemPort;
loop{
match serial::open(&COUNTER_SERIAL_PORT) {
Ok(p) => {port = p; break;},
Err(e) => {
println!("LocationTracker: Could not connect to arduino counter: {}", e);
thread::sleep(time::Duration::new(1,0));
},
}
}
interact(&mut port).unwrap();
//let mut l_tick: f32 = 0;
......@@ -264,12 +299,13 @@ fn main() {
};
println!("tracker exiting with local pos: {:?}", state);
});
}).unwrap();
// mapper (gets lidar data, requests position and updates its map.)
let mapper = thread::spawn(move || {
let builder = thread::Builder::new().name("Mapper Thread".into());
let mapper = builder.spawn(move || {
println!("mapper started.");
// create empty map
......@@ -341,7 +377,7 @@ fn main() {
});
}).unwrap();
println!("Main wating for join.");
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment