Diff: Dynamixel.cpp

Revision:

2:32377cbec534

Parent:

1:ce081666d225

Child:

3:61785d105315

--- a/Dynamixel.cpp	Wed Aug 19 06:10:52 2015 +0000
+++ b/Dynamixel.cpp	Wed Aug 19 06:31:45 2015 +0000
@@ -1,8 +1,6 @@
 #include "Dynamixel.h"
 #include "mbed.h"
 
-Serial pc(USBTX, USBRX);
-
 Dynamixel::Dynamixel(PinName tx, PinName rx, PinName txEnable, uint8_t motorID, int baudrate) :  m_link(tx, rx), m_txEnable(txEnable), m_motorID(motorID), m_baudrate(baudrate)
 {
     m_link.baud(m_baudrate);
@@ -29,11 +27,11 @@
         wait_ms(1);         // fix this!!!
         m_txEnable = 0;     // Disable Tx / Enable Rx
     } else {
-        pc.printf("Dynamixel not writeable\n");
+        //pc.printf("Dynamixel not writeable\n");
     }
     wait_ms(10);            // fix this!!!
     if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
-        for (int i = 0; i<= STATUS_PACKET_LENGTH; i++) {
+        for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
             statusBuffer[i] = m_link.getc();    // Read status packet
         }
     }
@@ -67,7 +65,7 @@
     }
     wait_ms(10);
     if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
-        for (int i = 0; i<= STATUS_PACKET_LENGTH; i++) {
+        for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
             statusBuffer[i] = m_link.getc();    // Read status packet
         }
     }
@@ -111,7 +109,7 @@
     }
     wait_ms(10);
     if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
-        for (int i = 0; i<= STATUS_PACKET_LENGTH; i++) {
+        for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
             statusBuffer[i] = m_link.getc();    // Read status packet
         }
     }
@@ -155,7 +153,7 @@
     }
     wait_ms(10);
        if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
-        for (int i = 0; i<= STATUS_PACKET_LENGTH; i++) {
+        for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
             statusBuffer[i] = m_link.getc();    // Read status packet
         }
     }