Arduino files initial sync

ArduinoDriver/ArduinoDriver.ino

+#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);
+}

interruptRust/interruptRust.ino

+
+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();
+  }
+}

src/main.rs

@@ -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;