diff --git a/ArduinoDriver/ArduinoDriver.ino b/ArduinoDriver/ArduinoDriver.ino
index dec8a02c746203aece320a53bbaa6187e7aa05c0..f9d9c51a129a257ca353e1f872b7ee7ad42f777e 100644
--- a/ArduinoDriver/ArduinoDriver.ino
+++ b/ArduinoDriver/ArduinoDriver.ino
@@ -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);
diff --git a/src/main.rs b/src/main.rs
index 39f787dbe78d36f175b2d2fe69eab80017707971..3fad1aad7f737e2716e0c16b48d744b045ad1c5a 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -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.");