Dan Gongora
mbed to Dynamixel servo communication.
Dependents: dynamixel_wheel_test
Diff: Dynamixel.cpp
- Revision:
- 4:92ff8b2f3b3f
- Parent:
- 3:61785d105315
- Child:
- 6:edba46e8f8b4
diff -r 61785d105315 -r 92ff8b2f3b3f Dynamixel.cpp
--- a/Dynamixel.cpp Wed Aug 19 08:59:16 2015 +0000
+++ b/Dynamixel.cpp Wed Aug 19 09:23:06 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);
@@ -10,7 +8,7 @@
uint8_t Dynamixel::ping()
{
- pc.printf("\n ---Ping--- \n");
+ DEBUG("\n ---Ping--- \n");
uint8_t elements = 6;
uint8_t instructionBuffer[elements];
uint8_t statusBuffer[STATUS_PACKET_LENGTH] = {0,0,0,0,0,0};
@@ -30,7 +28,7 @@
wait_ms(1); // fix this!!!
m_txEnable = 0; // Disable Tx / Enable Rx
} else {
- //pc.printf("Dynamixel not writeable\n");
+ // Dynamixel not writeable
}
wait_ms(10); // fix this!!!
@@ -43,14 +41,14 @@
}
// Check status packet content
for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
- pc.printf("%x ", statusBuffer[i]);
+ DEBUG("%x ", statusBuffer[i]);
}
return statusBuffer[4]; // Return error
}
uint8_t Dynamixel::toggleLED(uint8_t ledState)
{
- pc.printf("\n ---Toggle--- \n");
+ DEBUG("\n ---Toggle--- \n");
uint8_t elements = 8;
uint8_t instructionBuffer[elements];
uint8_t statusBuffer[STATUS_PACKET_LENGTH];
@@ -72,7 +70,7 @@
wait_ms(2); // fix this!!!
m_txEnable = 0; // Disable Tx / Enable Rx
} else {
- //pc.printf("Dynamixel not writeable\n");
+ //Dynamixel not writeable
}
wait_ms(10);
if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
@@ -86,7 +84,7 @@
uint8_t Dynamixel::move(uint16_t position)
{
- pc.printf("\n ---Move--- \n");
+ DEBUG("\n ---Move--- \n");
// 0 to 1023 (0x3FF)
uint8_t elements = 9;
uint8_t instructionBuffer[elements];
@@ -104,11 +102,11 @@
instructionBuffer[7] = (uint8_t) (position >> 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++) {
- pc.printf("%x ", instructionBuffer[i]);
+ DEBUG("%x ", instructionBuffer[i]);
}
- pc.printf("\n");
+ DEBUG("\n");
m_txEnable = 1; // Enable Tx / Disable Rx
for (int i = 0; i<= elements; i++) {
@@ -117,23 +115,23 @@
wait_ms(2); // fix this!!!
m_txEnable = 0; // Disable Tx / Enable Rx
} else {
- pc.printf("Dynamixel not writeable\n");
+ //Dynamixel not writeable
}
wait_ms(10);
if ( m_motorID != BROADCAST_ID ) {
- pc.printf("Trying to receiv status packet.\n");
+ 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);
}
- pc.printf("Read %d bytes.\n", i);
+ DEBUG("Read %d bytes.\n", i);
}
// Check status packet content
for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
- pc.printf("%x ", statusBuffer[i]);
+ DEBUG("%x ", statusBuffer[i]);
}
return statusBuffer[4]; // Return error
@@ -141,7 +139,7 @@
uint8_t Dynamixel::setSpeed(uint16_t speed)
{
- pc.printf("\n --- Set speed--- \n");
+ DEBUG("\n --- Set speed--- \n");
// 0 to 1023 (0x3FF)
uint8_t elements = 9;
uint8_t instructionBuffer[elements];
@@ -167,22 +165,22 @@
wait_ms(2); // fix this!!!
m_txEnable = 0; // Disable Tx / Enable Rx
} else {
- //pc.printf("Dynamixel not writeable\n");
+ //Dynamixel not writeable
}
wait_ms(10);
if ( m_motorID != BROADCAST_ID ) {
- pc.printf("Trying to receiv status packet.\n");
+ 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);
}
- pc.printf("Read %d bytes.\n", i);
+ DEBUG("Read %d bytes.\n", i);
}
// Check status packet content
for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
- pc.printf("%x ", statusBuffer[i]);
+ DEBUG("%x ", statusBuffer[i]);
}
return statusBuffer[4]; // Return error
}
@@ -206,11 +204,11 @@
checkSum = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6]);
instructionBuffer[7] = (uint8_t) (checkSum & 0xff); // Check sum
-
+ // Check instruction buffer
for (int i = 0; i < elements; i++) {
- pc.printf("%x ", instructionBuffer[i]);
+ DEBUG("%x ", instructionBuffer[i]);
}
- pc.printf("\n");
+ DEBUG("\n");
m_txEnable = 1; // Enable Tx / Disable Rx
for (int i = 0; i<= elements; i++) {
@@ -219,25 +217,25 @@
wait_ms(2); // fix this!!!
m_txEnable = 0; // Disable Tx / Enable Rx
} else {
- pc.printf("Dynamixel not writeable\n");
+ //Dynamixel not writeable
}
wait_ms(10);
if ( m_motorID != BROADCAST_ID ) {
- pc.printf("Trying to receiv status packet.\n");
+ DEBUG("Trying to receiv status packet.\n");
int i = 0;
while( m_link.readable() && i<STATUS_PACKET_LENGTH + 1) {
statusBuffer[i] = m_link.getc(); // Read status packet
i++;
//wait_ms(100);
}
- pc.printf("Read %d bytes.\n", i);
+ DEBUG("Read %d bytes.\n", i);
}
// Check status packet content
for (int i = 0; i < STATUS_PACKET_LENGTH + 1; i++) {
- pc.printf("%x ", statusBuffer[i]);
+ DEBUG("%x ", statusBuffer[i]);
}
- pc.printf("\n Delay time: %d [us]\n",statusBuffer[5]*2);
+ DEBUG("\n Delay time: %d [us]\n",statusBuffer[5]*2);
return statusBuffer[5]; // Return parameter
}
\ No newline at end of file