File content as of revision 2:7f9d2f135591:

#include "mbed.h"
#include "MX28.h"

//config param
//-motor robotis
#define ID_ROLL 1
#define ID_PITCH 2
#define BANDRATE_MOTOR 1000000


//param calibration
#define CENTER_ROLL     512
#define CENTER_PITCH    512



Serial pc(USBTX, USBRX);
MX28 mx28(PA_11, PA_12, BANDRATE_MOTOR);

DigitalIn mybutton(USER_BUTTON);

//function prototype
void setup_mode();
void run_mode();


int main() 
 {
    pc.baud(115200);   
    
    pc.getc();
    pc.printf("======================================================\r\n"); 
    
   uint8_t servoId = 0x01;
    
    uint16_t modelNumber;        
    mx28.GetModelNumber(servoId, &modelNumber);
    
    uint8_t firmwareVersion;        
    mx28.GetFirmwareVersion(servoId, &firmwareVersion);
    
    uint8_t id;        
    mx28.GetId(servoId, &id);               
    mx28.SetId(servoId, servoId);
    
    int32_t baudRate;
    mx28.GetBaudRate(servoId, &baudRate);    
    mx28.SetBaudRate(servoId, 57600); 
    
    uint8_t returnDelayTime;
    mx28.GetReturnDelayTime(servoId, &returnDelayTime);     
    mx28.SetReturnDelayTime(servoId, 0xFA); 
    
    uint16_t cwAngleLimit;
    mx28.GetCWAngleLimit(servoId, &cwAngleLimit);
    mx28.SetCWAngleLimit(servoId, 0x0000);
    
    uint16_t ccwAngleLimit;
    mx28.GetCCWAngleLimit(servoId, &ccwAngleLimit);    
    mx28.SetCCWAngleLimit(servoId, 0x0FFF);
    
    uint8_t highestTemperatureLimit;
    mx28.GetHighestTemperatureLimit(servoId, &highestTemperatureLimit);       
    mx28.SetHighestTemperatureLimit(servoId, 0x50);
    
    uint8_t downLimitVoltage;
    mx28.GetLowestVoltageLimit(servoId, &downLimitVoltage);       
    mx28.SetLowestVoltageLimit(servoId, 0x3C);
   
    uint8_t upLimitVoltage;
    mx28.GetHighestVoltageLimit(servoId, &upLimitVoltage);       
    mx28.SetHighestVoltageLimit(servoId, 0xA0);
    
    uint16_t maxTorque;
    mx28.GetMaxTorque(servoId, &maxTorque);    
    mx28.SetMaxTorque(servoId, 0x03FF);
    
    uint8_t statusReturnLevel;        
    mx28.GetStatusReturnLevel(servoId, &statusReturnLevel);
    mx28.SetStatusReturnLevel(servoId, 0x02);
    
    uint8_t alarmLED;        
    mx28.GetAlarmLED(servoId, &alarmLED);           
    mx28.SetAlarmLED(servoId, 0x24);
    
    uint8_t alarmShutdown;        
    mx28.GetAlarmShutdown(servoId, &alarmShutdown);           
    mx28.SetAlarmShutdown(servoId, 0x24); 
            
    uint8_t enableTorque;   
    mx28.GetEnableTorque(servoId, &enableTorque);    
    mx28.SetEnableTorque(servoId, 0x00);
    
    uint8_t enableLED;   
    mx28.GetEnableLED(servoId, &enableLED);    
    mx28.SetEnableLED(servoId, 0x00);
    
    uint8_t pGain;        
    mx28.GetPGain(servoId, &pGain);           
    mx28.SetPGain(servoId, 0x20);
    
    uint8_t iGain;        
    mx28.GetIGain(servoId, &iGain);           
    mx28.SetIGain(servoId, 0x00);
    
    uint8_t dGain;        
    mx28.GetDGain(servoId, &dGain);           
    mx28.SetDGain(servoId, 0x00);
    
    uint16_t goalPosition;
    mx28.GetGoalPosition(servoId, &goalPosition);     
    mx28.SetGoalPosition(servoId, 0x0800);
    
    uint16_t movingSpeed;
    mx28.GetMovingSpeed(servoId, &movingSpeed);     
    mx28.SetMovingSpeed(servoId, 0x00FF);
    
    uint16_t torqueLimit;
    mx28.GetTorqueLimit(servoId, &torqueLimit);     
    mx28.SetTorqueLimit(servoId, 0x03FF);
    
    uint16_t presentPosition;
    mx28.GetPresentPosition(servoId, &presentPosition);
    
    uint16_t presentSpeed;
    mx28.GetPresentSpeed(servoId, &presentSpeed); 
 
    uint16_t presentLoad;
    mx28.GetPresentLoad(servoId, &presentLoad);     
    
    uint8_t presentVoltage;        
    mx28.GetPresentVoltage(servoId, &presentVoltage);
    
    uint8_t presentTemperature;        
    mx28.GetPresentTemperature(servoId, &presentTemperature);
    
    uint8_t isRegistered;   
 
    mx28.GetIsRegistered(servoId, &isRegistered);
    
    uint8_t isMoving;   
    mx28.GetIsMoving(servoId, &isMoving);
    
    uint8_t lock;        
    mx28.GetIsLocked(servoId, &lock);           
    mx28.SetIsLocked(servoId, 0x00); 
    
    uint16_t punch;
    mx28.GetPunch(servoId, &punch);     
    mx28.SetPunch(servoId, 0x0020);
    
    mx28.Ping(servoId);
    
    mx28.Reset(servoId);
    
    uint8_t servo1Id = 0x01;
    uint8_t servo2Id = 0x02;
    uint8_t servo3Id = 0x03;
    
    uint16_t servo1GoalPosition = 0x0800;
    uint16_t servo2GoalPosition = 0x0800;
    uint16_t servo3GoalPosition = 0x0800;
    
    MX28_PROTOCOL_PACKET packet;
    packet.servoId = MX28_PROTOCOL_BROADCAST_ID;
    // (Data length + 1) * Number of servos + 4
    packet.length = (2+ 1) * 3 + 4;             
    packet.instructionErrorId = MX28_SYNC_WRITE;
    packet.parameter[0] = MX28_GOAL_POSITION_L;
    packet.parameter[1] = 0x06;
    packet.parameter[2] = servo1Id;
    Utilities::ConvertUInt16ToUInt8Array(servo1GoalPosition, (uint8_t*)&(packet.parameter[3]));    
    packet.parameter[9] = servo2Id;
    Utilities::ConvertUInt16ToUInt8Array(servo2GoalPosition, (uint8_t*)&(packet.parameter[10]));
    packet.parameter[16] = servo3Id;
    Utilities::ConvertUInt16ToUInt8Array(servo3GoalPosition, (uint8_t*)&(packet.parameter[17]));
   
    pc.printf("Set servos goal positions: %hu %hu %hu\r\n", servo1GoalPosition, servo2GoalPosition, servo3GoalPosition);
    
    mx28.CommunicatePacket(&packet);
          
    packet.servoId = servoId;
    packet.length = 4;
    packet.instructionErrorId = MX28_READ_DATA;
    packet.parameter[0] = MX28_PRESENT_POSITION_L;
    packet.parameter[1] = 0x08;
 
    mx28.CommunicatePacket(&packet);
    
    presentPosition = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
    presentSpeed = Utilities::ConvertUInt8ArrayToUInt16((uint8_t*)&(packet.parameter[2]));   
    presentLoad = Utilities::ConvertUInt8ArrayToUInt16((uint8_t*)&(packet.parameter[4]));    
    presentVoltage = packet.parameter[6];
    presentTemperature = packet.parameter[7];
        
    pc.printf("Present position: %hu\r\n", presentPosition);
    pc.printf("Present speed: %hu\r\n", presentSpeed);
    pc.printf("Present load: %hu\r\n", presentLoad);
    pc.printf("Present voltage: 0x%02X\r\n", presentVoltage);
    pc.printf("Present temperature: 0x%02X\r\n", presentTemperature);             
        
    uint8_t status = mx28.GetModelNumber(servoId, &modelNumber);
   
    if(status == MX28_ERRBIT_WRITE_TIMEOUT)
        pc.printf("Error: Write timeout\r\n");
    else if(status == MX28_ERRBIT_READ_TIMEOUT) 
        pc.printf("Error: Read timeout\r\n");
    else if(status == MX28_ERRBIT_MASTER_CHECKSUM) 
        pc.printf("Error: Master checksum error\r\n");
    else
    {
        if(status & MX28_ERRBIT_VOLTAGE)
            pc.printf("Error: Input voltage error\r\n");
        if(status & MX28_ERRBIT_ANGLE)
            pc.printf("Error: Angle limit error\r\n");
        if(status & MX28_ERRBIT_OVERHEAT)
            pc.printf("Error: Overheat error\r\n");
        if(status & MX28_ERRBIT_RANGE)
            pc.printf("Error: Out of range error\r\n");
        if(status & MX28_ERRBIT_CHECKSUM)
            pc.printf("Error: Checksum error\r\n");
        if(status & MX28_ERRBIT_OVERLOAD)
            pc.printf("Error: Overload error\r\n");
        if(status & MX28_ERRBIT_INSTRUCTION)
            pc.printf("Error: Instruction code error\r\n");
    }
    
    pc.printf("======================================================\r\n"); 
 
    return 0;           
 }
/*
int main()
{
    pc.baud(115200);
    pc.printf("======================================================\r\n");
    if(mybutton == 0) {
        //Setup mode
        pc.printf("Setup Mode\r\n");
        setup_mode();
    } else {
        //Run mode
        pc.printf("Run Mode\r\n");
        run_mode();
    }
    return 0;
}

void setup_mode()
{

}

void run_mode()
{

}
*/