File content as of revision 2:342f5327a1e0:

#include "DynamixelAX12.h"
#include "mbed.h"

Dynamixel::Dynamixel(void):serialPort(p13,p14),directionPin(p12,0){
    serialPort.baud(1000000);
    position=0;
    velocity=0;
    led=false;
    serialDirection=false;
    mode=false;
    ID=0xFE;
}//Builder0

Dynamixel::Dynamixel(unsigned int NewID):serialPort(p13,p14),directionPin(p12,0){
    serialPort.baud(1000000);
    position=0;
    velocity=0;
    led=false;
    serialDirection=false;
    mode=false;
    ID=(unsigned char)(NewID & 0XFF);
}//Builder1

Dynamixel::Dynamixel(unsigned int NewID,PinName Newtx,PinName Newrx,PinName newdirectionPin,bool NewMode):serialPort(Newtx,Newrx),directionPin(newdirectionPin,1){
    serialPort.baud(1000000);
    position=0;
    velocity=0;
    led=false;
    serialDirection=false;
    ID=NewID;
    mode=NewMode;
    ID=(unsigned char)(NewID & 0XFF);
}//Builder2

void Dynamixel::SetLED(bool ledState){
    char TxBuf[8];
    TxBuf[0]=0xFF;              //Start of incoming packet
    TxBuf[1]=0xFF;              //Start of incoming packet
    TxBuf[2]=ID;                //ID
    TxBuf[3]=0x04;              //Length = number of parameters + 2
    TxBuf[4]=iWriteData;        //Instruction
    TxBuf[5]=cLED;              //Parameter 1
    if(ledState)
        TxBuf[6]=0x01;          //Parameter 2
    else
        TxBuf[6]=0x00;          //Parameter 2
    TxBuf[7]= ~(ID+TxBuf[3]+iWriteData+cLED+TxBuf[6]);
    directionPin.write(1);
    for(int i=0;i<8;i++){
        serialPort.putc(TxBuf[i]);
    }//for
    wait_ms(10);
    directionPin.write(0);
}//SetLED

void Dynamixel::SetMode(bool Mode){
    char TxBuf[11];
    TxBuf[0]=0xFF;              //Start of incoming packet0
    TxBuf[1]=0xFF;              //Start of incoming packet1
    TxBuf[2]=ID;                //ID
    TxBuf[3]=0x07;              //Length = number of parameters + 2
    TxBuf[4]=iWriteData;        //Instruction
    TxBuf[5]=cCWAngleLimitL;    //Parameter 1
    TxBuf[6]=0x00;              //Parameter 2
    TxBuf[7]=0x00;              //Parameter 3
    if(Mode){
        TxBuf[8]=0x00;          //Parameter 4
        TxBuf[9]=0x00;          //Parameter 5
    }else{
        TxBuf[8]=0xFF;          //Parameter 4
        TxBuf[9]=0x03;          //Parameter 5
    }//else
    TxBuf[10]= ~(ID+TxBuf[3]+iWriteData+cCWAngleLimitL+TxBuf[6]+TxBuf[7]+TxBuf[8]+TxBuf[9]);
    directionPin.write(1);
    for(int i=0;i<11;i++){
        serialPort.putc(TxBuf[i]);
    }//for
    wait_ms(10);
    directionPin.write(0);
}//SetMode

void Dynamixel::SetPosition(unsigned long Newposition){
    char TxBuf[9];
    TxBuf[0]=0xFF;              //Start of incoming packet0
    TxBuf[1]=0xFF;              //Start of incoming packet1
    TxBuf[2]=ID;                //ID
    TxBuf[3]=0x05;              //Length = number of parameters + 2
    TxBuf[4]=iWriteData;        //Instruction
    TxBuf[5]=cGoalPositionL;    //Parameter 1
    TxBuf[6]=(unsigned char)(Newposition & 0XFF);
    TxBuf[7]=(unsigned char)((Newposition >> 8) & 0XFF);
    TxBuf[8]= ~(ID+TxBuf[3]+iWriteData+cGoalPositionL+TxBuf[6]+TxBuf[7]);
    directionPin.write(1);
    for(int i=0;i<9;i++){
        serialPort.putc(TxBuf[i]);
    }//for
    wait_ms(10);
    directionPin.write(0);
}//SetPosition

void Dynamixel::SetVelocity(unsigned long Newvelocity){
    char TxBuf[9];
    TxBuf[0]=0xFF;              //Start of incoming packet0
    TxBuf[1]=0xFF;              //Start of incoming packet1
    TxBuf[2]=ID;                //ID
    TxBuf[3]=0x05;              //Length = number of parameters + 2
    TxBuf[4]=iWriteData;         //Instruction
    TxBuf[5]=cMovingSpeedL;     //Parameter 1
    TxBuf[6]=(unsigned char)(Newvelocity & 0XFF);
    TxBuf[7]=(unsigned char)((Newvelocity >> 8) & 0XFF);
    TxBuf[8]= ~(ID+TxBuf[3]+iWriteData+TxBuf[5]+TxBuf[6]+TxBuf[7]);
    directionPin.write(1);
    for(int i=0;i<9;i++){
        serialPort.putc(TxBuf[i]);
    }//for
    wait_ms(10);
    directionPin.write(0);
}//SetVelocity

void Dynamixel::SetDirection(bool Mode){
    char TxBuf[9];
    TxBuf[0]=0xFF;              //Start of incoming packet0
    TxBuf[1]=0xFF;              //Start of incoming packet1
    TxBuf[2]=ID;                //ID
    TxBuf[3]=0x05;              //Length = number of parameters + 2
    TxBuf[4]=iWriteData;        //Instruction
    TxBuf[5]=cMovingSpeedL;     //Parameter 1
    TxBuf[6]=0xFF;              //Parameter 2
    if(Mode)
        TxBuf[7]=0x03;          //Parameter 3
    else
        TxBuf[7]=0x07;          //Parameter 3
    TxBuf[8]= ~(ID+TxBuf[3]+iWriteData+cCWAngleLimitL+TxBuf[6]+TxBuf[7]);
    directionPin.write(1);
    for(int i=0;i<9;i++){
        serialPort.putc(TxBuf[i]);
    }//for
    wait_ms(10);
    directionPin.write(0);
}//SetDirection

void Dynamixel::move(){
    char TxBuf[16];
    TxBuf[0]=0xFF;              //Start of incoming packet0
    TxBuf[1]=0xFF;              //Start of incoming packet1
    TxBuf[2]=0xFE;              //ID
    TxBuf[3]=0x02;              //Length = number of parameters + 2
    TxBuf[4]=iAction;           //Instruction
    TxBuf[5]=0xFA;
    directionPin.write(1);
    for(int i=0;i<6;i++){
        serialPort.putc(TxBuf[i]);
    }//for
    wait_ms(10);
    directionPin.write(0);
}//move