File content as of revision 0:03f84c95f73b:

// Dynamixel Servo Setup Code
// 
// Modify the commented lines in main() to setup required servos

#include "servo.h"
#include "mbed.h"
#include "SerialHalfDuplex.h"

#define BASE 10
#define SHOULDER 11
#define ELBOW 12
#define WRIST 13

Serial pc(USBTX,USBRX);
Servo servo(p9,p10);

void scan_for_servos()
{
    pc.printf("Scanning for servos at 57600 baud:\n");
    servo.SetInitBaud(57600,250);
    for(int i=0;i<254;i++){
        int model = servo.GetModelNumber(i);
        if(model == AX12_MODEL) pc.printf("AX-12 found at ID %d",i);
        if(model == MX28_MODEL) pc.printf("MX-28 found at ID %d",i);
    }   
    wait(0.5);
    pc.printf("\n\nScanning for servos at 1M baud:\n");
    servo.SetInitBaud(1000000,50);
    for(int i=0;i<254;i++){
        int model = servo.GetModelNumber(i);
        if(model == AX12_MODEL) pc.printf("AX-12 found at ID %d",i);
        if(model == MX28_MODEL) pc.printf("MX-28 found at ID %d",i);
    }   
    while(1);
}

void initialise_mx28_servo(int current_address, int target_servo)
{
    pc.printf("RUNNING SERVO SETUP ROUTINE FOR SERVO %d\n\n",target_servo);

    // MX-28s default to a baud rate of 57600 with delay time of 500us
    servo.SetInitBaud(57600, 250);
    int model = servo.GetModelNumber(current_address);
    if(model != MX28_MODEL) {
        pc.printf("Error: No MX-28 servo with ID %d found...\n",current_address);
        while(1);
    }
    pc.printf("\n\nSetting up MX-28 servo to ID %d\n",target_servo);
    servo.SetID(current_address,target_servo);
    wait(0.25);
    pc.printf("Getting servo data:\n");
    servo.DebugData(target_servo);
    pc.printf("\n\nDone (check messages above to verify status)\n");
    while(1);
}

void initialise_ax12_servo(int current_address, int target_servo)
{
    pc.printf("RUNNING SERVO SETUP ROUTINE FOR SERVO %d\n\n",target_servo);

    // AX-12s default to a baud rate of 1M with delay time of 100us
    servo.SetInitBaud(1000000, 50);
    int model = servo.GetModelNumber(current_address);
    if(model != AX12_MODEL) {
        pc.printf("Error: No AX12 servo with ID %d found...\n",current_address);
        while(1);
    }
    pc.printf("\n\nSetting up AX-12 servo to ID %d\n",target_servo);
    servo.SetID(current_address,target_servo);
    wait(0.25);
    pc.printf("Setting return delay time for servo to 500us\n");
    servo.SetDelayTime(target_servo,250);
    wait(0.25);
    pc.printf("Setting baud rate for servo to 57600\n");
    servo.SetBaud(target_servo,0x22);
    wait(0.25);
    pc.printf("Resetting connecting baud rate\n");
    servo.SetInitBaud(57600, 250);
    wait(0.25);
    pc.printf("Getting servo data:\n");
    servo.DebugData(target_servo);
    pc.printf("\n\nDone (check messages above to verify status)\n");
    while(1);
}
int main()
{
    pc.baud(115200);
    pc.printf("Dynamixel Servo Setup\n");
    
    // To scan for servos, uncomment the following line:
    //scan_for_servos();
   
    // To setup a new MX-28 servo, uncomment the following line and replace first number with current ID and second number with target ID:
    //initialise_mx28_servo(1,BASE);
    
    // To setup a new AX-12 servo, uncomment the following line and replace first number with current ID and second number with target ID:
    //initialise_ax12_servo(1,WRIST); 
    
    pc.printf("Edit the comments in main() to check and program servos.\n");
}