Revision 0:3c49891bc39d, committed 2011-07-29

Comitter:

nucho

Date:

Fri Jul 29 11:23:44 2011 +0000

Child:

1:7f0fc0d1f777

Commit message:

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/BasicDataType.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,165 @@
+/*******************************************
+ * BasicDataType.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef BASIC_DATA_TYPE_HEADER_INCLUDED
+#define BASIC_DATA_TYPE_HEADER_INCLUDED
+
+#include "SequenceDataType.h"
+#include "SequenceOctet.h"
+#include "SequenceChar.h"
+#include "SequenceBoolean.h"
+#include "SequenceLong.h"
+#include "SequenceDouble.h"
+#include "SequenceFloat.h"
+/*
+typedef struct __Time__ {
+  long sec;
+  long usec;
+}Time;
+*/
+
+class TimedData {
+public:
+    virtual void* GetBuffer() {
+        return 0;
+    }
+};
+
+class TimedBoolean : public TimedData {
+public:
+    char data;
+    virtual void* GetBuffer() {
+        return &data;
+    }
+    const static char typecode = 'b';
+};
+
+class TimedChar : public TimedData {
+public:
+    char data;
+    virtual void* GetBuffer() {
+        return &data;
+    }
+    const static char typecode = 'c';
+};
+
+class TimedOctet : public TimedData {
+public:
+    char data;
+    virtual void* GetBuffer() {
+        return &data;
+    }
+    const static char typecode = 'o';
+};
+
+class TimedLong : public TimedData {
+    //  Time timestamp;
+public:
+    long data;
+    virtual void* GetBuffer() {
+        return &data;
+    }
+    const static char typecode = 'l';
+};
+
+class TimedDouble : public TimedData {
+//  Time timestamp;
+public:
+    double data;
+    virtual void* GetBuffer() {
+        return &data;
+    }
+    const static char typecode = 'd';
+};
+
+class TimedFloat : public TimedData {
+    //  unsigned char length;
+public:
+    float data;
+    virtual void* GetBuffer() {
+        return &data;
+    }
+    const static char typecode = 'f';
+};
+
+class TimedDataSeq {
+public:
+    virtual SequenceDataType* GetBuffer() {
+        return 0;
+    }
+};
+
+class TimedOctetSeq : public TimedDataSeq {
+    //  Time timestamp;
+    //  long length;
+public:
+    SequenceOctet data;
+    virtual SequenceDataType* GetBuffer() {
+        return &data;
+    }
+    const static char typecode = 'O';
+};
+
+class TimedCharSeq : public TimedDataSeq {
+    //  Time timestamp;
+    //  long length;
+public:
+    SequenceChar data;
+    virtual SequenceDataType* GetBuffer() {
+        return &data;
+    }
+    const static char typecode = 'C';
+};
+
+class TimedBooleanSeq : public TimedDataSeq {
+    //  Time timestamp;
+    //  long length;
+public:
+    SequenceBoolean data;
+    virtual SequenceDataType* GetBuffer() {
+        return &data;
+    }
+    const static char typecode = 'B';
+};
+
+class TimedLongSeq : public TimedDataSeq {
+    //  Time timestamp;
+    //  long length;
+public:
+    SequenceLong data;
+    virtual SequenceDataType* GetBuffer() {
+        return &data;
+    }
+    const static char typecode = 'L';
+};
+
+class TimedFloatSeq : public TimedDataSeq {
+    //  Time timestamp;
+    //  long length;
+    //  float* data;
+public:
+    SequenceFloat data;
+    virtual SequenceDataType* GetBuffer() {
+        return &data;
+    }
+    const static char typecode = 'F';
+};
+
+struct TimedDoubleSeq : public TimedDataSeq {
+//  Time timestamp;
+    //  long length;
+    //  double* data;
+public:
+    SequenceDouble data;
+    virtual SequenceDataType* GetBuffer() {
+        return &data;
+    }
+    const static char typecode = 'D';
+};
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/InPort.cpp	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,151 @@
+/*******************************************
+ * InPort.cpp
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+
+#include <string.h>
+#include "Packet.h"
+#include "ReceivePacket.h"
+#include "SendPacket.h"
+#include "BasicDataType.h"
+#include "InPort.h"
+
+InPort::InPort(char* name, TimedOctet& Data):
+  InPortBase(name)
+{
+  m_pData = &Data;
+  m_TypeCode = 'o';
+}
+
+InPort::InPort(char* name, TimedChar& Data):
+  InPortBase(name)
+{
+  m_pData = &Data;
+  m_TypeCode = 'c';
+}
+
+InPort::InPort(char* name, TimedBoolean& Data):
+  InPortBase(name)
+{
+  m_pData = &Data;
+  m_TypeCode = 'b';
+}
+
+
+InPort::InPort(char* name, TimedDouble& Data):
+  InPortBase(name)
+{
+  m_pData = &Data;
+  m_TypeCode = 'd';
+}
+
+InPort::InPort(char* name, TimedFloat& Data):
+  InPortBase(name)
+{
+  m_pData = &Data;
+  m_TypeCode = 'f';
+}
+
+InPort::InPort(char* name, TimedLong& Data):
+  InPortBase(name)
+{
+  m_pData = &Data;
+  m_TypeCode = 'l';
+}
+
+InPort::InPort(char* name, TimedOctetSeq& Data):
+  InPortBase(name)
+{
+  m_pData = &Data;
+  m_TypeCode = 'O';
+}
+
+InPort::InPort(char* name, TimedCharSeq& Data):
+  InPortBase(name)
+{
+  m_pData = &Data;
+  m_TypeCode = 'C';
+}
+
+InPort::InPort(char* name, TimedBooleanSeq& Data):
+  InPortBase(name)
+{
+  m_pData = &Data;
+  m_TypeCode = 'B';
+}
+
+
+
+InPort::InPort(char* name, TimedDoubleSeq& Data):
+  InPortBase(name)
+{
+  m_pData = &Data;
+  m_TypeCode = 'D';
+}
+
+
+InPort::InPort(char* name, TimedFloatSeq& Data):
+  InPortBase(name)
+{
+  m_pData = &Data;
+  m_TypeCode = 'F';
+}
+
+InPort::InPort(char* name, TimedLongSeq&Data):
+  InPortBase(name)
+{
+  m_pData = &Data;
+  m_TypeCode = 'L';
+}
+
+InPort::~InPort()
+{
+}
+
+
+int InPort::isNew()
+{
+  char packet_buffer[MAX_PACKET_SIZE];
+  SendPacket(INPORT_ISNEW, strlen(GetName()), GetName());
+  ReceivePacket(packet_buffer);
+  if(packet_buffer[INTERFACE] != INPORT_ISNEW) {
+    return 0;//-INVALID_PACKET_INTERFACE;
+  }
+  return packet_buffer[DATA_START_ADDR];
+}
+
+
+int InPort::read() {
+  char packet_buffer[MAX_PACKET_SIZE];
+  SendPacket(INPORT_READ, strlen(GetName()), GetName());
+  ReceivePacket(packet_buffer);
+  if(packet_buffer[INTERFACE] != INPORT_READ) {
+    return -INVALID_PACKET_INTERFACE;
+  }
+  int len = packet_buffer[DATA_LENGTH] / SizeofData();
+  int isSequence = isSequenceType();
+  SetLength(len);
+  void *pBuffer = GetBuffer();
+  memcpy(pBuffer, &(packet_buffer[DATA_START_ADDR]), len*SizeofData());
+  
+  return len;
+}
+
+
+int InPort::SizeofData() {
+  switch(m_TypeCode) {
+  case 'b':
+  case 'B':
+  case 'o':
+  case 'O':
+  case 'c':
+  case 'C':
+    return 1;
+  default:
+    return 4;
+  }
+}
+  

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/InPort.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,45 @@
+#ifndef INPORT_HEADER_INCLUDED
+#define INPORT_HEADER_INCLUDED
+
+/*******************************************
+ * InPort.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#include "InPortBase.h"
+
+class InPort : public InPortBase {
+ private:
+  //  int isSequence;
+ public:
+  InPort(char* name, TimedOctet &Data);
+  InPort(char* name, TimedBoolean &Data);
+  InPort(char* name, TimedChar &Data);
+
+  InPort(char* name, TimedLong &Data);
+  InPort(char* name, TimedFloat &Data);
+  InPort(char* name, TimedDouble &Data);
+
+  InPort(char* name, TimedOctetSeq &Data);
+  InPort(char* name, TimedBooleanSeq &Data);
+  InPort(char* name, TimedCharSeq &Data);
+
+  InPort(char* name, TimedLongSeq &Data);
+  InPort(char* name, TimedFloatSeq &Data);
+  InPort(char* name, TimedDoubleSeq &Data);
+
+  ~InPort();
+
+ public:
+  int isNew();
+  int read();
+  int SizeofData();
+
+
+};
+
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/InPortBase.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,23 @@
+/*******************************************
+ * InPortBase.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef INPORT_BASE_HEADER_INCLUDED
+#define INPORT_BASE_HEADER_INCLUDED
+
+#include "PortBase.h"
+
+class InPortBase : public PortBase {
+ public:
+
+ public:
+ InPortBase(char *name) : PortBase(name){}
+
+  //  virtual bool isNew() = 0;
+  //  virtual bool read(char* dst, int size) = 0;
+
+};
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/OutPort.cpp	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,143 @@
+/*******************************************
+ * OutPort.cpp
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#include <string.h>
+#include "BasicDataType.h"
+#include "OutPort.h"
+#include "SendPacket.h"
+#include "ReceivePacket.h"
+
+
+OutPort::OutPort(char* name, TimedBoolean& Data) :
+  OutPortBase(name)
+{
+  //  m_pData = pData;
+  m_pData = &Data;
+  m_TypeCode = 'b';
+}
+
+OutPort::OutPort(char* name, TimedChar& Data) :
+  OutPortBase(name)
+{
+  //  m_pData = pData;
+  m_pData = &Data;
+  m_TypeCode = 'c';
+}
+
+OutPort::OutPort(char* name, TimedOctet& Data) :
+  OutPortBase(name)
+{
+  //  m_pData = pData;
+  m_pData = &Data;
+  m_TypeCode = 'o';
+}
+
+OutPort::OutPort(char* name, TimedLong& Data) :
+  OutPortBase(name)
+{
+  //  m_pData = pData;
+  m_pData = &Data;
+  m_TypeCode = 'l';
+}
+
+OutPort::OutPort(char* name, TimedFloat& Data) :
+  OutPortBase(name)
+{
+  //  m_pData = pData;
+  m_pData = &Data;
+  m_TypeCode = 'f';
+}
+
+OutPort::OutPort(char* name, TimedDouble& Data) :
+  OutPortBase(name)
+{
+  //  m_pData = pData;
+  m_pData = &Data;
+  m_TypeCode = 'd';
+}
+
+OutPort::OutPort(char* name, TimedBooleanSeq& Data) :
+  OutPortBase(name)
+{
+  //  m_pData = pData;
+  m_pData = &Data;
+  m_TypeCode = 'B';
+}
+
+OutPort::OutPort(char* name, TimedCharSeq& Data) :
+  OutPortBase(name)
+{
+  //  m_pData = pData;
+  m_pData = &Data;
+  m_TypeCode = 'C';
+}
+
+OutPort::OutPort(char* name, TimedOctetSeq& Data) :
+  OutPortBase(name)
+{
+  //  m_pData = pData;
+  m_pData = &Data;
+  m_TypeCode = 'O';
+}
+
+
+OutPort::OutPort(char* name, TimedLongSeq& Data) :
+  OutPortBase(name)
+{
+  //  m_pData = pData;
+  m_pData = &Data;
+  m_TypeCode = 'L';
+}
+
+OutPort::OutPort(char* name, TimedFloatSeq& Data) :
+  OutPortBase(name)
+{
+  //  m_pData = pData;
+  m_pData = &Data;
+  m_TypeCode = 'F';
+}
+
+OutPort::OutPort(char* name, TimedDoubleSeq& Data) :
+  OutPortBase(name)
+{
+  //  m_pData = pData;
+  m_pData = &Data;
+  m_TypeCode = 'D';
+}
+
+OutPort::~OutPort()
+{
+
+}
+
+int OutPort::SizeofData() {
+  switch(m_TypeCode) {
+  case 'b':
+  case 'B':
+  case 'o':
+  case 'O':
+  case 'c':
+  case 'C':
+    return 1;
+  default:
+    return 4;
+  }
+}
+
+int OutPort::write()
+{
+  int datalen = GetLength() * SizeofData();
+  int namelen = strlen(GetName());
+  char packet_buffer[MAX_PACKET_SIZE];
+  packet_buffer[0] = namelen;
+  packet_buffer[1] = datalen;
+  memcpy(&(packet_buffer[2]), GetName(), namelen);
+  memcpy(&(packet_buffer[2 + namelen]), GetBuffer(), datalen);
+  SendPacket(OUTPORT_WRITE, 2+namelen+datalen, packet_buffer);
+  ReceivePacket(packet_buffer);
+  return PACKET_HEADER_SIZE+2+namelen+datalen;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/OutPort.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,43 @@
+#ifndef OUTPORT_HEADER_INCLUDED
+#define OUTPORT_HEADER_INCLUDED
+
+/*******************************************
+ * OutPort.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#include "OutPortBase.h"
+
+class OutPort : public OutPortBase
+{
+ private:
+  //  void* m_pData;
+
+ public:
+  OutPort(char* name, TimedBoolean& pData);
+  OutPort(char* name, TimedChar& pData);
+  OutPort(char* name, TimedOctet& pData);
+
+  OutPort(char* name, TimedLong& pData);
+  OutPort(char* name, TimedFloat& pData);
+  OutPort(char* name, TimedDouble& pData);
+
+  OutPort(char* name, TimedBooleanSeq& pData);
+  OutPort(char* name, TimedCharSeq& pData);
+  OutPort(char* name, TimedOctetSeq& pData);
+
+  OutPort(char* name, TimedLongSeq& pData);
+  OutPort(char* name, TimedFloatSeq& pData);
+  OutPort(char* name, TimedDoubleSeq& pData);
+
+  ~OutPort();
+
+ public:
+  int SizeofData();
+  int write();
+};
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/OutPortBase.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,24 @@
+/*******************************************
+ * OutPortBase.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+
+#ifndef OUTPORT_BASE_HEADER_INCLUDED
+#define OUTPORT_BASE_HEADER_INCLUDED
+
+#include "PortBase.h"
+
+class OutPortBase : public PortBase {
+
+ public:
+ OutPortBase(char* name): PortBase(name) {}
+
+ public:
+  //  virtual bool write(char* src, int size) = 0;
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/Packet.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,74 @@
+/*******************************************
+ * Packet.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef PACKET_HEADER_INCLUDED
+#define PACKET_HEADER_INCLUDED
+    
+// Return Values
+#define TIMEOUT 1
+#define DATA_TIMEOUT 2
+#define CHECKSUM_ERROR 3
+
+#define INVALID_PACKET_INTERFACE 65
+#define INVALID_PACKET_DATASIZE  66
+
+// Packet Settings
+#define PACKET_HEADER_SIZE 2
+#define INTERFACE 0
+#define DATA_LENGTH 1
+#define DATA_START_ADDR 2
+
+// Protocol
+// Interface
+#define INITIALIZE 'I'
+#define ACTIVATE 'A'
+#define DEACTIVATE 'D'
+#define EXECUTE 'E'
+#define ONERROR 'C'
+#define RESET 'R'
+#define GET_STATUS 'X'
+
+#define ADD_INPORT 'P'
+#define ADD_OUTPORT 'Q'
+
+#define INPORT_ISNEW 'N'
+#define INPORT_READ  'J'
+
+#define RTNO_OK '@'
+#define RTNO_ERROR 'x'
+
+
+#define OUTPORT_WRITE 'W'
+
+// Communication Settings
+#define PACKET_WAITING_TIME 50 // ms
+#define PACKET_WAITING_DELAY 100 //us
+#define PACKET_WAITING_COUNT (PACKET_WAITING_TIME*1000/PACKET_WAITING_DELAY)
+
+
+#define TYPECODE_TIMED_BOOLEAN 'b'
+#define TYPECODE_TIMED_CHAR 'c'
+#define TYPECODE_TIMED_OCTET 'o'
+
+
+#define TYPECODE_TIMED_LONG 'l'
+#define TYPECODE_TIMED_FLOAT 'f'
+#define TYPECODE_TIMED_DOUBLE 'd'
+
+#define TYPECODE_TIMED_BOOLEAN_SEQ 'B'
+#define TYPECODE_TIMED_CHAR_SEQ 'C'
+#define TYPECODE_TIMED_OCTET_SEQ 'O'
+
+
+#define TYPECODE_TIMED_LONG_SEQ 'L'
+#define TYPECODE_TIMED_FLOAT_SEQ 'F'
+#define TYPECODE_TIMED_DOUBLE_SEQ 'D'
+
+#define MAX_PACKET_SIZE 64
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/PortBase.cpp	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,22 @@
+/*******************************************
+ * PortBase.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#include <stdlib.h>
+#include <string.h>
+#include "PortBase.h"
+
+PortBase::PortBase(char* name)
+{
+  m_pName = (char*)malloc(strlen(name)+1);
+  strcpy(m_pName, name);
+}
+
+
+PortBase::~PortBase()
+{
+  free(m_pName);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/PortBase.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,73 @@
+/*******************************************
+ * PortBase.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef PORT_BASE_HEADER_INCLUDED
+#define PORT_BASE_HEADER_INCLUDED
+
+#include "BasicDataType.h"
+
+class PortBase {
+ private:
+
+    
+ protected:
+  void* m_pData;
+  char* m_pName;
+  char m_TypeCode;
+ public:
+  PortBase(char* name);
+  ~PortBase();
+
+ public:
+  char* GetName() {return m_pName;}
+  char GetTypeCode() {return m_TypeCode;}
+
+  int isSequenceType() {
+    switch(m_TypeCode) {
+    case 'b':
+    case 'B':
+    case 'o':
+    case 'O':
+    case 'c':
+    case 'C':
+      return 0;
+    default:
+      return 1;
+    }
+  }
+
+  int GetLength() {
+    if(!isSequenceType()) {
+      return 1;
+    } else {
+      return ((TimedDataSeq*)m_pData)->GetBuffer()->length();
+    }
+  }
+
+  void SetLength(int len) {
+    if(!isSequenceType()) {
+      return;
+    }
+    SequenceDataType *seqData = ((TimedDataSeq*)m_pData)->GetBuffer();
+    seqData->length(len);
+  }
+
+  void* GetBuffer() {
+    void* pBuffer;
+      if(!isSequenceType()) {
+      //      if (len != 1) return 0;// -INVALID_PACKET_DATASIZE;
+      pBuffer = ((TimedData*)m_pData)->GetBuffer();
+    } else {
+      SequenceDataType *seqData = ((TimedDataSeq*)m_pData)->GetBuffer();
+      pBuffer = seqData->GetBuffer();
+    }
+    return pBuffer;
+  }
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/RTno.cpp	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,148 @@
+/*******************************************
+ * RTno.cpp
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+/********************************************
+It changed a little for mbed. 
+The serial buffer is not used. 
+2011/7/27 @nucho
+********************************************/
+
+#include "RTno.h"
+//#include "WConstants.h"
+//#include "HardwareSerial.h"
+#include "ReceivePacket.h"
+#include "SendPacket.h"
+#include "Serial.h"
+#include "mbed.h"
+
+using namespace RTno;
+
+config_str conf;
+exec_cxt_str exec_cxt;
+
+enum {
+    CREATED='C',
+    INACTIVE='I',
+    ACTIVE='A',
+    ERROR='E',
+};
+
+char condition = CREATED;
+static char packet_buffer[64];
+
+void RTno::setup() {
+    rtcconf();
+    pc.baud(conf._default.baudrate);
+}
+
+
+static const char rtc_ok = RTNO_OK;
+static const char rtc_error = RTNO_ERROR;
+#define LP_RTC_OK (&rtc_ok)
+#define LP_RTC_ERROR (&rtc_error)
+
+
+int main() {
+    RTno::setup();
+
+    while(1){
+      RTno::loop();
+    }
+}
+
+void RTno::loop() {
+    ReceivePacket(packet_buffer);
+
+    switch (condition) {
+        case CREATED:
+            if (packet_buffer[INTERFACE] == INITIALIZE) {
+                if (onInitialize() == RTC_OK) {
+                    condition = INACTIVE;
+                    SendPacket(INITIALIZE, 1, LP_RTC_OK);
+                } else {
+                    condition = ERROR;
+                    SendPacket(INITIALIZE, 1, LP_RTC_ERROR);
+                }
+            }
+            break;
+        case ERROR:
+            if (packet_buffer[INTERFACE] == ONERROR) {
+                onError();
+                SendPacket(ONERROR, 1, LP_RTC_OK);
+            } else if (packet_buffer[INTERFACE] == RESET) {
+                if (onReset() == RTC_OK) {
+                    condition = INACTIVE;
+                    SendPacket(RESET, 1, LP_RTC_OK);
+                } else {
+                    condition = ERROR;
+                    SendPacket(RESET, 1, LP_RTC_ERROR);
+                }
+            }
+            break;
+        case INACTIVE:
+            if (packet_buffer[INTERFACE] == ACTIVATE) {
+                if (onActivated() == RTC_OK) {
+                    condition = ACTIVE;
+                    SendPacket(ACTIVATE, 1, LP_RTC_OK);
+                } else {
+                    condition = ERROR;
+                    SendPacket(ACTIVATE, 1, LP_RTC_ERROR);
+                }
+            }
+            break;
+        case ACTIVE:
+            if (packet_buffer[INTERFACE] == DEACTIVATE) {
+                onDeactivated();
+                condition = INACTIVE;
+                SendPacket(DEACTIVATE, 1, LP_RTC_OK);
+            } else if (packet_buffer[INTERFACE] == EXECUTE) {
+                if (onExecute() == RTC_OK) {
+                    SendPacket(EXECUTE, 1, LP_RTC_OK);
+                } else {
+                    condition = ERROR;
+                    SendPacket(EXECUTE, 1, LP_RTC_ERROR);
+                }
+            }
+            break;
+        default:
+            condition = ERROR;
+            break;
+    }
+
+    if (packet_buffer[INTERFACE] == GET_STATUS) {
+        SendPacket(GET_STATUS, 1, &condition);
+    }
+}
+
+
+/*NNN
+l:TimedLong
+d:TimedDouble
+f:TimedFloat
+L:TimedLongSeq
+D:TimedDoubleSeq
+F:TimedFloatSeq
+*/
+
+void addInPort(InPortBase& Port) {
+    int len = strlen(Port.GetName());
+    char *data = (char*)malloc(len+1);
+    data[0] = Port.GetTypeCode();
+    memcpy(&(data[1]), Port.GetName(), len);
+    SendPacket(ADD_INPORT, len+1, data);
+    ReceivePacket(packet_buffer);
+    free(data);
+}
+
+void addOutPort(OutPortBase& Port) {
+    int len = strlen(Port.GetName());
+    char *data = (char*)malloc(len+1);
+    data[0] = Port.GetTypeCode();
+    memcpy(&(data[1]), Port.GetName(), len);
+    SendPacket(ADD_OUTPORT, len+1, data);
+    ReceivePacket(packet_buffer);
+    free(data);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/RTno.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,106 @@
+#ifndef RTNO_HEADER_INCLUDED
+#define RTNO_HEADER_INCLUDED
+
+/*******************************************
+ * RTno.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+
+//#define BAUDRATE 19200
+
+#define RTC_OK 0
+#define RTC_ERROR -1
+
+#include "BasicDataType.h"
+#include "InPort.h"
+#include "OutPort.h"
+
+
+extern "C" {
+    void addInPort(InPortBase& inPort);
+    void addOutPort(OutPortBase& outPort);
+}
+
+struct config_str {
+    struct default_str {
+        long baudrate;
+    } _default;
+};
+
+struct exec_cxt_str {
+    struct periodic_str {
+        long type;
+    } periodic;
+};
+
+#define ProxySynchronousExecutionContext 1
+
+extern exec_cxt_str exec_cxt;
+
+extern  config_str conf;
+
+
+extern "C" {
+    void rtcconf(void);
+};
+
+namespace RTno {
+extern "C" {
+    // setup function is defined in RTno.cpp
+    void setup();
+
+    // loop fuction is defined in RTno.cpp
+    void loop();
+
+    // These call-back funcitons should be defined in user program code.
+    // Use RTno_template.pde to create your own project.
+
+    /**
+     * onInitialize()
+     * This function is called when RTno is initialized.
+     * RTno is usually initialized when the RTno-proxy is launched.
+     */
+    int onInitialize();
+
+    /**
+     * onActivated()
+     * This function is called when RTno is activated.
+     * RTno is usually activated by RT System Editor or other tools for OpenRTM-aist.
+     */
+    int onActivated();
+
+    /**
+     * onExecute()
+     * This function is periodically called when the RTno-proxy is active.
+     */
+    int onExecute();
+
+    /**
+     * onDeactivated()
+     * This function is called when RTno is deactivated.
+     * RTno is usually deactivated by RT System Editor or other tools for OpenRTM-aist.
+     */
+    int onDeactivated();
+
+    /**
+     * onError
+     * This function is called when RTno is error.
+     * [DANGEROUS] This function is periodically called in very short interval.
+     */
+    int onError();
+
+    /**
+     * onReset
+     * This function is called when RTno is reset.
+     * RTno is usually reset by RT System Editor or other tools for OpenRTM-aist.
+     */
+    int onReset();
+    };// extern "C"
+};
+
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/ReceivePacket.cpp	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,78 @@
+/*******************************************
+ * ReceivePacket.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+/********************************************
+It changed a little for mbed. 
+The serial buffer is not used. 
+2011/7/27 @nucho
+********************************************/
+#include "ReceivePacket.h"
+#include "Serial.h"
+//#include <HardwareSerial.h>
+//#include <WConstants.h>
+
+int ReceivePacket(char* packet) {
+    int counter = 0;
+    unsigned char sum = 0;
+    /*
+    while (Serial.available() < PACKET_HEADER_SIZE) {
+        delayMicroseconds(PACKET_WAITING_DELAY);
+        counter++;
+        if (counter == PACKET_WAITING_COUNT) {
+            return -TIMEOUT;
+        }
+    }*/
+    //    char host = Serial.read();
+    //    char target = Serial.read();
+
+    packet[INTERFACE] = pc.getc();
+    sum += packet[INTERFACE];
+
+    packet[DATA_LENGTH] = pc.getc();
+    sum += packet[DATA_LENGTH];
+
+    /*
+    while (Serial.available() < packet[DATA_LENGTH]+1) {
+        //delayMicroseconds(PACKET_WAITING_DELAY);
+        wait_us(PACKET_WAITING_DELAY);
+        counter++;
+        if (counter == PACKET_WAITING_COUNT) {
+            return -DATA_TIMEOUT;
+        }
+    }*/
+
+    for (int i = 0; i < packet[DATA_LENGTH]; i++) {
+        packet[PACKET_HEADER_SIZE+i] = pc.getc();
+        sum += packet[PACKET_HEADER_SIZE+i];
+    }
+    packet[PACKET_HEADER_SIZE+packet[DATA_LENGTH]] = pc.getc();
+    if (sum != packet[PACKET_HEADER_SIZE+packet[DATA_LENGTH]]) {
+        return -CHECKSUM_ERROR;
+    }
+    return PACKET_HEADER_SIZE + packet[DATA_LENGTH] + 1;
+}
+
+/*
+int read_string(char* buffer, int flag=1) {
+  int counter = 0;
+  for(int i = 0;i < 100;i++) {
+    if(Serial.available() > 0) {
+      buffer[counter] = Serial.read();
+      if(buffer[counter] == '^') {
+    buffer[counter] = 0;
+    Serial.print("AO^");
+    return counter+1;
+      }
+      counter++;
+    }
+    delayMicroseconds(100);
+  }
+  if(flag)
+  Serial.print("AE^");
+  return -1;
+}
+
+*/

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/ReceivePacket.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,15 @@
+/*******************************************
+ * ReceivePacket.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef RECEIVE_PACKET_HEADER_INCLUDED
+#define RECEIVE_PACKET_HEADER_INCLUDED
+
+#include "Packet.h"
+
+int ReceivePacket(char* packet);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/SendPacket.cpp	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,35 @@
+/*******************************************
+ * SendPacket.cpp
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+/********************************************
+It changed a little for mbed. 
+The serial buffer is not used. 
+2011/7/27 @nucho
+********************************************/
+#include "Packet.h"
+//#include <HardwareSerial.h>
+//#include <WConstants.h>
+#include "Serial.h"
+
+int SendPacket(const char interface, const char data_length, const char* packet_data) {
+  unsigned char sum = 0;
+
+  pc.putc(interface);
+  sum += interface;
+
+  pc.putc(data_length);
+  sum += data_length;
+  for(int i = 0;i < data_length;i++) {
+    sum += packet_data[i];
+    pc.putc(packet_data[i]);
+  }
+  //  if(data_length != 0) {
+  //n    Serial.write((const uint8_t*)packet_data, data_length);
+  //  }
+  //sum = 32;
+  pc.putc(sum);
+  return PACKET_HEADER_SIZE + data_length + 1;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/SendPacket.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,16 @@
+/*******************************************
+ * SendPacket.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef SEND_PACKET_HEADER_INCLUDED
+#define SEND_PACKET_HEADER_INCLUDED
+
+#include "Packet.h"
+
+int SendPacket(const char interface, const char data_length, const char* packet_data);
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/SequenceBoolean.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,27 @@
+/*******************************************
+ * SequenceBoolean.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef SEQUENCE_BOOLEAN_HEADER_INCLUDED
+#define SEQUENCE_BOOLEAN_HEADER_INCLUDED
+
+class SequenceBoolean : public SequenceDataType
+{
+ private:
+  char* m_pData;
+ public:
+ SequenceBoolean() : SequenceDataType((void**)&m_pData) {
+
+  }
+  ~SequenceBoolean(){}
+ virtual int SizeofData() { return 1; }
+  char& operator[](int index) {
+    return m_pData[index];
+  }
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/SequenceChar.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,27 @@
+/*******************************************
+ * SequenceChar.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef SEQUENCE_CHAR_HEADER_INCLUDED
+#define SEQUENCE_CHAR_HEADER_INCLUDED
+
+class SequenceChar : public SequenceDataType
+{
+ private:
+  char* m_pData;
+ public:
+ SequenceChar() : SequenceDataType((void**)&m_pData) {
+
+  }
+  ~SequenceChar(){}
+ virtual int SizeofData() { return 1; }
+  char& operator[](int index) {
+    return m_pData[index];
+  }
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/SequenceDataType.cpp	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,49 @@
+/*******************************************
+ * SequenceDataType.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#include <stdlib.h>
+#include "SequenceDataType.h"
+#include "mbed.h"
+
+Serial pc3(p9,p10);
+
+SequenceDataType::SequenceDataType(void** ptrptr)
+{
+  m_ptrptr = ptrptr;*m_ptrptr = 0;len = 0;
+}
+
+SequenceDataType::~SequenceDataType()
+{
+  free(*m_ptrptr);
+}
+
+
+void SequenceDataType::length(int size)
+{
+  len = size;
+  pc3.printf("len222\n\r");
+  free(*m_ptrptr);
+  pc3.printf("len333\n\r");
+  *m_ptrptr = (void*)malloc(size * SizeofData());
+  //*m_ptrptr = (void*)malloc(size * 4);
+}
+
+#if 0
+int SequenceDataType::SizeofData() {
+  switch(m_TypeCode) {
+  case 'b':
+  case 'B':
+  case 'o':
+  case 'O':
+  case 'c':
+  case 'C':
+    return 1;
+  default:
+    return 4;
+  }
+}
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/SequenceDataType.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,25 @@
+/*******************************************
+ * SequenceDataType.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef SEQUENCE_DATA_TYPE_HEADER_INCLUDED
+#define SEQUENCE_DATA_TYPE_HEADER_INCLUDED
+
+class SequenceDataType {
+ private:
+  void** m_ptrptr;
+  int len;
+ public:
+  SequenceDataType(void** ptrptr); 
+  ~SequenceDataType();
+ public:
+  virtual int SizeofData() {return 0;}
+  void length(int size) ;
+  int length() {return len;}
+  void* GetBuffer() { return *m_ptrptr; }
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/SequenceDouble.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,27 @@
+/*******************************************
+ * SequenceDouble.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef SEQUENCE_DOUBLE_HEADER_INCLUDED
+#define SEQUENCE_DOUBLE_HEADER_INCLUDED
+
+class SequenceDouble : public SequenceDataType
+{
+ private:
+  double* m_pData;
+ public:
+ SequenceDouble() : SequenceDataType((void**)&m_pData) {
+
+  }
+  ~SequenceDouble(){}
+ virtual int SizeofData() { return 4; }
+
+  double& operator[](int index) {
+    return m_pData[index];
+  }
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/SequenceFloat.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,27 @@
+/*******************************************
+ * SequenceFloat.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef SEQUENCE_FLOAT_HEADER_INCLUDED
+#define SEQUENCE_FLOAT_HEADER_INCLUDED
+
+class SequenceFloat : public SequenceDataType
+{
+ private:
+  float* m_pData;
+ public:
+ SequenceFloat() : SequenceDataType((void**)&m_pData) {
+
+  }
+  ~SequenceFloat(){}
+ virtual int SizeofData() { return 4; }
+  float& operator[](int index) {
+    return m_pData[index];
+  }
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/SequenceLong.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,27 @@
+/*******************************************
+ * SequenceLong.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef SEQUENCE_LONG_HEADER_INCLUDED
+#define SEQUENCE_LONG_HEADER_INCLUDED
+
+class SequenceLong : public SequenceDataType
+{
+ private:
+  long* m_pData;
+ public:
+ SequenceLong() : SequenceDataType((void**)&m_pData) {
+
+  }
+  ~SequenceLong(){}
+ virtual int SizeofData() { return 4; }
+  long& operator[](int index) {
+    return m_pData[index];
+  }
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTno/SequenceOctet.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,27 @@
+/*******************************************
+ * SequenceOctet.h
+ * @author Yuki Suga
+ * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
+ * @license LGPLv3
+ *****************************************/
+
+#ifndef SEQUENCE_OCTET_HEADER_INCLUDED
+#define SEQUENCE_OCTET_HEADER_INCLUDED
+
+class SequenceOctet : public SequenceDataType
+{
+ private:
+  char* m_pData;
+ public:
+ SequenceOctet() : SequenceDataType((void**)&m_pData) {
+
+  }
+  ~SequenceOctet(){}
+  virtual int SizeofData() { return 4; }
+  char& operator[](int index) {
+    return m_pData[index];
+  }
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Serial.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,10 @@
+#ifndef SERIAL_HEADER_INCLUDED
+#define SERIAL_HEADER_INCLUDED
+
+#include "mbed.h"
+
+//static MODSERIAL pc(USBTX,USBRX,128,128);
+static Serial pc(USBTX,USBRX);
+//static Serial debug(p9,p10);
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SimplePID.h	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,58 @@
+/**************************
+This class referred to following URL. 
+http://monoist.atmarkit.co.jp/mn/articles/1007/26/news083.html
+**************************/
+
+class SimplePID {
+public:
+    SimplePID(double P,double I,double D,double rate) {
+        _gainP=P;
+        _gainI=I;
+        _gainD=D;
+        _rate = rate;
+
+        _diff_new=0;
+        _diff_old=0;
+    }
+    
+    void setGoal(int goal)
+    {
+        _goal = goal;
+    }
+
+    double compute(int current) {
+       double p,i,d;
+       
+       _diff_old=_diff_new;
+       _diff_new=current-_goal;
+       
+       _integral += (_diff_old + _diff_new)/2.0 * _rate;
+       
+       p= _gainP*_diff_new;
+       i= _gainI*_integral;
+       d= _gainD*(_diff_new - _diff_old) *_rate;
+       
+       return math_limit(p+i+d,_min,_max);
+    }
+    
+    void setLimits(int min,int max){
+        _min = min;
+        _max = max;
+    }
+    
+    
+private:
+    int _diff_new,_diff_old,_goal;
+    double _integral,_rate;
+
+    double _gainP,_gainI,_gainD;
+    int _min,_max;
+
+    int math_limit(double ctrl,double min,double max) {
+        int ret=ctrl;
+        if(ctrl < min)  ret=min;
+        if(ctrl >max) ret=max;
+        
+        return ret;
+    }
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,188 @@
+#include "mbed.h"
+#include "RTno.h"
+
+#include "QEI.h"
+#include "SimplePID.h"
+#define MOTOR_OFFSET 1460
+#define KP 5.1
+#define KI 0.0
+#define KD 0.0
+#define RATE 0.2
+
+PwmOut motor1(p21);
+QEI qei_motor1(p29, p30, NC, 624);
+SimplePID pid_motor1(KP,KI,KD,RATE);
+
+PwmOut motor2(p22);
+QEI qei_motor2(p27, p28, NC, 624);
+SimplePID pid_motor2(KP,KI,KD,RATE);
+/**
+ * digitalInOut.pde
+ * RTno is RT-middleware and arduino.
+ *
+ * Using RTno, arduino device can communicate any RT-components
+ *  through the RTno-proxy component which is launched in PC.
+ * Connect arduino with USB, and program with RTno library.
+ * You do not have to define any protocols to establish communication
+ *  between arduino and PC.
+ *
+ * Using RTno, you must not define the function "setup" and "loop".
+ * Those functions are automatically defined in the RTno libarary.
+ * You, developers, must define following functions:
+ *  int onInitialize(void);
+ *  int onActivated(void);
+ *  int onDeactivated(void);
+ *  int onExecute(void);
+ *  int onError(void);
+ *  int onReset(void);
+ * These functions are spontaneously called by the RTno-proxy
+ *  RT-component which is launched in the PC.
+ */
+
+
+/**
+ * This function is called at first.
+ * conf._default.baudrate: baudrate of serial communication
+ * exec_cxt.periodic.type: reserved but not used.
+ */
+void rtcconf(void) {
+    conf._default.baudrate = 115200;
+    exec_cxt.periodic.type = ProxySynchronousExecutionContext;
+}
+
+/**
+ * Declaration Division:
+ *
+ * DataPort and Data Buffer should be placed here.
+ *
+ * Currently, following 6 types are available.
+ * TimedLong:
+ * TimedDouble:
+ * TimedFloat:
+ * TimedLongSeq:
+ * TimedDoubleSeq:
+ * TimedFloatSeq:
+ *
+ * Please refer following comments. If you need to use some ports,
+ * uncomment the line you want to declare.
+ **/
+TimedLongSeq position;
+InPort positionIn("position", position);
+
+TimedLongSeq encorder;
+OutPort encorderOut("encorder", encorder);
+
+
+//////////////////////////////////////////
+// on_initialize
+//
+// This function is called in the initialization
+// sequence. The sequence is triggered by the
+// PC. When the RTnoRTC is launched in the PC,
+// then, this function is remotely called
+// through the USB cable.
+// In on_initialize, usually DataPorts are added.
+//
+//////////////////////////////////////////
+int RTno::onInitialize() {
+    /* Data Ports are added in this section.
+    */
+    addInPort(positionIn);
+    addOutPort(encorderOut);
+
+    // Some initialization (like port direction setting)
+    
+    return RTC_OK;
+}
+
+////////////////////////////////////////////
+// on_activated
+// This function is called when the RTnoRTC
+// is activated. When the activation, the RTnoRTC
+// sends message to call this function remotely.
+// If this function is failed (return value
+// is RTC_ERROR), RTno will enter ERROR condition.
+////////////////////////////////////////////
+int RTno::onActivated() {
+    // Write here initialization code.
+
+    return RTC_OK;
+}
+
+/////////////////////////////////////////////
+// on_deactivated
+// This function is called when the RTnoRTC
+// is deactivated.
+/////////////////////////////////////////////
+int RTno::onDeactivated() {
+    // Write here finalization code.
+
+    return RTC_OK;
+}
+
+//////////////////////////////////////////////
+// This function is repeatedly called when the
+// RTno is in the ACTIVE condition.
+// If this function is failed (return value is
+// RTC_ERROR), RTno immediately enter into the
+// ERROR condition.r
+//////////////////////////////////////////////
+int RTno::onExecute() {
+
+    /*
+     * Input 
+     */
+    if (positionIn.isNew()) {
+        positionIn.read();
+        pid_motor1.setGoal(position.data[0]);
+        pid_motor2.setGoal(position.data[1]);
+    }
+    
+    pid_motor1.setLimits(-500,500);
+    pid_motor2.setLimits(-500,500);
+
+    /*
+     * Output 
+     */
+    int current1,current2;
+    current1 = qei_motor1.getPulses();
+    current2 = qei_motor2.getPulses();
+
+    encorder.data.length(2);    
+    encorder.data[0] = current1;
+    encorder.data[1] = current2;
+    encorderOut.write();
+
+    int ctrl1,ctrl2;
+    ctrl1 = pid_motor1.compute(current1);
+    ctrl2 = pid_motor2.compute(current2);
+    
+    motor1.pulsewidth_us(ctrl1+MOTOR_OFFSET);
+    motor2.pulsewidth_us(ctrl2+MOTOR_OFFSET);
+
+    return RTC_OK;
+}
+
+
+//////////////////////////////////////
+// on_error
+// This function is repeatedly called when
+// the RTno is in the ERROR condition.
+// The ERROR condition can be recovered,
+// when the RTno is reset.
+///////////////////////////////////////
+int RTno::onError() {
+    return RTC_OK;
+}
+
+////////////////////////////////////////
+// This function is called when
+// the RTno is reset. If on_reset is
+// succeeded, the RTno will enter into
+// the INACTIVE condition. If failed
+// (return value is RTC_ERROR), RTno
+// will stay in ERROR condition.ec
+///////////////////////////////////////
+int RTno::onReset() {
+    return RTC_OK;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Jul 29 11:23:44 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912