#include <SoftwareSerial.h>
#include <Wire.h>

const int RE[] = {0,7,2};
const int DE[] = {0,8,3};
const byte codes[] = {0x01 ,0x03 ,0x00 ,0x00 ,0x00 ,0x02 ,0xC4 ,0x0B};
 
byte values[3][20];
SoftwareSerial mod1(10, 11); // RO / DI
SoftwareSerial mod2(5, 6); // RO / DI
 
void setup() {
  Serial.begin(4800);
  while(!Serial){;}
  mod1.begin(4800);
  delay(500);
  mod2.begin(4800);
  delay(500);
  pinMode(RE[1], OUTPUT);
  pinMode(DE[1], OUTPUT);
  pinMode(RE[2], OUTPUT);
  pinMode(DE[2], OUTPUT);
}
 
void loop() {
  int val1 = 0 ; // SPEED
  int val2 = 0 ; // DIR

  readSensor(1);
  //val1 = ((values[1][5]*256)+values[1][6]);
  val1 = values[1][4];
  Serial.print("speed: ");
  Serial.println(val1);

  delay(1000);
  readSensor(2);
  val2 = values[2][4];
  Serial.print("dir: ");
  Serial.println(val2);

  delay(1000);
}
 
byte readSensor(int sensor) {
  digitalWrite(DE[sensor], HIGH);
  digitalWrite(RE[sensor], HIGH);
  delay(10);
  if(sensor == 1) {
    if (mod1.write(codes, sizeof(codes)) == 8) {
      digitalWrite(DE[sensor], LOW);
      digitalWrite(RE[sensor], LOW);
      for (byte i = 0; i < 11; i++) {
        values[sensor][i] = mod1.read();
        Serial.print(values[sensor][i]);
        Serial.print(" ");
      }
    }
  } else if(sensor == 2) {
    if (mod2.write(codes, sizeof(codes)) == 8) {
      digitalWrite(DE[sensor], LOW);
      digitalWrite(RE[sensor], LOW);
      for (byte i = 0; i < 11; i++) {
        values[sensor][i] = mod2.read();
        Serial.print(values[sensor][i]);
        Serial.print(" ");
      }
    }
  }
  Serial.println(" ");
}