File content as of revision 4:2df002cf5f6c:

//Author : Muhammad Fajar Adityo
//This file is based on Arduino Library for Dynamixel XL-320 Servo found somewhere in the Internet

#ifndef XL320_H
#define XL320_H

#include "mbed.h"

static Serial pc(USBTX,USBRX);
////////// DEBUG ///////////
#define DATA_DEBUG				//Command this line to disable some of pc.printf in XL320_MFA.cpp

////////// SHIFT BYTES ///////////
#define SHIFT_TO_LSB(w)			(uint8_t((uint16_t(w)) & 0x00ff))
#define SHIFT_TO_MSB(w)			(uint8_t((uint16_t(w) >> 8) & 0x00ff))

////////// EEPROM ///////////
#define MODEL_NUMBER            0
#define VERSION                 2
#define XL_ID                   3
#define BAUD_RATE               4	//0: 9600, 1:57600, 2:115200, 3:1Mbps
#define RETURN_DELAY_TIME       5
#define CW_ANGLE_LIMIT          6
#define CCW_ANGLE_LIMIT         8
#define SYSTEM_DATA2            10
#define CONTROL_MODE            11
#define LIMIT_TEMPERATURE       12
#define LOWER_LIMIT_VOLTAGE     13
#define UPPER_LIMIT_VOLTAGE     14
#define MAX_TORQUE              15
#define RETURN_LEVEL            17
#define ALARM_SHUTDOWN          18

////////// RAM ///////////
#define TORQUE_ENABLE           24
#define LED                     25
#define D_GAIN                  27
#define I_GAIN                  28
#define P_GAIN                  29
#define GOAL_POSITION           30
#define GOAL_SPEED              32
#define GOAL_TORQUE             35
#define PRESENT_POSITION        37
#define PRESENT_SPEED           39
#define PRESENT_LOAD            41
#define PRESENT_VOLTAGE         45
#define PRESENT_TEMPERATURE     46
#define REGISTERED_INSTRUCTION  47
#define MOVING                  49
#define HARDWARE_ERROR_STATUS   50
#define PUNCH                   51

////////// INSTRUCTION ///////////
#define PING                    1
#define READ_DATA               2
#define WRITE_DATA              3
#define REG_WRITE               4
#define ACTION                  5
#define FACTORY_RESET           6
#define REBOOT                  8
#define STATUS                  85
#define SYNC_READ               130
#define SYNC_WRITE              131
#define BULK_READ               146
#define BULK_WRITE              147

////////// ERROR ///////////
#define RESULT_FAIL				0x01
#define INS_ERR					0x02
#define CRC_ERR					0x03
#define DATA_RANGE_ERR			0x04
#define DATA_LENGTH_ERR			0x05
#define DATA_LIMIT_ERR			0x06
#define ACCESS_ERR				0x07

////////// RESET ///////////
#define RESET_ALL				0xFF
#define RESET_EXP_ID			0x01
#define RESET_EXP_ID_BR			0x02

class XL320{
    
    public:
		XL320(uint32_t baudrate, PinName tx, PinName rx, PinName dirpin);
		~XL320();
    
		////// EEPROM ACCESS METHODS //////
		
		/***** XL320 Network Parameter *****/
		void SetID(uint8_t id,uint8_t idnew);
		uint8_t GetID(uint8_t id);
		void SetBaudRate(uint8_t id,uint8_t baudrate);
		uint8_t GetBaudRate(uint8_t id);
		void SetRetDelTime(uint8_t id,uint8_t time);
		uint8_t GetRetDelTime(uint8_t id);

		/***** XL320 Motor Setting *****/
		void SetCWAngLim(uint8_t id,uint16_t angle);
		uint16_t GetCWAngLim(uint8_t id);
		void SetCCWAngLim(uint8_t id,uint16_t angle);
		uint16_t GetCCWAngLim(uint8_t id);
		void SetContMode(uint8_t id,uint8_t mode);
		uint8_t GetContMode(uint8_t id);
		void SetTempLim(uint8_t id,uint8_t temp);
		uint8_t GetTempLim(uint8_t id);
		void SetLowVoltLim(uint8_t id,uint8_t volt);
		uint8_t GetLowVoltLim(uint8_t id);
		void SetUpVoltLim(uint8_t id,uint8_t volt);
		uint8_t GetUpVoltLim(uint8_t id);
		void SetMaxTorq(uint8_t id,uint16_t torque);
		uint16_t GetMaxTorq(uint8_t id);
		void SetRetLev(uint8_t id,uint8_t level);
		uint8_t GetRetLev(uint8_t id);
		void SetAlarmShut(uint8_t id,uint8_t alarm);
		uint8_t GetAlarmShut(uint8_t id);

		////// RAM ACCESS METHODS //////
		/***** XL320 On/Off *****/
		void SetTorqueEn(uint8_t id,uint8_t enable);
		uint8_t GetTorqueEn(uint8_t id);
		void TurnOnLED(uint8_t id,uint8_t led);
		uint8_t GetStatusLED(uint8_t id);

		/***** XL320 Motor Control *****/
		void SetDGain(uint8_t id,uint8_t d_cons);
		uint8_t GetDGain(uint8_t id);
		void SetIGain(uint8_t id,uint8_t i_cons);
		uint8_t GetIGain(uint8_t id);
		void SetPGain(uint8_t id,uint8_t p_cons);
		uint8_t GetPGain(uint8_t id);
		void SetGoalPos(uint8_t id,uint16_t position);
		uint16_t GetGoalPos(uint8_t id);
		void SetGoalVel(uint8_t id,uint16_t velocity);
		uint16_t GetGoalVel(uint8_t id);
		void SetGoalTorq(uint8_t id,uint16_t torque);
		uint16_t GetGoalTorq(uint8_t id);

		/***** XL320 Feedback *****/
		uint16_t GetPresentPos(uint8_t id);
		uint16_t GetPresentSpeed(uint8_t id);
		uint16_t GetPresentLoad(uint8_t id);
		uint8_t GetPresentVolt(uint8_t id);
		uint8_t GetPresentTemp(uint8_t id);

		/***** XL320 Status *****/
		uint8_t GetRegInst(uint8_t id);
		uint8_t GetMoving(uint8_t id);
		uint8_t GetHWErr(uint8_t id);
		void SetPunch(uint8_t id,uint16_t punch);
		uint16_t GetPunch(uint8_t id);
		
		/***** XL320 Instruction Method *****/  
		void iFactoryReset(uint8_t id,uint8_t option);
		void iPing(uint8_t id);
		
		/***** Debugging *****/  
		void printLastPackets();
		void translateLastRPacket();
    	
    private:
		/***** XL320 Instruction Method *****/	
		void iRead(uint16_t length, uint16_t address);
		void iWrite(uint8_t length, uint16_t address, uint8_t param[]);
		
		/***** Dynamixel Protocol 2.0 Methods *****/
		void sendIPacket();
		void getRPacket();

		/***** Calculating CRC Method *****/
		unsigned short update_crc(unsigned short crc_accum, unsigned char *data_blk_ptr, unsigned short data_blk_size);	
		
		Serial sXL320;			//Serial Line for XL320
		
		const int nBytes = 20;
		uint8_t iPacket[nBytes];	//Instruction Packet //not optimal size?
		uint8_t rPacket[nBytes];	//Return Packet //not optimal size?
		uint8_t parameter[nBytes/2];	//Parameter for Instruction //not optimal size?
		
		const int restTime = 500;		//delay time before sending another Instruction Packet
		uint8_t targetID;		//id of targeted XL320 
		uint8_t targetInst;		//instruction for targeted XL320
		uint16_t targetAddr;	//address for instruction
		uint16_t CRC;
		uint16_t iPacketLength;	//Instruction Packet Length
		uint16_t rPacketLength;
		uint16_t returnCRC;
		DigitalOut dPin;		//Direction Pin = Enable Tx/RX Pin
    
};

#endif