|
Dynamixel code
//JDN oct 10 2018
// SEE: http://emanual.robotis.com/docs/en/dxl/protocol2/
// NB it you make pkgs longert than 30 then change below
// PROGRAM USES txPkg as package
// you chould change code a little so you have
// #define ARDUINO
#define PLGT 30
uint8_t txPkg[PLGT];
// PUT AND GET OFR 8/16/32 INTs
void putInt8t(int8_t v, int16_t pos)
{
txPkg[pos] = v;
}
int8_t getInt8t(int16_t pos)
{
return txPkg[pos];
}
void putInt16t(int16_t val, int16_t pos)
{
txPkg[pos] = ( uint8_t)(val & 0x00ff);
txPkg[pos + 1] = ( uint8_t)(val >> 8 );
}
int16_t getInt16t(int16_t pos)
{
int16_t v = 0;
v = txPkg[pos + 1];
v = v << 8;
v = v | txPkg[pos];
return v;
}
void putInt32t(int32_t val, int16_t pos)
{
for (int16_t i = 0; i < 4 ; i++) {
txPkg[pos + i] = ( uint8_t)(val & 0x000000ff);
val = val >> 8;
}
}
int32_t getInt32t(int16_t pos)
{
int32_t v = 0;
for (int16_t i = 0; i < 4 ; i++) {
v = v | (int32_t)txPkg[pos + i];
v = v << 8;
}
return v;
}
// CRC16 PART
// see http://emanual.robotis.com/docs/en/dxl/crc/
#define ARDUINOMEMUSE
#ifdef ARDUINOMEMUSE
const uint16_t crc_table[] PROGMEM = {
#else
const uint16_t crc_table[] = {
#endif
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
};
uint16_t calc_crc(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size)
{
uint16_t i, j;
for (j = 0; j < data_blk_size; j++)
{
i = ((uint16_t)(crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF;
#ifdef ARDUINOMEMUSE
crc_accum = (crc_accum << 8) ^ pgm_read_word_near(crc_table + i);
#else
crc_accum = (crc_accum << 8) ^ crc_table[i];
#endif
}
return crc_accum;
}
void add_crc( uint8_t *p, int16_t l)
{
uint16_t crc;
crc = calc_crc(0, p, l);
putInt16t(crc, l);
}
boolean chk_crc(int16_t l)
{
uint16_t calc, inPkg;
inPkg = getInt16t(l - 2);
calc = calc_crc(0, txPkg, l - 2);
if (calc == inPkg)
return true;
else
return false;
}
void setHdrAndID( uint8_t id)
{
txPkg[0] = 0xff;
txPkg[1] = 0xff;
txPkg[2] = 0xfd;
txPkg[3] = 0x00;
txPkg[4] = id;
}
void dmpPkg( uint8_t *p, int16_t l)
{
for (int i = 0; i < l; i++) {
Serial.print((int)p[i], HEX); Serial.print(" ");
}
Serial.println("");
}
void transmitPkg(int16_t l)
{
for (int16_t i = 0; i < l; i++) {
// Serial.write(txPkg[i]); Serial/Serial1 or whatever
}
}
void test1()
{
uint16_t crc;
uint8_t cc;
/*
Ping Instruction Packet
0 1 2 3 4 5 6 7 8 9
H1 H2 H3 RSRV ID LEN1 LEN2 INST CRC1 CRC2
0xFF 0xFF 0xFD 0x00 0x01 0x03 0x00 0x01 0x19 0x4E
*/
setHdrAndID(0x01);
putInt16t(3,5); // lgt is at pos 5,6
putInt8t(0x01,7); // ping cmd at pos 7
add_crc(txPkg, 8); // add crc to pkg
// now we have a pkg og length 10 bytes (0..9)
//testing
Serial.println(calc_crc(0, txPkg, 8), HEX);
Serial.println("should be 0x4e19");
Serial.print("chksum in pkg added by add_crc is : ");
Serial.println( getInt16t(8), HEX); // crc is at pos 8,9
Serial.println("Lets see the raw pkg");
dmpPkg(txPkg, 10);
if ( chk_crc(10))
Serial.println("chk sum is ok in pkg");
else
Serial.println("chksum is bad");
putInt16t(4, 5);
Serial.println(getInt16t(5));
}
void setup() {
Serial.begin(9600);
test1();
}
void loop() {
// put your main code here, to run repeatedly:
}
#include
#define PLGT 30
unsigned char txPkg[PLGT];
void setL(unsigned char l)
{
txPkg[5] = l; // lsb int
txPkg[6] = 0; // msb int
}
void setHdr(unsigned char id)
{
txPkg[0] = 0xff;
txPkg[1] = 0xff;
txPkg[2] = 0xfd;
txPkg[3] = 0x00;
txPkg[4] = id;
}
void doPing(char motorID)
{
setHdr(motorID);
setL(3);
txPkg[7] = 0x01; // ping
add_crc16(txPkg,10); // will place crc16 @ loc 8,9
}
void dmpPkg(unsigned char *p, int l)
{
for (int i = 0; i < l; i++) {
Serial.print((int)p[i],HEX); Serial.print(" ");
}
Serial.println(" ende");
}
void setup() {
Serial.begin(9600);
doPing(0x01);
Serial.print((int)txPkg[8],HEX); Serial.print(" "); Serial.println((int)txPkg[9],HEX);
dmpPkg(txPkg,10);
}
void loop() {
// put your main code here, to run repeatedly:
}
//JDN oct 10 2018
// SEE: http://emanual.robotis.com/docs/en/dxl/protocol2/
// NB it you make pkgs longert than 30 then change below
// PROGRAM USES txPkg as package
// you chould change code a little so you have
// #define ARDUINO
#define PLGT 30
uint8_t txPkg[PLGT];
// PUT AND GET OFR 8/16/32 INTs
void putInt8t(int8_t v, int16_t pos)
{
txPkg[pos] = v;
}
int8_t getInt8t(int16_t pos)
{
return txPkg[pos];
}
void putInt16t(int16_t val, int16_t pos)
{
txPkg[pos] = ( uint8_t)(val & 0x00ff);
txPkg[pos + 1] = ( uint8_t)(val >> 8 );
}
int16_t getInt16t(int16_t pos)
{
int16_t v = 0;
v = txPkg[pos + 1];
v = v << 8;
v = v | txPkg[pos];
return v;
}
void putInt32t(int32_t val, int16_t pos)
{
for (int16_t i = 0; i < 4 ; i++) {
txPkg[pos + i] = ( uint8_t)(val & 0x000000ff);
val = val >> 8;
}
}
int32_t getInt32t(int16_t pos)
{
int32_t v = 0;
for (int16_t i = 0; i < 4 ; i++) {
v = v | (int32_t)txPkg[pos + i];
v = v << 8;
}
return v;
}
// CRC16 PART
// see http://emanual.robotis.com/docs/en/dxl/crc/
#define ARDUINOMEMUSE
#ifdef ARDUINOMEMUSE
const uint16_t crc_table[] PROGMEM = {
#else
const uint16_t crc_table[] = {
#endif
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
};
uint16_t calc_crc(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size)
{
uint16_t i, j;
for (j = 0; j < data_blk_size; j++)
{
i = ((uint16_t)(crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF;
#ifdef ARDUINOMEMUSE
crc_accum = (crc_accum << 8) ^ pgm_read_word_near(crc_table + i);
#else
crc_accum = (crc_accum << 8) ^ crc_table[i];
#endif
}
return crc_accum;
}
void add_crc( uint8_t *p, int16_t l)
{
uint16_t crc;
crc = calc_crc(0, p, l);
putInt16t(crc, l);
}
boolean chk_crc(int16_t l)
{
uint16_t calc, inPkg;
inPkg = getInt16t(l - 2);
calc = calc_crc(0, txPkg, l - 2);
if (calc == inPkg)
return true;
else
return false;
}
void setHdrAndID( uint8_t id)
{
txPkg[0] = 0xff;
txPkg[1] = 0xff;
txPkg[2] = 0xfd;
txPkg[3] = 0x00;
txPkg[4] = id;
}
void dmpPkg( uint8_t *p, int16_t l)
{
for (int i = 0; i < l; i++) {
Serial.print((int)p[i], HEX); Serial.print(" ");
}
Serial.println("");
}
void transmitPkg(int16_t l)
{
for (int16_t i = 0; i < l; i++) {
// Serial.write(txPkg[i]); Serial/Serial1 or whatever
}
}
void test1()
{
uint16_t crc;
uint8_t cc;
/*
Ping Instruction Packet
0 1 2 3 4 5 6 7 8 9
H1 H2 H3 RSRV ID LEN1 LEN2 INST CRC1 CRC2
0xFF 0xFF 0xFD 0x00 0x01 0x03 0x00 0x01 0x19 0x4E
*/
setHdrAndID(0x01);
putInt16t(3,5); // lgt is at pos 5,6
putInt8t(0x01,7); // ping cmd at pos 7
add_crc(txPkg, 8); // add crc to pkg
// now we have a pkg og length 10 bytes (0..9)
//testing
Serial.println(calc_crc(0, txPkg, 8), HEX);
Serial.println("should be 0x4e19");
Serial.print("chksum in pkg added by add_crc is : ");
Serial.println( getInt16t(8), HEX); // crc is at pos 8,9
Serial.println("Lets see the raw pkg");
dmpPkg(txPkg, 10);
if ( chk_crc(10))
Serial.println("chk sum is ok in pkg");
else
Serial.println("chksum is bad");
putInt16t(4, 5);
Serial.println(getInt16t(5));
}
void setup() {
Serial.begin(9600);
test1();
}
void loop() {
// put your main code here, to run repeatedly:
}
/*
*
*
*
*/
|
