Dan Gongora
mbed to Dynamixel servo communication.
Dependents: dynamixel_wheel_test
Diff: Dynamixel.cpp
- Revision:
- 1:ce081666d225
- Parent:
- 0:fb8a2d249639
- Child:
- 2:32377cbec534
--- a/Dynamixel.cpp Wed Aug 19 05:39:57 2015 +0000
+++ b/Dynamixel.cpp Wed Aug 19 06:10:52 2015 +0000
@@ -8,58 +8,57 @@
m_link.baud(m_baudrate);
}
-void Dynamixel::ping()
+uint8_t Dynamixel::ping()
{
uint8_t elements = 6;
- uint8_t packetBuffer[elements];
+ uint8_t instructionBuffer[elements];
+ uint8_t statusBuffer[STATUS_PACKET_LENGTH];
+
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
+ instructionBuffer[0] = 0xff;
+ instructionBuffer[1] = 0xff;
+ instructionBuffer[2] = m_motorID; // ID
+ instructionBuffer[3] = 0x02; // Length
+ instructionBuffer[4] = PING; // Instruction
+ instructionBuffer[5] = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4]) & 0xFF; // Check sum
- pc.printf("\n Instruction packet:\n");
+ m_txEnable = 1; // Enable Tx / Disable Rx
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]);
+ m_link.putc(instructionBuffer[i]);
}
wait_ms(1); // fix this!!!
- m_txEnable = 0; // Disable Tx / Enable Rx
+ 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
+ wait_ms(10); // fix this!!!
+ if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
+ for (int i = 0; i<= STATUS_PACKET_LENGTH; i++) {
+ statusBuffer[i] = m_link.getc(); // Read status packet
+ }
}
- pc.printf("\n PING: Done.\n");
+ return statusBuffer[4]; // Return error
}
-void Dynamixel::toggleLED(uint8_t ledState)
+uint8_t Dynamixel::toggleLED(uint8_t ledState)
{
uint8_t elements = 8;
- uint8_t packetBuffer[elements];
+ uint8_t instructionBuffer[elements];
+ uint8_t statusBuffer[STATUS_PACKET_LENGTH];
+
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
+ instructionBuffer[0] = 0xff;
+ instructionBuffer[1] = 0xff;
+ instructionBuffer[2] = m_motorID; // ID
+ instructionBuffer[3] = 0x04; // Length
+ instructionBuffer[4] = WRITE_DATA; // Instruction
+ instructionBuffer[5] = ADDRESS_LED; // Parameter 1: Starting address
+ instructionBuffer[6] = ledState; // Parameter 2: First value to be writen
+ instructionBuffer[7] = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6]) & 0xFF; // Check sum
m_txEnable = 1; // Enable Tx / Disable Rx
for (int i = 0; i<= elements; i++) {
- m_link.putc(packetBuffer[i]);
+ m_link.putc(instructionBuffer[i]);
}
wait_ms(2); // fix this!!!
m_txEnable = 0; // Disable Tx / Enable Rx
@@ -67,39 +66,43 @@
//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
+ if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
+ for (int i = 0; i<= STATUS_PACKET_LENGTH; i++) {
+ statusBuffer[i] = m_link.getc(); // Read status packet
+ }
}
+ return statusBuffer[4]; // Return error
}
-void Dynamixel::move(uint16_t position)
+uint8_t Dynamixel::move(uint16_t position)
{
// 0 to 1023 (0x3FF)
uint8_t elements = 9;
- uint8_t packetBuffer[elements];
+ uint8_t instructionBuffer[elements];
+ uint8_t statusBuffer[STATUS_PACKET_LENGTH];
+
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
+ instructionBuffer[0] = 0xff;
+ instructionBuffer[1] = 0xff;
+ instructionBuffer[2] = m_motorID; // ID
+ instructionBuffer[3] = 0x05; // Length
+ instructionBuffer[4] = WRITE_DATA; // Instruction
+ instructionBuffer[5] = ADDRESS_GOAL_POSITION; // Parameter 1: Starting address
+ instructionBuffer[6] = (uint8_t) position & 0xff; // Parameter 2: First value to be writen
+ 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
/*
for (int i = 0; i<= elements; i++) {
- pc.printf("%c ", packetBuffer[i]);
+ pc.printf("%c ", instructionBuffer[i]);
}
pc.printf("\n");
*/
m_txEnable = 1; // Enable Tx / Disable Rx
for (int i = 0; i<= elements; i++) {
- m_link.putc(packetBuffer[i]);
+ m_link.putc(instructionBuffer[i]);
}
wait_ms(2); // fix this!!!
m_txEnable = 0; // Disable Tx / Enable Rx
@@ -107,39 +110,43 @@
//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
+ if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
+ for (int i = 0; i<= STATUS_PACKET_LENGTH; i++) {
+ statusBuffer[i] = m_link.getc(); // Read status packet
+ }
}
+ return statusBuffer[4]; // Return error
}
-void Dynamixel::setSpeed(uint16_t speed)
+uint8_t Dynamixel::setSpeed(uint16_t speed)
{
// 0 to 1023 (0x3FF)
uint8_t elements = 9;
- uint8_t packetBuffer[elements];
+ uint8_t instructionBuffer[elements];
uint16_t checkSum = 0;
+ uint8_t statusBuffer[STATUS_PACKET_LENGTH];
+
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
+ instructionBuffer[0] = 0xff;
+ instructionBuffer[1] = 0xff;
+ instructionBuffer[2] = m_motorID; // ID
+ instructionBuffer[3] = 0x05; // Length
+ instructionBuffer[4] = WRITE_DATA; // Instruction
+ instructionBuffer[5] = ADDRESS_MOVING_SPEED; // Parameter 1: Starting address
+ instructionBuffer[6] = (uint8_t) speed & 0xff; // Parameter 2: First value to be writen
+ instructionBuffer[7] = (uint8_t) (speed >> 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
/*
for (int i = 0; i<= elements; i++) {
- pc.printf("%c ", packetBuffer[i]);
+ pc.printf("%c ", instructionBuffer[i]);
}
pc.printf("\n");
*/
m_txEnable = 1; // Enable Tx / Disable Rx
for (int i = 0; i<= elements; i++) {
- m_link.putc(packetBuffer[i]);
+ m_link.putc(instructionBuffer[i]);
}
wait_ms(2); // fix this!!!
m_txEnable = 0; // Disable Tx / Enable Rx
@@ -147,8 +154,10 @@
//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
+ if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
+ for (int i = 0; i<= STATUS_PACKET_LENGTH; i++) {
+ statusBuffer[i] = m_link.getc(); // Read status packet
+ }
}
+ return statusBuffer[4]; // Return error
}
\ No newline at end of file