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

const int enable[] = {0,8,9};
const byte codes[] = {0x01 ,0x03 ,0x00 ,0x00 ,0x00 ,0x02 ,0xC4 ,0x0B};
 
byte values[3][20];
SoftwareSerial sensor(10, 11); // RO / DI
 
void setup() {
  Serial.begin(4800);
  while(!Serial){;}
  sensor.begin(4800);
  delay(500);
  pinMode(enable[1], OUTPUT);
  pinMode(enable[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("dir: ");
  Serial.println(val1);

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

  delay(1000);

  
}
 
byte readSensor(int index) {
  digitalWrite(enable[index], HIGH);
  delay(10);
  if(sensor.write(codes, sizeof(codes)) == 8) {
    digitalWrite(enable[index], LOW);
    for (byte i = 0; i < 11; i++) {
      values[index][i] = sensor.read();
      Serial.print(values[index][i]);
      Serial.print(" ");
    }
  }
  Serial.println(" ");
}