diff --git a/HansonServo.ino b/HansonServo.ino
index e697f7a..d8ab8b4 100644
--- a/HansonServo.ino
+++ b/HansonServo.ino
@@ -107,7 +107,7 @@ void setup() {
   servos[0] = new Feetech(Serial1, DE_PIN, RE_PIN, CH0_TX_PIN, CH0_RX_PIN);  // SCS
   servos[0]->setFeetechMode(Feetech::MODE_SCS);
   servos[1] = new Feetech(Serial2, DE_PIN, RE_PIN, CH1_TX_PIN, CH1_RX_PIN);  // STS
-  servos[1]->setFeetechMode(Feetech::MODE_STS);
+  servos[1]->setFeetechMode(Feetech::MODE_SCS);
 
   // pinMode(RX_PIN, OUTPUT);
   // pinMode(TX_PIN, OUTPUT);
@@ -153,12 +153,20 @@ void setup() {
 
 
   // PLAY ANIMATION
-  anim.loadFromFile("/aa.anim");
-  currentAnimation = &anim;
-  currentAnimation->setActive(true);
-  playMode = PLAY_ONCE;
+  // anim.loadFromFile("/aa.anim");
+  // currentAnimation = &anim;
+  // currentAnimation->setActive(true);
+  // playMode = PLAY_ONCE;
   //Serial.println(anim.header.frameCount);
   //Serial.println(anim.printCurves());
+  // Serial.println("Model");
+  // Serial.println(servos[0]->getModel(10));
+  // delay(20);
+  // Serial.println(servos[0]->getModel(11));
+  // delay(50);
+  // Serial.println(servos[0]->getModel(12));
+  // Serial.println(servos[0]->getModel(17));
+  // Serial.println(servos[0]->getModel(14));
 }
 
 void SetID(uint8_t oldID, uint8_t newID) {
@@ -381,8 +389,9 @@ void handleDataWrite(const uint8_t* payload, uint16_t length) {
   uint8_t id = payload[1];
   uint8_t writeByte = payload[2];
 
-  uint8_t major = config.getMotorModel(id);
-  servos[payload[0]]->setFeetechMode(major);  // put feetech interface in correct mode
+  uint8_t model = servos[payload[0]]->getMajorModel(id);//  config.getMotorModel(id);
+  sendMessage(String(model));
+  servos[payload[0]]->setFeetechMode(model);  // put feetech interface in correct mode
 
 
   // Special case for ID changes, perform unlock, change, lock routine
diff --git a/feetech.cpp b/feetech.cpp
index 4ce9417..c2837c4 100644
--- a/feetech.cpp
+++ b/feetech.cpp
@@ -204,7 +204,13 @@ uint8_t Feetech::disableTorque(uint8_t id) {
 
 uint16_t Feetech::getModel(uint8_t id) {
   sendRequest(id, REQUEST_MODEL, 2);
-  return waitOnData2Bytes(10);
+  return waitOnData2Bytes(50);
+}
+
+uint16_t Feetech::getMajorModel(uint8_t id) {
+  sendRequest(id, REQUEST_MODEL, 2);
+  uint16_t fullModel = waitOnData2Bytes(10);
+  return fullModel; // return low byte (major model)
 }
 
 uint8_t Feetech::getID(uint8_t id) {
diff --git a/feetech.h b/feetech.h
index 0127b94..29360cc 100644
--- a/feetech.h
+++ b/feetech.h
@@ -24,6 +24,7 @@ public:
   void sendPing(uint8_t id);
   void clearEcho(uint8_t length);
   uint16_t getModel(uint8_t id);
+  uint16_t getMajorModel(uint8_t id);
   uint8_t getID(uint8_t id);
   uint8_t setID(uint8_t id, uint8_t newId);
   uint8_t changeID(uint8_t id, uint8_t newId);
@@ -115,7 +116,8 @@ public:
 
   static const byte REQUEST_VOLTAGE = 0x3E;  // 1 byte
 
-  static const byte REQUEST_TEMPERATURE = 0x3F;      // 1 byte
+  static const byte REQUEST_TEMPERATURE = 0x3F;      // 1 byte  
+  static const byte REQUEST_STATUS = 0x41;      // 1 byte  Bit0, Bit1, Bit2, Bit3, Bit4, and Bit5 are set to 1 to indicate the occurrence of a corresponding error (no voltage, no temperature, no overload). A corresponding bit of 0 indicates no error.
   static const byte REQUEST_MOVING = 0x42;           // 1 byte
   static const byte REQUEST_CURRENT_CURRENT = 0x45;  // 2 bytes