Revision 0:4707f8d8d7de, committed 2016-02-16

Comitter:

Ucuppe

Date:

Tue Feb 16 09:32:50 2016 +0000

Commit message:

yowasap

Changed in this revision

diff -r 000000000000 -r 4707f8d8d7de dynamixel.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dynamixel.cpp	Tue Feb 16 09:32:50 2016 +0000
@@ -0,0 +1,355 @@
+#include "dynamixel.h"
+
+dynamixel::dynamixel(uint32_t baudrate,PinName pctx,PinName pcrx,PinName tx,PinName rx,PinName dirpin):_pc(pctx,pcrx),_dynamixel(tx,rx),_dirpin(dirpin)
+{   
+    _dirpin=0;
+    _pc.baud(115200);
+    _dynamixel.baud(baudrate); 
+}
+
+unsigned short dynamixel::update_crc(unsigned short crc_accum, unsigned char *data_blk_ptr, unsigned short data_blk_size)
+{
+    unsigned short i, j;
+    unsigned short crc_table[256] = {
+        0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
+        0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
+        0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
+        0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
+        0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
+        0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
+        0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
+        0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
+        0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
+        0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
+        0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
+        0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
+        0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
+        0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
+        0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
+        0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
+        0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
+        0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
+        0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
+        0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
+        0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
+        0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
+        0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
+        0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
+        0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
+        0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
+        0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
+        0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
+        0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
+        0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
+        0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
+        0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
+    };
+ 
+    for(j = 0; j < data_blk_size; j++)
+    {
+        i = ((unsigned short)(crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF;
+        crc_accum = (crc_accum << 8) ^ crc_table[i];
+    }
+ 
+    return crc_accum;
+}
+
+void dynamixel::prepacket(uint8_t ID,uint16_t Address,uint8_t instruction,uint8_t sizeofpacket)
+{
+    txbuffer[0] = 0xff; //Header
+    txbuffer[1] = 0xff; //Header
+    txbuffer[2] = 0xfd; //Header
+    txbuffer[3] = 0x00; //Reserved
+    txbuffer[4] = ID;   //ID
+    txbuffer[5] = DXL_LOBYTE(5+sizeofpacket);   //Packet Length Low
+    txbuffer[6] = DXL_HIBYTE(5+sizeofpacket);   //Packet Length High
+    txbuffer[7] = instruction;                  //Instruction
+    txbuffer[8] = DXL_LOBYTE(Address);          //Parameter address,Low
+    txbuffer[9] = DXL_HIBYTE(Address);          //Parameter address,High 
+}
+
+void dynamixel::postpacket(uint8_t sizeofpacket)
+{
+    uint8_t a;
+    crc = 0;
+    crc = update_crc(0, txbuffer, 10+sizeofpacket);
+    txbuffer[10+sizeofpacket] = DXL_LOBYTE(crc);
+    txbuffer[11+sizeofpacket] = DXL_HIBYTE(crc);    
+    //Send data buffer
+    _dirpin= 1; 
+    for(a= 0; a < 12+sizeofpacket; a++)
+    {
+        _dynamixel.putc(txbuffer[a]);
+    }
+    //Clean buffer
+    for(a = 0; a < 12+sizeofpacket; a++)
+    {
+        txbuffer[a] = 0;
+    }
+     wait_us(10);
+    _dirpin= 0;    
+}
+
+void dynamixel::returnpacket(uint8_t sizeofreturnpacket)
+{
+    uint8_t a;
+   //Receive dynamixel return packet    
+    _pc.printf("[");
+    a=0;
+    while(_dynamixel.readable() && a < 11+sizeofreturnpacket) 
+    {
+       
+        //for(a = 0; a < 11+sizeofreturnpacket; a++)
+        //{
+            uint8_t x = _dynamixel.getc();            
+            _pc.printf("%x |",x);
+        //}
+        a++;
+        //_pc.printf("]\n");
+    }     
+    _pc.printf("]\n");
+}
+    
+    
+void dynamixel::writePacket2(uint8_t ID,uint16_t Address,uint16_t value){
+    prepacket(ID,Address,WRITE_DATA,2);
+    //packet data array
+    txbuffer[10]= DXL_LOBYTE(value); //Parameter value,low
+    txbuffer[11]= DXL_HIBYTE(value); //Parameter value,high 
+    postpacket(2);
+    returnpacket(0);
+}
+
+void dynamixel::writePacket(uint8_t ID,uint16_t Address,uint8_t value){
+    prepacket(ID,Address,WRITE_DATA,1);
+    //packet data array
+    txbuffer[10]= value; //Parameter value,low
+    postpacket(1);
+    returnpacket(0);
+}
+
+void dynamixel::readPacket(uint8_t ID,uint16_t Address,uint8_t value){
+    prepacket(ID,Address,READ_DATA,2);
+    //packet data array
+    txbuffer[10]= DXL_LOBYTE(value); //Parameter value,low
+    txbuffer[11]= DXL_HIBYTE(value); //Parameter value,high 
+    postpacket(2);
+    returnpacket(value);    
+}
+
+////// EEPROM ACCESS METHODS //////
+    
+    /***** XL320 Network Parameter *****/
+void dynamixel::SetID(uint8_t id,uint8_t idnew)
+{
+    writePacket(id,XL_ID,idnew);
+}
+uint8_t dynamixel::GetID(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetBaudRate(uint8_t id,uint8_t baudrate)
+{
+    writePacket(id,BAUD_RATE,baudrate);
+}
+uint8_t dynamixel::GetBaudRate(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetRetDelTime(uint8_t id,uint8_t time)
+{
+    writePacket(id,RETURN_DELAY_TIME,time);
+}
+uint8_t dynamixel::GetRetDelTime(uint8_t id)
+{
+    return 0;
+}
+
+    /***** XL320 Motor Setting *****/
+void dynamixel::SetCWAngLim(uint8_t id,uint16_t angle)
+{
+    writePacket2(id,CW_ANGLE_LIMIT,angle);
+}
+uint16_t dynamixel::GetCWAngLim(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetCCWAngLim(uint8_t id,uint16_t angle)
+{
+    writePacket2(id,CCW_ANGLE_LIMIT,angle);
+}
+uint16_t dynamixel::GetCCWAngLim(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetContMode(uint8_t id,uint8_t mode)
+{
+    writePacket(id,CONTROL_MODE,mode);
+}
+uint8_t dynamixel::GetContMode(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetTempLim(uint8_t id,uint8_t temp)
+{
+    writePacket(id,LIMIT_TEMPERATURE,temp);
+}
+uint8_t dynamixel::GetTempLim(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetLowVoltLim(uint8_t id,uint8_t volt)
+{
+    writePacket(id,LOWER_LIMIT_VOLTAGE,volt);
+}
+uint8_t dynamixel::GetLowVoltLim(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetUpVoltLim(uint8_t id,uint8_t volt)
+{
+    writePacket(id,UPPER_LIMIT_VOLTAGE,volt);
+}
+uint8_t dynamixel::GetUpVoltLim(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetMaxTorq(uint8_t id,uint16_t torque)
+{
+    writePacket2(id,MAX_TORQUE,torque);
+}
+uint16_t dynamixel::GetMaxTorq(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetRetLev(uint8_t id,uint8_t level)
+{
+    writePacket(id,RETURN_LEVEL,level);
+}
+uint8_t dynamixel::GetRetLev(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetAlarmShut(uint8_t id,uint8_t alarm)
+{
+    writePacket(id,ALARM_SHUTDOWN,alarm);
+}
+uint8_t dynamixel::GetAlarmShut(uint8_t id)
+{
+    return 0;
+}
+
+////// RAM ACCESS METHODS //////
+    /***** XL320 On/Off *****/
+void dynamixel::SetTorqueEn(uint8_t id,uint8_t enable)
+{
+    writePacket(id,TORQUE_ENABLE,enable);
+}
+uint8_t dynamixel::GetTorqueEn(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::TurnOnLED(uint8_t id,uint8_t led)
+{
+    writePacket(id,LED,led);
+}
+uint8_t dynamixel::GetStatusLED(uint8_t id)
+{
+    return 0;
+}
+    /***** XL320 Motor Control *****/
+void dynamixel::SetDGain(uint8_t id,uint8_t d_cons)
+{
+    writePacket(id,D_GAIN,d_cons);
+}
+uint8_t dynamixel::GetDGain(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetIGain(uint8_t id,uint8_t i_cons)
+{
+    writePacket(id,I_GAIN,i_cons);
+}
+uint8_t dynamixel::GetIGain(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetPGain(uint8_t id,uint8_t p_cons)
+{
+    writePacket(id,P_GAIN,p_cons);
+}
+uint8_t dynamixel::GetPGain(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetGoalPos(uint8_t id,uint16_t position)
+{
+    writePacket(id,GOAL_POSITION,position);
+}
+void dynamixel::SetGoalVel(uint8_t id,uint16_t velocity)
+{
+    writePacket(id,GOAL_SPEED,velocity);
+}
+void dynamixel::SetGoalTorq(uint8_t id,uint16_t torque)
+{
+    writePacket2(id,GOAL_TORQUE,torque);
+}
+uint16_t dynamixel::GetGoalTorq(uint8_t id)
+{
+    return 0;
+}
+
+    /***** XL320 Feedback *****/
+uint16_t dynamixel::GetPresentPos(uint8_t id)
+{
+    return 0;
+}
+uint16_t dynamixel::GetPresentSpeed(uint8_t id)
+{
+    return 0;
+}
+uint16_t dynamixel::GetPresentLoad(uint8_t id)
+{
+    return 0;
+}
+uint8_t dynamixel::GetPresentVolt(uint8_t id)
+{
+    return 0;
+}
+uint8_t dynamixel::GetPresentTemp(uint8_t id)
+{
+    return 0;
+}
+
+    /***** XL320 Status *****/
+uint8_t dynamixel::GetRegInst(uint8_t id)
+{
+    return 0;
+}  
+uint8_t dynamixel::GetMoving(uint8_t id)
+{
+    return 0;
+}
+uint8_t dynamixel::GetHWErr(uint8_t id)
+{
+    return 0;
+}
+void dynamixel::SetPunch(uint8_t id,uint16_t punch)
+{
+    writePacket2(id,PUNCH,punch);
+}
+
+    
+    /***** XL320 Instruction Method *****/  
+void dynamixel::FactoryReset(uint8_t id,uint8_t option)
+{
+    
+}
+void dynamixel::Ping(uint8_t id)
+{
+    
+}
+
+
+
+dynamixel::~dynamixel(){};
\ No newline at end of file

diff -r 000000000000 -r 4707f8d8d7de dynamixel.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dynamixel.h	Tue Feb 16 09:32:50 2016 +0000
@@ -0,0 +1,148 @@
+#ifndef DYNAMIXEL_H
+#define DYNAMIXEL_H
+
+#include "mbed.h"
+
+#define DXL_LOBYTE(w)           (uint8_t((uint16_t(w)) & 0xff))//masih ragu
+#define DXL_HIBYTE(w)           (uint8_t((uint16_t(w) >> 8) & 0xff))
+  // EEPROM AREA  ///////////////////////////////////////////////////////////
+#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 AREA  //////////////////////////////////////////////////////////////
+#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  //ADA YANG ANEH
+#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
+
+
+
+class dynamixel{
+    
+    public:
+    dynamixel(uint32_t baudrate,PinName pctx,PinName pcrx,PinName tx,PinName rx,PinName dirpin);
+    ~dynamixel();
+    void writePacket(uint8_t ID,uint16_t Address,uint8_t value);
+    void writePacket2(uint8_t ID,uint16_t Address,uint16_t value);
+    void readPacket(uint8_t ID,uint16_t Address,uint8_t value);
+    
+    ////// 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);
+    void SetGoalVel(uint8_t id,uint16_t velocity);
+    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 FactoryReset(uint8_t id,uint8_t option);
+    void Ping(uint8_t id);
+    
+    private:
+    uint16_t crc;
+    uint8_t txbuffer[256];
+    Serial _pc;
+    Serial _dynamixel;
+    DigitalOut _dirpin;
+    //receive();
+    void prepacket(uint8_t ID,uint16_t Address,uint8_t instruction,uint8_t sizeofpacket);
+    void postpacket(uint8_t sizeofpacket);
+    void returnpacket(uint8_t sizeofreturnpacket);
+    unsigned short update_crc(unsigned short crc_accum, unsigned char *data_blk_ptr, unsigned short data_blk_size);
+    
+};
+
+#endif    
\ No newline at end of file

diff -r 000000000000 -r 4707f8d8d7de main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Feb 16 09:32:50 2016 +0000
@@ -0,0 +1,20 @@
+#include "dynamixel.h"
+
+dynamixel servo(1000000,USBTX,USBRX,p9,p10,p5);
+
+int main() {
+    
+   while(1) {
+    wait(1);
+    servo.readPacket(1,GOAL_POSITION,2);
+    servo.writePacket2(1,GOAL_POSITION,0);
+    wait(1);
+    servo.writePacket2(1,GOAL_POSITION,100);
+    //wait(0.8);
+    servo.readPacket(1,GOAL_POSITION,2);
+    //servo.readPacket(1,BAUD_RATE,1);
+    //return 0;
+
+}
+
+}

diff -r 000000000000 -r 4707f8d8d7de mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Feb 16 09:32:50 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/6f327212ef96
\ No newline at end of file