Diff: Dynamixel.cpp

Revision:

6:edba46e8f8b4

Parent:

4:92ff8b2f3b3f

--- a/Dynamixel.cpp	Wed Aug 19 09:24:38 2015 +0000
+++ b/Dynamixel.cpp	Sun Oct 25 02:15:04 2015 +0000
@@ -238,4 +238,71 @@
     DEBUG("\n Delay time: %d [us]\n",statusBuffer[5]*2);
 
     return statusBuffer[5]; // Return parameter
+}
+
+uint8_t Dynamixel::setAngleLimit(uint16_t mode, uint8_t address)
+{
+    DEBUG("\n ---setAngleLimit--- \n");
+    // Sets allowable position values (angles) for Goal position
+    /* 
+    Valid address
+    -------------
+    Use the values defined in the header file:
+     - ADDRESS_CW_ANGLE_LIMIT
+     - ADDRESS_CCW_ANGLE_LIMIT
+    Mode values
+    -----------
+     - Wheel mode: set both CW and CCW registers to 0
+     - Joint mode: set both CW and CCW registers to a value different of 0
+     - Multi-turn mode: both CW and CCW registers are 4095 (0x0FFF)
+    */
+    uint8_t elements = 9;
+    uint8_t instructionBuffer[elements];
+    uint8_t statusBuffer[STATUS_PACKET_LENGTH] = {0,0,0,0,0,0};
+    
+    uint16_t checkSum = 0;
+    if (m_link.writeable() ) {
+        instructionBuffer[0] = 0xff;
+        instructionBuffer[1] = 0xff;
+        instructionBuffer[2] = m_motorID;                   // ID
+        instructionBuffer[3] = 0x05;                        // Length
+        instructionBuffer[4] = WRITE_DATA;                  // Instruction
+        instructionBuffer[5] = address;                     // Parameter 1: Starting address (Low byte)
+        instructionBuffer[6] = (uint8_t) mode & 0xff;       // Parameter 2: First value to be writen
+        instructionBuffer[7] = (uint8_t) (mode >> 8);       // Parameter 3: Second value to be writen
+        checkSum = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6] + instructionBuffer[7]);
+        instructionBuffer[8] = (uint8_t) (checkSum & 0xff); // Check sum
+        // Check instruction buffer
+        for (int i = 0; i <  elements; i++) {
+            DEBUG("%x ", instructionBuffer[i]);
+        }
+        DEBUG("\n");
+        
+        m_txEnable = 1;       // Enable Tx / Disable Rx
+        for (int i = 0; i<= elements; i++) {
+            m_link.putc(instructionBuffer[i]);
+        }
+        wait_ms(2);         // fix this!!!
+        m_txEnable = 0;       // Disable Tx / Enable Rx
+    } else {
+        //Dynamixel not writeable
+    }
+    
+    wait_ms(10);
+    if ( m_motorID != BROADCAST_ID ) {
+        DEBUG("Trying to receiv status packet.\n");
+        int i = 0;
+        while( m_link.readable() && i<STATUS_PACKET_LENGTH) {
+            statusBuffer[i] = m_link.getc();    // Read status packet
+            i++;
+            //wait_ms(100);
+        }
+        DEBUG("Read %d bytes.\n", i);
+    }
+    // Check status packet content
+    for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
+        DEBUG("%x ", statusBuffer[i]);
+    }
+    
+    return statusBuffer[4]; // Return error
 }
\ No newline at end of file