mbed to Dynamixel ping test.

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }