Diff: SCI.cpp

Revision:

16:e65c192b7ecf

Parent:

14:522bb06f0f0d

Child:

17:60de3bfdc70b

--- a/SCI.cpp	Thu Feb 21 15:21:12 2013 +0000
+++ b/SCI.cpp	Wed Feb 27 12:32:44 2013 +0000
@@ -12,26 +12,42 @@
     delete serial;
 }
 
+char SCI::getheader()
+{
+    char comm = getc_nowait();
+    if (comm != 0x00) {
+        serial->putc(comm);
+    }
+    return comm;
+}
+
 int SCI::getid()
 {
-    char comm;
     int id = 255;
-    comm = getc_nowait();
-    if (comm == 'A') {
-        serial->putc(comm);
-        stringstream sstr;
+    stringstream sstr;
+    sstr << getc_wait();
+    sstr >> id;
+    serial->printf("id = %d \r\n", id);
+    return id;
+}
+
+uint16_t SCI::getservoval()
+{
+    uint16_t val = 1500;
+    stringstream sstr;
+    while (!serial->readable());
+    for (int i = 0; i < 4; ++i) {
         sstr << getc_wait();
-        sstr >> id;
-        serial->printf("id = %d \r\n", id);
     }
-    return id;
+    sstr >> val;
+    serial->printf("val:%d", val);
+    serial->printf("\r\n");
+    return val;
 }
 
 char SCI::getc_wait()
 {
-    while (!serial->readable()) {
-        serial->printf("waiting..");
-    }
+    while (!serial->readable());
     char val = serial->getc();
     return val;
 }
@@ -50,17 +66,17 @@
     serial->printf(str);
 }
 
-/*uint16_t readint(void)
+/*uint16_t SCI::readint()
 {
-    uint8_t buff[16];
-    buff[0] = pc->getc();
-    buff[1] = pc->getc();
+    uint8_t buff[2];
+    buff[0] = getc_wait();
+    buff[1] = getc_wait();
     uint16_t val;
     val = (uint16_t)(buff[1] << 8) | (uint16_t)buff[0];
     return val;
-}
+}*/
 
-void sendint(uint16_t val)
+/*void sendint(uint16_t val)
 {
     uint8_t buff[16];
     buff[0] = (uint8_t)(val & 0x00FF);