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

Added code fro arduinos, added naming of threads.

parent 47238a29
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
ArduinoDriver/ArduinoDriver.ino
+ 57
− 113
View file @ da985306
......@@ -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,65 +66,46 @@ 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");
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 {
Serial.print("Corrupt data!\n");
motors[0] = -1;
motors[1] = round(2.5*bytes[2]);
}
// 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 (bytes[3] == 0) {
motors[2] = 0;
motors[3] = 1;
if (mData[3] == 1) {
//digitalWrite(M2, HIGH);
mData[3] = 1;
} else if (mData[3] == 0) {
//digitalWrite(M2, LOW);
mData[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]);
}
//analogWrite(E2, mData[2]);
motor1.writeMicroseconds(1500+mData[1]*mData[0]*2);
motor2.writeMicroseconds(1500+mData[3]*mData[2]*2);
//delay(6);
motorL.writeMicroseconds(1500+motors[0]*motors[1]);
motorR.writeMicroseconds(1500+motors[2]*motors[3]);
for (int i = 0; i < sNum; i++)
servos[i].write(sData[i]);
while (Serial.available() > 0) {
char t = Serial.read();
} else {
motorL.writeMicroseconds(1500);
motorR.writeMicroseconds(1500);
}
timer0 = 0;
delay(10);
}
src/main.rs
+ 53
− 17
View file @ da985306
......@@ -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 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.");
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment