Commit c2e1a9bb authored by Robert Hedman's avatar Robert Hedman

Arduino files initial sync

parent 0f04818d
#include <elapsedMillis.h>
#include <Servo.h>
#define BAUDRATE 57600 // Serial baudrate
#define SIZE 12 // 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
};
// Byte array to hold incoming serial data
byte bytes[SIZE];
// Data arrays to hold motor values
int mData[mNum];
int sData[sNum];
// Servos
Servo servos[sNum];
// 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);
// Write initial DC motor values
//digitalWrite(E1, LOW);
//digitalWrite(E2, LOW);
motor2.attach(E2);
motor1.attach(E1);
// Initialize serial
Serial.begin(BAUDRATE);
// Serial.println("COMMUNICATION ESTABLISHED !!\n");
}
void loop(void)
{
// Clear bytes array
for (int i = 0; i < SIZE; i++) {
bytes[i] = 0;
}
// Check for connection problems
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);
}
delay(10);
}
// Fill bytes array with serial data
while (Serial.available() >= SIZE) {
for (int i = 0; i < SIZE; i++) {
bytes[i] = Serial.read();
}
}
// 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");
}
// 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;
}
//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);
}
const byte interruptPinL = 3;
const byte interruptPinR = 8;
volatile long countL = 0;
volatile long countR = 0;
byte *countPtrL = (byte*) &countL;
byte *countPtrR = (byte*) &countR;
const long timeThresh = 500; //ms
long lastTime = 0;
volatile long lastCountL = 0;
volatile long lastCountR = 0;
const long countTimeThresh = 100;
void setup() {
pinMode(interruptPinL, INPUT_PULLUP);
pinMode(interruptPinR, INPUT_PULLUP);
Serial.begin(9600);
lastTime = millis();
attachInterrupt(digitalPinToInterrupt(interruptPinL), myInterruptL, RISING);
attachInterrupt(digitalPinToInterrupt(interruptPinR), myInterruptR, RISING);
}
void loop() {
if(millis() - lastTime > timeThresh) {
lastTime = millis();
for(byte i=3;i<4;i--) { // underflows
Serial.write(*(countPtrL+i));
}
for(byte i=3;i<4;i--) { // underflows
Serial.write(*(countPtrR+i));
}
countL=0;
countR=0;
}
}
void myInterruptL() {
if(millis() - lastCountL > countTimeThresh) {
countL+=50;
lastCountL = millis();
}
}
void myInterruptR() {
if(millis() - lastCountR > countTimeThresh) {
countR+=50;
lastCountR = millis();
}
}
......@@ -17,7 +17,6 @@ use serial::prelude::*;
//use std::str;
/////////////////////////////
/////// constants ///////
/////////////////////////////
......@@ -68,6 +67,8 @@ fn main() {
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);
let left_motor = Pin::new(127); // number depends on chip, etc.
let right_motor = Pin::new(128);
match listener.accept() {
Ok((socket, addr)) => {
println!("controller: new client: {:?}", addr); stream=socket;
......
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