tetrix_return_encoder_count_0x49

//в цикле крутит мотор туда сюда и считывает значения энкодера
//delay 2000 дан чтобы успела выполниться команда
#include <Wire.h>
uint8_t id = 1;
void setup() {
      Wire.begin();//инициализиурем библиотеку wire
      Serial.begin(9600);
}

void loop() {
      Wire.beginTransmission(id);
      Wire.write(0x25);//controller enable
      Wire.endTransmission();

  Wire.beginTransmission(id);

  //encoder count motor1, всего 6 байт в команде
  Wire.write(0x46);

  Wire.write(0x01);//два первые байта это скорость в градусах в сек
  Wire.write(0x68);//hex0168 - это 360 гр/сек
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x05);
  Wire.write(0xa0);
  Wire.endTransmission();
  delay(2000);

  Wire.beginTransmission(id);
  Wire.write(0x49);//Returns the Motor 1 encoder count
  Wire.endTransmission();
  Wire.requestFrom(id,4); //читаем 4 байта
  Serial.print("current position_1 = ");
  while(Wire.available())
  {
    Serial.print((int)Wire.read(),HEX);
  }
  Serial.println();

//второй оборот
  Wire.beginTransmission(id);
  Wire.write(0x46);
  Wire.write(0x01);//два первые байта это скорость в градусах в сек
  Wire.write(0x68);//hex0168 - это 360 гр/сек
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x0b);
  Wire.write(0x40);
  Wire.endTransmission();
  delay(2000);

  Wire.beginTransmission(id);
  Wire.write(0x49);//Returns the Motor 1 encoder count
  Wire.endTransmission();
  Wire.requestFrom(id,4); //читаем 4 байта
  Serial.print("current position_2 = ");
  while(Wire.available())
  {
    Serial.print((int)Wire.read(),HEX);
  }
  Serial.println();
}