mbed to Dynamixel ping test.

Dependencies:   mbed

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");
    }
}