From cd5cdf049bb64add4c52a0d5734bec9a6d3c31dd Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Sat, 22 Sep 2018 08:31:11 +0200
Subject: [PATCH] Modified the slave example to simulate an RX28 Servo.
---
src/examples/test_dynamixel_slave.cpp | 17 +++++++++++++++++
1 file changed, 17 insertions(+)
diff --git a/src/examples/test_dynamixel_slave.cpp b/src/examples/test_dynamixel_slave.cpp
index 3360f36..7a275be 100644
--- a/src/examples/test_dynamixel_slave.cpp
+++ b/src/examples/test_dynamixel_slave.cpp
@@ -3,6 +3,16 @@
#include <iostream>
#include <boost/bind.hpp>
+#define SERVO_MEMORY_SIZE 50
+
+unsigned char rx28_servo_data[SERVO_MEMORY_SIZE]={0x1c,0x00,0x00,0x01,0x22,0xFA,0x00,0x00,
+ 0xFF,0x03,0x00,0x50,0x3C,0xBE,0xFF,0x03,
+ 0x02,0x24,0x24,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x01,0x01,0x20,0x20,0x00,0x00,
+ 0x00,0x00,0xFF,0x03,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x78,0x19,0x00,0x00,0x00,0x00,
+ 0x20,0x00};
+
std::string serial="A400gavm";
void on_ping(void)
@@ -12,7 +22,11 @@ void on_ping(void)
unsigned char on_read(unsigned short int address, unsigned short int length, unsigned char *data)
{
+ unsigned char num;
+
std::cout << "read operation at address " << address << " with length " << length << std::endl;
+ for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
+ data[num]=rx28_servo_data[i];
return 0x00;
}
@@ -21,6 +35,9 @@ unsigned char on_write(unsigned short int address, unsigned short int length, un
{
std::cout << "wrirte operation at address " << address << " with length " << length << std::endl;
+ for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
+ rx28_servo_data[i]=data[num];
+
return 0x00;
}
--
GitLab