   command_index = (uint8_t)idx;
   int action = readBuffer(4);
   int device = readBuffer(5);
-  switch (action) {
-    case GET: {
-        if (device != ULTRASONIC) {
-          writeHead();
-          writeSerial(idx);
+  int port = readBuffer(6);
+  switch(action){
+    case GET:{
+      if(device == ULTRASONIC){
+        if(!isUltrasonic) {
+          setUltrasonicMode(true);
+          trigPin = readBuffer(6);
+          echoPin = readBuffer(7);
+          digitals[trigPin] = 1;
+          digitals[echoPin] = 1;
+          pinMode(trigPin, OUTPUT);
+          pinMode(echoPin, INPUT);
+          delay(50);
+        } else {
+          int trig = readBuffer(6);
+          int echo = readBuffer(7);
+          if(trig != trigPin || echo != echoPin) {
+            trigPin = trig;
+            echoPin = echo;
+            digitals[trigPin] = 1;
+            digitals[echoPin] = 1;
+            pinMode(trigPin, OUTPUT);            
+            pinMode(echoPin, INPUT);
+            delay(50);
+          }
         }
-        readSensor(device);
-        writeEnd();
+      } else if(port == trigPin || port == echoPin) {
+        setUltrasonicMode(false);
+        digitals[port] = 0;
+      } else {
+        digitals[port] = 0;
       }
-      break;
-    case SET: {
-        runModule(device);
-        callOK();
-      }
-      break;
-    case RESET: {
-        callOK();
-      }
-      break;
-  }
-}
-
-void callOK() {
-  writeSerial(0xff);
-  writeSerial(0x55);
-  writeEnd();
-}
-
-int searchServoPin(int pin) {
-  for (int i = 0; i < 8; i++) {
-    if (servo_pins[i] == pin) {
-      return i;
-    }
-    if (servo_pins[i] == 0) {
-      servo_pins[i] = pin;
-      return i;
-    }
-  }
-  return 0;
+    }
+    break;
+    case SET:{
+      runModule(device);
+      callOK();
+    }
+    break;
+    case RESET:{
+      callOK();
+    }
+    break;
+  }
 }
 
 void runModule(int device) {
   //0xff 0x55 0x6 0x0 0x1 0xa 0x9 0x0 0x0 0xa
   int port = readBuffer(6);
   int pin = port;
-  switch (device) {
-    case DIGITAL: {
-        pinMode(pin, OUTPUT);
-        int v = readBuffer(7);
-        digitalWrite(pin, v);
+
+  if(pin == trigPin || pin == echoPin) {
+    setUltrasonicMode(false);
+  }
+  
+  switch(device){
+    case DIGITAL:{      
+      setPortWritable(pin);
+      int v = readBuffer(7);
+      digitalWrite(pin,v);
+    }
+    break;
+    case PWM:{
+      setPortWritable(pin);
+      int v = readBuffer(7);
+      analogWrite(pin,v);
+    }
+    break;
+    case TONE:{
+      setPortWritable(pin);
+      int hz = readShort(7);
+      int ms = readShort(9);
+      if(ms>0) {
+        tone(pin, hz, ms);
+      } else {
+        noTone(pin);
       }
-      break;
-    case PWM: {
-        pinMode(pin, OUTPUT);
-        int v = readBuffer(7);
-        analogWrite(pin, v);
+    }
+    break;
+    case SERVO_PIN:{
+      setPortWritable(pin);
+      int v = readBuffer(7);
+      if(v>=0&&v<=180){
+        Servo sv = servos[searchServoPin(pin)];
+        sv.attach(pin);
+        sv.write(v);
       }
-      break;
-    case TONE: {
-        pinMode(pin, OUTPUT);
-        int hz = readShort(7);
-        int ms = readShort(9);
-        if (ms > 0) {
-          tone(pin, hz, ms);
-        } else {
-          noTone(pin);
-        }
-      }
-      break;
-    case SERVO_PIN: {
-        int v = readBuffer(7);
-        if (v >= 0 && v <= 180) {
-          Servo sv = servos[searchServoPin(pin)];
-          sv.attach(pin);
-          sv.write(v);
-        }
-      }
-      break;
-    case TIMER: {
-        lastTime = millis() / 1000.0;
-      }
-      break;
-  }
-}
-void sendString(String s) {
+    }
+    break;
+    case TIMER:{
+      lastTime = millis()/1000.0; 
+    }
+    break;
+  }
+}
+
+void sendPinValues() {  
+  int pinNumber = 0;
+  for (pinNumber = 0; pinNumber < 12; pinNumber++) {
+    if(digitals[pinNumber] == 0) {
+      sendDigitalValue(pinNumber);
+      callOK();
+    }
+  }
+  for (pinNumber = 0; pinNumber < 6; pinNumber++) {
+    if(analogs[pinNumber] == 0) {
+      sendAnalogValue(pinNumber);
+      callOK();
+    }
+  }
+  
+  if(isUltrasonic) {
+    sendUltrasonic();  
+    callOK();
+  }
+}
+
+void setUltrasonicMode(boolean mode) {
+  isUltrasonic = mode;
+  if(!mode) {
+    lastUltrasonic = 0;
+  }
+}
+
+void sendUltrasonic() {
+  digitalWrite(trigPin, LOW);
+  delayMicroseconds(2);
+  digitalWrite(trigPin, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(trigPin, LOW);
+
+  float value = pulseIn(echoPin, HIGH, 30000) / 29.0 / 2.0;
+
+  if(value == 0) {
+    value = lastUltrasonic;
+  } else {
+    lastUltrasonic = value;
+  }
+  writeHead();
+  sendFloat(value);
+  writeSerial(trigPin);
+  writeSerial(echoPin);
+  writeSerial(ULTRASONIC);
+  writeEnd();
+}
+
+void sendDigitalValue(int pinNumber) {
+  pinMode(pinNumber,INPUT);
+  writeHead();
+  sendFloat(digitalRead(pinNumber));  
+  writeSerial(pinNumber);
+  writeSerial(DIGITAL);
+  writeEnd();
+}
+
+void sendAnalogValue(int pinNumber) {
+  writeHead();
+  sendFloat(analogRead(pinNumber));  
+  writeSerial(pinNumber);
+  writeSerial(ANALOG);
+  writeEnd();
+}
+
+void writeBuffer(int index,unsigned char c){
+  buffer[index]=c;
+}
+
+void writeHead(){
+  writeSerial(0xff);
+  writeSerial(0x55);
+}
+
+void writeEnd(){
+  Serial.println();
+}
+
+void writeSerial(unsigned char c){
+  Serial.write(c);
+}
+
+void sendString(String s){
   int l = s.length();
   writeSerial(4);
   writeSerial(l);
-  for (int i = 0; i < l; i++) {
+  for(int i=0;i<l;i++){
     writeSerial(s.charAt(i));
   }
 }
-void sendFloat(float value) {
+
+void sendFloat(float value){ 
   writeSerial(2);
   val.floatVal = value;
   writeSerial(val.byteVal[0]);

  lcd.print("with Entry");
}

unsigned char readBuffer(int index) {
  return buffer[index];
}

void writeBuffer(int index, unsigned char c) {
  buffer[index] = c;
}
void writeHead() {
  writeSerial(0xff);
  writeSerial(0x55);
}
void writeEnd() {
  Serial.println();
}
void writeSerial(unsigned char c) {
  Serial.write(c);
}
void readSerial() {
  isAvailable = false;
  if (Serial.available() > 0) {
    isAvailable = true;
    serialRead = Serial.read();
  }
}

void parseData() {

  writeSerial(val.byteVal[2]);
  writeSerial(val.byteVal[3]);
}

void sendShort(double value){
  writeSerial(3);
  valShort.shortVal = value;
  writeSerial(valShort.byteVal[0]);
  writeSerial(valShort.byteVal[1]);
}

short readShort(int idx){
  valShort.byteVal[0] = readBuffer(idx);
  valShort.byteVal[1] = readBuffer(idx+1);
  return valShort.shortVal; 
}

float readFloat(int idx){
  val.byteVal[0] = readBuffer(idx);
  val.byteVal[1] = readBuffer(idx+1);
  val.byteVal[2] = readBuffer(idx+2);
  val.byteVal[3] = readBuffer(idx+3);
  return val.floatVal;
}

long readLong(int idx){
  val.byteVal[0] = readBuffer(idx);
  val.byteVal[1] = readBuffer(idx+1);
  val.byteVal[2] = readBuffer(idx+2);
  val.byteVal[3] = readBuffer(idx+3);
  return val.longVal;
}

int searchServoPin(int pin){
  for(int i=0;i<8;i++){
    if(servo_pins[i] == pin){
      return i;
    }
    if(servo_pins[i]==0){
      servo_pins[i] = pin;
      return i;
    }
  }
  return 0;
}

void setPortWritable(int pin) {
  if(digitals[pin] == 0) {
    digitals[pin] = 1;
    pinMode(pin, OUTPUT);
  } 
}

void callOK(){
  writeSerial(0xff);
  writeSerial(0x55);
  writeEnd();
}

void callDebug(char c){
  writeSerial(0xff);
  writeSerial(0x55);
  writeSerial(c);
  writeEnd();
}

