   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 setPinValue(unsigned char c) {
+  if(c==0x55&&isStart==false){
+    if(prevc==0xff){
+      index=1;
+      isStart = true;
+    }    
+  } else {    
+    prevc = c;
+    if(isStart) {
+      if(index==2){
+        dataLen = c; 
+      } else if(index>2) {
+        dataLen--;
+      }
+      
+      writeBuffer(index,c);
+    }
+  }
+    
+  index++;
+  
+  if(index>51) {
+    index=0; 
+    isStart=false;
+  }
+    
+  if(isStart&&dataLen==0&&index>3){  
+    isStart = false;
+    parseData(); 
+    index=0;
+  }
+}
+
+unsigned char readBuffer(int index){
+  return buffer[index]; 
 }
 
 void parseData() {

/**********************************************************************************
   The following software may be included in this software : orion_firmware.ino
   from http://www.makeblock.cc/
   This software contains the following license and notice below:
   CC-BY-SA 3.0 (https://creativecommons.org/licenses/by-sa/3.0/)
   Author : Ander, Mark Yan
   Updated : Ander, Mark Yan
   Date : 01/09/2016
   Description : Firmware for Makeblock Electronic modules with Scratch.
   Copyright (C) 2013 - 2016 Maker Works Technology Co., Ltd. All right reserved.
 **********************************************************************************/
#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servos[8];

char buffer[52];
byte index = 0;
byte dataLen;
unsigned char prevc = 0;
boolean isAvailable = false;
boolean isStart = false;
char serialRead;

union {
  byte byteVal[4];
  float floatVal;
  long longVal;
} val;

union {
  byte byteVal[2];
  short shortVal;
} valShort;

#define ALIVE 0
#define DIGITAL 1
#define ANALOG 2

  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();
}

