Diff: Dynamixel.cpp

Revision:

2:0ed32411598c

Parent:

1:ef90d942ce78

Child:

3:e5577be2fe70

--- a/Dynamixel.cpp	Wed Aug 19 05:36:55 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,154 +0,0 @@
-#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);
-}
-
-void Dynamixel::ping()
-{
-    uint8_t elements = 6;
-    uint8_t packetBuffer[elements];
-    if (m_link.writeable() ) {
-        packetBuffer[0] = 0xff;
-        packetBuffer[1] = 0xff;
-        packetBuffer[2] = m_motorID;    // ID
-        packetBuffer[3] = 0x02;         // Length
-        packetBuffer[4] = PING;         // Instruction
-        packetBuffer[5] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4]) & 0xFF;   // Check sum
-
-        pc.printf("\n Instruction packet:\n");
-        for (int i = 0; i<= elements; i++) {
-            pc.printf("0x%x\t", packetBuffer[i]);
-        }
-
-        m_txEnable = 1;       // Enable Tx / Disable Rx
-        for (int i = 0; i<= elements; i++) {
-            m_link.putc(packetBuffer[i]);
-        }
-        wait_ms(1);         // fix this!!!
-        m_txEnable = 0;       // Disable Tx / Enable Rx
-    } else {
-        pc.printf("Dynamixel not writeable\n");
-    }
-    //pc.baud(9600);
-    wait_ms(10);
-    pc.printf("\n Status packet:\n");
-    while ( m_link.readable() ) {
-        pc.printf("0x%x\t", m_link.getc());  // Read status packet if available
-    }
-    pc.printf("\n PING: Done.\n");
-}
-
-void Dynamixel::toggleLED(uint8_t ledState)
-{
-    uint8_t elements = 8;
-    uint8_t packetBuffer[elements];
-    if (m_link.writeable() ) {
-        packetBuffer[0] = 0xff;
-        packetBuffer[1] = 0xff;
-        packetBuffer[2] = m_motorID;    // ID
-        packetBuffer[3] = 0x04;         // Length
-        packetBuffer[4] = WRITE_DATA;   // Instruction
-        packetBuffer[5] = ADDRESS_LED;  // Parameter 1: Starting address
-        packetBuffer[6] = ledState;     // Parameter 2: First value to be writen
-        packetBuffer[7] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6]) & 0xFF;   // Check sum
-
-        m_txEnable = 1;       // Enable Tx / Disable Rx
-        for (int i = 0; i<= elements; i++) {
-            m_link.putc(packetBuffer[i]);
-        }
-        wait_ms(2);         // fix this!!!
-        m_txEnable = 0;       // Disable Tx / Enable Rx
-    } else {
-        //pc.printf("Dynamixel not writeable\n");
-    }
-    wait_ms(10);
-    pc.printf("\n Status packet:\n");
-    while ( m_link.readable() ) {
-        pc.printf("0x%x\t", m_link.getc());  // Read status packet if available
-    }
-}
-
-void Dynamixel::move(uint16_t position)
-{
-    // 0 to 1023 (0x3FF)
-    uint8_t elements = 9;
-    uint8_t packetBuffer[elements];
-    uint16_t checkSum = 0;
-    if (m_link.writeable() ) {
-        packetBuffer[0] = 0xff;
-        packetBuffer[1] = 0xff;
-        packetBuffer[2] = m_motorID;                    // ID
-        packetBuffer[3] = 0x05;                         // Length
-        packetBuffer[4] = WRITE_DATA;                   // Instruction
-        packetBuffer[5] = ADDRESS_GOAL_POSITION;        // Parameter 1: Starting address
-        packetBuffer[6] = (uint8_t) position & 0xff;    // Parameter 2: First value to be writen
-        packetBuffer[7] = (uint8_t) (position >> 8);    // Parameter 3: Second value to be writen
-        checkSum = packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6] + packetBuffer[7];
-        packetBuffer[8] = (uint8_t) (~checkSum & 0xff); // Check sum
-
-        /*
-        for (int i = 0; i<= elements; i++) {
-            pc.printf("%c ", packetBuffer[i]);
-        }
-        pc.printf("\n");
-        */
-        m_txEnable = 1;       // Enable Tx / Disable Rx
-        for (int i = 0; i<= elements; i++) {
-            m_link.putc(packetBuffer[i]);
-        }
-        wait_ms(2);         // fix this!!!
-        m_txEnable = 0;       // Disable Tx / Enable Rx
-    } else {
-        //pc.printf("Dynamixel not writeable\n");
-    }
-    wait_ms(10);
-    pc.printf("\n Status packet:\n");
-    while ( m_link.readable() ) {
-        pc.printf("0x%x\t", m_link.getc());  // Read status packet if available
-    }
-}
-
-void Dynamixel::setSpeed(uint16_t speed)
-{
-    // 0 to 1023 (0x3FF)
-    uint8_t elements = 9;
-    uint8_t packetBuffer[elements];
-    uint16_t checkSum = 0;
-    if (m_link.writeable() ) {
-        packetBuffer[0] = 0xff;
-        packetBuffer[1] = 0xff;
-        packetBuffer[2] = m_motorID;                    // ID
-        packetBuffer[3] = 0x05;                         // Length
-        packetBuffer[4] = WRITE_DATA;                   // Instruction
-        packetBuffer[5] = ADDRESS_MOVING_SPEED;         // Parameter 1: Starting address
-        packetBuffer[6] = (uint8_t) speed & 0xff;       // Parameter 2: First value to be writen
-        packetBuffer[7] = (uint8_t) (speed >> 8);       // Parameter 3: Second value to be writen
-        checkSum = packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6] + packetBuffer[7];
-        packetBuffer[8] = (uint8_t) (~checkSum & 0xff); // Check sum
-
-        /*
-        for (int i = 0; i<= elements; i++) {
-            pc.printf("%c ", packetBuffer[i]);
-        }
-        pc.printf("\n");
-        */
-        m_txEnable = 1;       // Enable Tx / Disable Rx
-        for (int i = 0; i<= elements; i++) {
-            m_link.putc(packetBuffer[i]);
-        }
-        wait_ms(2);         // fix this!!!
-        m_txEnable = 0;       // Disable Tx / Enable Rx
-    } else {
-        //pc.printf("Dynamixel not writeable\n");
-    }
-    wait_ms(10);
-    pc.printf("\n Status packet:\n");
-    while ( m_link.readable() ) {
-        pc.printf("0x%x\t", m_link.getc());  // Read status packet if available
-    }
-}
\ No newline at end of file