Dan Gongora
/
dynamixel_pingTest
mbed to Dynamixel ping test.
main.cpp
- Committer:
- dmgongora
- Date:
- 2015-08-18
- Revision:
- 1:0750ffd50d77
- Parent:
- 0:2f6ebc930afa
File content as of revision 1:0750ffd50d77:
/***************************************************** - Description: mbed to Dynamixel connection test - Requirements: Dynamixel (i.e. DX116, RX28) MAX3088/MAX485 (RS485 transceiver) - Connections: MAX3088 -- mbed ====================== Pin 1 -- Pin 14 Pin 2 -- Pin 15 Pin 4 -- Pin 13 - Comments: See schematic for wiring details *****************************************************/ #include "mbed.h" // From Dynamixel instruction set const unsigned char PING = 0x01; void pingDynamixel(uint8_t); // Half duplex Asynchronouse serial communication // 8 bit, 1 stop, No parity (mbed default config) Serial dx116(p13, p14); // Control pin for the half duplex communication DigitalOut TxEnable(p15); Serial pc(USBTX, USBRX); int main() { TxEnable = 0; // Disable Tx/Enable Rx const uint8_t motorID = 0x01; // Dynamixel ID const int dynamixelBR = 57600; dx116.baud(dynamixelBR); pc.baud(9600); pc.printf("Request status packet from ID #%x\n with %d bps", motorID, dynamixelBR); pingDynamixel(motorID); while(1) { if (dx116.readable()) { pc.printf("0x%x\n", dx116.getc()); // Read status packet if available wait_ms(100); } wait_ms(5); } } void pingDynamixel(uint8_t myID) { uint8_t elements = 6; uint8_t packetBuffer[elements]; if (dx116.writeable() ) { packetBuffer[0] = 0xff; packetBuffer[1] = 0xff; packetBuffer[2] = myID; // ID packetBuffer[3] = 0x02; // Length packetBuffer[4] = PING; // Instruction packetBuffer[5] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4]) & 0xFF; // Check sum // Send instruction packet TxEnable = 1; // Enable Tx / Disable Rx for (int i = 0; i<= elements; i++) { dx116.putc(packetBuffer[i]); } wait_ms(1); // fix this!!! TxEnable = 0; // Disable Tx / Enable Rx } else { pc.printf("Dynamixel not writeable\n"); } }