       //Servo pins on Due are offset by 2
       int pin = cmd[1];
       int pin_idx = pin-SERVO_PIN_OFFSET; 
-      int angle = (b[1] << 8) +b[2];
-
-      Serial.print("Servo Write. Pin: ");
-      Serial.print(pin);
-      Serial.print(", Angle: ");
-      Serial.println(angle);
+      int angle = (cmd[3] << 8) + cmd[2];
+
+      //Serial.print("Servo Write. Pin: ");
+      //Serial.print(pin);
+      //Serial.print(", Angle: ");
+      //Serial.println(angle);
 
       if(servos[pin_idx] != nullptr) {
         servos[pin_idx]->write(angle);

      int pin = cmd[1];
      int pin_idx = pin-SERVO_PIN_OFFSET; 

      Serial.print("Servo Attach. Pin: ");
      Serial.println(pin);

      if(servos[pin_idx] == nullptr) {
        servos[pin_idx] = new Servo;

        b[2]=servoRead%256;
      }

      //Serial.print(", Angle: ");
      //Serial.println(servoRead);

      run_once = false;    
    }
