Dan Gongora
/
dynamixel_pingTest
mbed to Dynamixel ping test.
main.cpp@1:0750ffd50d77, 2015-08-18 (annotated)
- Committer:
- dmgongora
- Date:
- Tue Aug 18 04:43:28 2015 +0000
- Revision:
- 1:0750ffd50d77
- Parent:
- 0:2f6ebc930afa
Deleted reference to unused header file
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dmgongora | 0:2f6ebc930afa | 1 | /***************************************************** |
dmgongora | 0:2f6ebc930afa | 2 | - Description: mbed to Dynamixel connection test |
dmgongora | 0:2f6ebc930afa | 3 | - Requirements: |
dmgongora | 0:2f6ebc930afa | 4 | Dynamixel (i.e. DX116, RX28) |
dmgongora | 0:2f6ebc930afa | 5 | MAX3088/MAX485 (RS485 transceiver) |
dmgongora | 0:2f6ebc930afa | 6 | - Connections: |
dmgongora | 0:2f6ebc930afa | 7 | MAX3088 -- mbed |
dmgongora | 0:2f6ebc930afa | 8 | ====================== |
dmgongora | 0:2f6ebc930afa | 9 | Pin 1 -- Pin 14 |
dmgongora | 0:2f6ebc930afa | 10 | Pin 2 -- Pin 15 |
dmgongora | 0:2f6ebc930afa | 11 | Pin 4 -- Pin 13 |
dmgongora | 0:2f6ebc930afa | 12 | |
dmgongora | 0:2f6ebc930afa | 13 | - Comments: |
dmgongora | 0:2f6ebc930afa | 14 | See schematic for wiring details |
dmgongora | 0:2f6ebc930afa | 15 | *****************************************************/ |
dmgongora | 0:2f6ebc930afa | 16 | #include "mbed.h" |
dmgongora | 0:2f6ebc930afa | 17 | // From Dynamixel instruction set |
dmgongora | 0:2f6ebc930afa | 18 | const unsigned char PING = 0x01; |
dmgongora | 0:2f6ebc930afa | 19 | void pingDynamixel(uint8_t); |
dmgongora | 0:2f6ebc930afa | 20 | // Half duplex Asynchronouse serial communication |
dmgongora | 0:2f6ebc930afa | 21 | // 8 bit, 1 stop, No parity (mbed default config) |
dmgongora | 0:2f6ebc930afa | 22 | Serial dx116(p13, p14); |
dmgongora | 0:2f6ebc930afa | 23 | // Control pin for the half duplex communication |
dmgongora | 0:2f6ebc930afa | 24 | DigitalOut TxEnable(p15); |
dmgongora | 0:2f6ebc930afa | 25 | Serial pc(USBTX, USBRX); |
dmgongora | 0:2f6ebc930afa | 26 | int main() |
dmgongora | 0:2f6ebc930afa | 27 | { |
dmgongora | 0:2f6ebc930afa | 28 | TxEnable = 0; // Disable Tx/Enable Rx |
dmgongora | 0:2f6ebc930afa | 29 | const uint8_t motorID = 0x01; // Dynamixel ID |
dmgongora | 0:2f6ebc930afa | 30 | const int dynamixelBR = 57600; |
dmgongora | 0:2f6ebc930afa | 31 | dx116.baud(dynamixelBR); |
dmgongora | 0:2f6ebc930afa | 32 | pc.baud(9600); |
dmgongora | 0:2f6ebc930afa | 33 | |
dmgongora | 0:2f6ebc930afa | 34 | pc.printf("Request status packet from ID #%x\n with %d bps", motorID, dynamixelBR); |
dmgongora | 0:2f6ebc930afa | 35 | pingDynamixel(motorID); |
dmgongora | 0:2f6ebc930afa | 36 | while(1) { |
dmgongora | 0:2f6ebc930afa | 37 | if (dx116.readable()) { |
dmgongora | 0:2f6ebc930afa | 38 | pc.printf("0x%x\n", dx116.getc()); // Read status packet if available |
dmgongora | 0:2f6ebc930afa | 39 | wait_ms(100); |
dmgongora | 0:2f6ebc930afa | 40 | } |
dmgongora | 0:2f6ebc930afa | 41 | wait_ms(5); |
dmgongora | 0:2f6ebc930afa | 42 | } |
dmgongora | 0:2f6ebc930afa | 43 | } |
dmgongora | 0:2f6ebc930afa | 44 | |
dmgongora | 0:2f6ebc930afa | 45 | void pingDynamixel(uint8_t myID) |
dmgongora | 0:2f6ebc930afa | 46 | { |
dmgongora | 0:2f6ebc930afa | 47 | uint8_t elements = 6; |
dmgongora | 0:2f6ebc930afa | 48 | uint8_t packetBuffer[elements]; |
dmgongora | 0:2f6ebc930afa | 49 | if (dx116.writeable() ) { |
dmgongora | 0:2f6ebc930afa | 50 | packetBuffer[0] = 0xff; |
dmgongora | 0:2f6ebc930afa | 51 | packetBuffer[1] = 0xff; |
dmgongora | 0:2f6ebc930afa | 52 | packetBuffer[2] = myID; // ID |
dmgongora | 0:2f6ebc930afa | 53 | packetBuffer[3] = 0x02; // Length |
dmgongora | 0:2f6ebc930afa | 54 | packetBuffer[4] = PING; // Instruction |
dmgongora | 0:2f6ebc930afa | 55 | packetBuffer[5] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4]) & 0xFF; // Check sum |
dmgongora | 0:2f6ebc930afa | 56 | // Send instruction packet |
dmgongora | 0:2f6ebc930afa | 57 | TxEnable = 1; // Enable Tx / Disable Rx |
dmgongora | 0:2f6ebc930afa | 58 | for (int i = 0; i<= elements; i++) { |
dmgongora | 0:2f6ebc930afa | 59 | dx116.putc(packetBuffer[i]); |
dmgongora | 0:2f6ebc930afa | 60 | } |
dmgongora | 0:2f6ebc930afa | 61 | wait_ms(1); // fix this!!! |
dmgongora | 0:2f6ebc930afa | 62 | TxEnable = 0; // Disable Tx / Enable Rx |
dmgongora | 0:2f6ebc930afa | 63 | } else { |
dmgongora | 0:2f6ebc930afa | 64 | pc.printf("Dynamixel not writeable\n"); |
dmgongora | 0:2f6ebc930afa | 65 | } |
dmgongora | 0:2f6ebc930afa | 66 | } |