Diff: XL320_MFA/XL320_MFA.h

Revision:

0:2071f8e463dd

Child:

2:0ec2dce7c5f8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/XL320_MFA/XL320_MFA.h	Wed Feb 24 10:25:39 2016 +0000
@@ -0,0 +1,189 @@
+//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);
+
+////////// 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    
\ No newline at end of file