Checkout this code. It was working fine, and then one day any value received that was above 512 was translated it as a very different (larger) integer. therefore i adjusted it to handle values up to 512, but i still need to understand why i got the error
#include <SoftwareSerial.h>
#include <DynamixelShield.h>
//This namespace is required to use Control table item names
using namespace ControlTableItem;
// Shield object
DynamixelShield dxl;
// MAX SPEED OF THE SERVOS
const int MAX_SPEED = 40; //20 for quiet
//4096 = 1x full turn
const int GOAL_THRESHOLD = 50;
// Position offset for Dynamixel 1
const long offset_1 = 2139;
// Position offset for Dynamixel 2
const long offset_2 = 1909;
// First Dynamixel (swivel)
const uint8_t DXL_ID1 = 1;
// Second Dynamixel (tilt)
const uint8_t DXL_ID2 = 2;
// Boilerplate
const float DXL_PROTOCOL_VERSION = 2.0;
long dmx_01_goal = 0;
long dmx_02_goal = 0;
// Get current Dynamixel position
int pos(int DXL_ID) {
return dxl.getPresentPosition(DXL_ID);
}
// Check if the Dynamixel has almost reached the goal
bool goal_reached(int DXL_ID, int goal) {
return (abs(pos(DXL_ID) - goal) < GOAL_THRESHOLD);
}
//============= END OF DYNAMIXEL SETUP ===================/
void dx_Init() {
// Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
dxl.begin(57600);
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
// Get DYNAMIXEL information
dxl.ping(DXL_ID1);
dxl.ping(DXL_ID2);
// DXL1 (turn off torque when configuring items in EEPROM area)
dxl.torqueOff(DXL_ID1);
dxl.setOperatingMode(DXL_ID1, OP_EXTENDED_POSITION);
// dxl.writeControlTableItem(MAX_POSITION_LIMIT, DXL_ID1, DXL_MAX); //1 for sexy
// dxl.writeControlTableItem(MIN_POSITION_LIMIT, DXL_ID1, DXL_MIN); //1 for sexy
dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID1, 1); //1 for sexy
dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID1, MAX_SPEED);
dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID1, 140); //14V
dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID1, 100); //0.25A
dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID1, 70); //13.74 RPM
dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID1, 0); //13.74 RPM
dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID1, 5);
dxl.torqueOn(DXL_ID1);
// DXL2 (turn off torque when configuring items in EEPROM area)
dxl.torqueOff(DXL_ID2);
dxl.setOperatingMode(DXL_ID2, OP_EXTENDED_POSITION);
// dxl.writeControlTableItem(MAX_POSITION_LIMIT, DXL_ID2, DXL_MAX); //1 for sexy
// dxl.writeControlTableItem(MIN_POSITION_LIMIT, DXL_ID2, DXL_MIN); //1 for sexy
dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID2, 1); //1 for sexy
dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID2, MAX_SPEED);
dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID2, 140); //14V
dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID2, 100); //0.25A
dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID2, 70); //13.74 RPM
dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID2, 0); //13.74 RPM
dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID2, 5);
dxl.torqueOn(DXL_ID2);
delay(100);
//---------------------------
// DYNAMIXEL SET TO 0 Position
//---------------------------
dxl.setGoalPosition(DXL_ID1, offset_1);
dxl.setGoalPosition(DXL_ID2, offset_2);
}
void move_dynamixel_1(uint32_t value) {
dxl.setGoalPosition(DXL_ID1, value + offset_1);
}
void move_dynamixel_2(uint32_t value) {
dxl.setGoalPosition(DXL_ID2, value + offset_2);
}
SoftwareSerial mySerial(9, 10); // RX, TX (TX not used)
void setup() {
mySerial.begin(600);
Serial.begin(9600);
dx_Init();
delay(2000);
// Serial.println(mySerial.available());
// Serial.println(“Hello UNO”);
}
void loop() {
static boolean receiving = false;
static byte incomingData[16]; // Large enough to handle escapes
static int dataIndex = 0;
bool escapeNext = false;
while (mySerial.available()) {
byte incomingByte = mySerial.read();
if (!receiving && incomingByte == 0x02) {
if (receiving && incomingByte == 0x03 && !escapeNext) {
processPacket(incomingData, dataIndex);
incomingByte ^= 0x20; // Unescape the byte
} else if (incomingByte == 0x7D) {
continue; // Skip further processing of this byte
if (dataIndex < sizeof(incomingData)) {
incomingData[dataIndex++] = incomingByte;
}
}
int reconstructInt(uint8_t a, uint8_t b, uint8_t c, uint8_t d)
{
return (a << 24) | (b << 16) | (c << 8) | d;
}
void processPacket(byte *data, int length) {
int a = reconstructInt(data[0], data[1], data[2], data[3]);
int b = reconstructInt(data[4], data[5], data[6], data[7]);
}