Hello everyone, I use the Raspberry Pi to processing something and obtained some datas which orginzed be an array. Then transfer to Arduino via I2C as a input to control three XM430-W350 motors. The problem now is that I can receive data correctly, but the motor can't move. And following is the code:
#include <Wire.h>
#include <Dynamixel2Arduino.h>
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560)
#include <SoftwareSerial.h>
SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX
#define DEBUG_SERIAL soft_serial
#elif defined(ARDUINO_SAM_DUE) || defined(ARDUINO_SAM_ZERO)
#define DEBUG_SERIAL SerialUSB
#else
#define DEBUG_SERIAL Serial
#endif
const uint8_t DXL_ID_1 = 12;
const uint8_t DXL_ID_2 = 13;
const uint8_t DXL_ID_3 = 14;
const float DXL_PROTOCOL_VERSION = 2.0;
double receivedData[101][3];
int dataIndex = 1;
double dataGroup[3];
int groupIndex = 0;
bool dataReady = false;
Dynamixel2Arduino dxl;
void displayAllData() {
Serial.println("Displaying all received data:");
for (int i = 0; i < 101; i++) {
Serial.print("Index ");
Serial.print(i);
Serial.print(": ");
for (int j = 0; j < 3; j++) {
Serial.print(receivedData[i][j]);
if (j < 2) {
Serial.print(", ");
}
}
Serial.println();
}
}
void smoothMove(float start_pos_1, float end_pos_1,
float start_pos_2, float end_pos_2,
float start_pos_3, float end_pos_3,
float step, uint16_t delay_time) {
float pos_1 = start_pos_1;
float pos_2 = start_pos_2;
float pos_3 = start_pos_3;
bool moving_up_1 = start_pos_1 < end_pos_1;
bool moving_up_2 = start_pos_2 < end_pos_2;
bool moving_up_3 = start_pos_3 < end_pos_3;
while ((moving_up_1 ? pos_1 <= end_pos_1 : pos_1 >= end_pos_1) ||
(moving_up_2 ? pos_2 <= end_pos_2 : pos_2 >= end_pos_2) ||
(moving_up_3 ? pos_3 <= end_pos_3 : pos_3 >= end_pos_3)) {
if (moving_up_1 ? pos_1 <= end_pos_1 : pos_1 >= end_pos_1) {
dxl.setGoalPosition(DXL_ID_1, pos_1, UNIT_DEGREE);
pos_1 += (moving_up_1 ? step : -step);
}
if (moving_up_2 ? pos_2 <= end_pos_2 : pos_2 >= end_pos_2) {
dxl.setGoalPosition(DXL_ID_2, pos_2, UNIT_DEGREE);
pos_2 += (moving_up_2 ? step : -step);
}
if (moving_up_3 ? pos_3 <= end_pos_3 : pos_3 >= end_pos_3) {
dxl.setGoalPosition(DXL_ID_3, pos_3, UNIT_DEGREE);
pos_3 += (moving_up_3 ? step : -step);
}
delay(delay_time);
DEBUG_SERIAL.print("Present Position(degree) for ID 1: ");
DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID_1, UNIT_DEGREE));
DEBUG_SERIAL.print("Present Position(degree) for ID 2: ");
DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID_2, UNIT_DEGREE));
DEBUG_SERIAL.print("Present Position(degree) for ID 3: ");
DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID_3, UNIT_DEGREE));
}
}
void receiveData(int byteCount) {
int byteIndex = 0;
byte commandByte = Wire.read();
while (Wire.available() > 1) {
byte highByte = Wire.read();
byte lowByte = Wire.read();
byteIndex += 2;
int value = (highByte << 8) | lowByte;
if (value > 32767) {
value -= 65536;
}
dataGroup[groupIndex++] = value;
if (groupIndex == 3) {
for (int i = 0; i < 3; i++) {
receivedData[dataIndex][i] = dataGroup[i] / 1000;
}
Serial.print("Stored in index ");
Serial.print(dataIndex);
Serial.print(": ");
for (int i = 0; i < 3; i++) {
Serial.print(receivedData[dataIndex][i]);
if (i < 2) {
Serial.print(", ");
}
}
Serial.println();
dataIndex++;
if (dataIndex >= 101) {
dataIndex = 1;
dataReady = true;
}
groupIndex = 0;
}
}
}
void setup() {
Serial.begin(19200);
Serial.println("Ready to receive I2C data...");
DEBUG_SERIAL.begin(115200);
dxl.begin(115200);
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
for (int id = DXL_ID_1; id <= DXL_ID_3; id++) {
dxl.ping(id);
dxl.torqueOff(id);
dxl.setOperatingMode(id, OP_POSITION);
dxl.torqueOn(id);
}
Wire.begin(8);
Wire.onReceive(receiveData);
}
void loop() {
if (groupIndex == 3) {
for (int i = 0; i < 3; i++) {
receivedData[dataIndex][i] = dataGroup[i];
}
Serial.print("Stored in index ");
Serial.print(dataIndex);
Serial.print(": ");
for (int i = 0; i < 3; i++) {
Serial.print(receivedData[dataIndex][i]);
if (i < 2) {
Serial.print(", ");
}
}
Serial.println();
if (dataReady) {
smoothMove(
dxl.getPresentPosition(DXL_ID_1, UNIT_DEGREE), receivedData[dataIndex][0],
dxl.getPresentPosition(DXL_ID_2, UNIT_DEGREE), receivedData[dataIndex][1],
dxl.getPresentPosition(DXL_ID_3, UNIT_DEGREE), receivedData[dataIndex][2],
1.0,
50
);
}
dataIndex++;
if (dataIndex >= 101) {
dataIndex = 1;
}
groupIndex = 0;
}
delay(1);
}