Issue: If i comment other motor and run the specific motor in the code, the shaft rotates but if I try to run all three motor at the same time even as daisy chain then it wont work in spite of different motor id as well. Any suggestion or help would be appreciated. I am giving power from 12 V ac-dc adaptor 6A current. Model - HOUHUI-1206
PROVIDE A THOUROUGH DESCRIPTION OF YOUR ISSUE, THE MORE DETAIL YOU PROVIDE THE EASIER IT WILL BE FOR THE COMMUNITY TO PROVIDE YOU ASSISTANCE
DYNAMIXEL Servo: MX 28T
DYNAMIXEL Controller: Openrb 150
Software Interface: Arduino ide`
code :-
`#include <math.h>
#include <Dynamixel2Arduino.h>
#if defined(ARDUINO_OpenRB) // When using OpenRB-150
//OpenRB does not require the DIR control pin.
#define DXL_SERIAL Serial1
#define DEBUG_SERIAL Serial
const int DXL_DIR_PIN = -1;
#endif
const uint8_t DXL_ID1 = 1;
const uint8_t DXL_ID2 = 2;
const uint8_t DXL_ID3 = 3;
const float DXL_PROTOCOL_VERSION = 1.0;
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); // Make sure DXL_SERIAL and DXL_DIR_PIN are correctly defined
//This namespace is required to use Control table item names
using namespace ControlTableItem;
//Providing input variables of angles as radians
float Alpha_0 = 10 * (PI / 180);
float Alpha_1 = radians(30);
float Alpha_2 = radians(30);
float Theta_1 = radians(50);
float Theta_2 = radians(45);
float Theta_3 = radians(30);
float r_4 = 1;
float theta1,theta2,theta3;
// Define the terms of the transformation matrix
double S_x, S_y, S_z, n_x, n_y, n_z, a_x, a_y, a_z, p_x, p_y, p_z;
void setup() {
// Use UART port of DYNAMIXEL Shield to debug.
DEBUG_SERIAL.begin(115200);
while(!DEBUG_SERIAL);
// 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
// Set up for each motor
dxl.ping(DXL_ID1);
dxl.ping(DXL_ID2);
dxl.ping(DXL_ID3);
dxl.torqueOff(DXL_ID1);
dxl.torqueOff(DXL_ID2);
dxl.torqueOff(DXL_ID3);
dxl.setOperatingMode(DXL_ID1, OP_POSITION);
dxl.setOperatingMode(DXL_ID2, OP_POSITION);
dxl.setOperatingMode(DXL_ID3, OP_POSITION);
dxl.torqueOn(DXL_ID1);
dxl.torqueOn(DXL_ID2);
dxl.torqueOn(DXL_ID3);
dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID1, 30);
dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID2, 30);
dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID3, 30);
Serial.print("Alpha_0 (radians): ");
Serial.println(Alpha_0);
Serial.print("Alpha_1 (radians): ");
Serial.println(Alpha_1);
S_x = ((((((cos(Theta_1) * cos(Theta_2)) - sin(Theta_1)) * cos(Alpha_1)) * sin(Theta_2)) * cos(Theta_3)) -
(((((((cos(Theta_1) * sin(Theta_2)) + sin(Theta_1)) * cos(Alpha_1)) * cos(Theta_2))) * cos(Alpha_1)) * sin(Theta_3)) +
(((sin(Alpha_1) * sin(Theta_1)) * sin(Alpha_2)) * sin(Theta_3)));
S_y = (((cos(Alpha_0) * sin(Theta_1) * cos(Theta_2)) + (cos(Alpha_0) * cos(Theta_1) * cos(Alpha_1) * sin(Theta_2)) -
(sin(Alpha_0) * sin(Alpha_1) * sin(Theta_2))) * cos(Theta_3) +
((-cos(Alpha_0) * sin(Theta_1) * sin(Theta_2)) + (cos(Alpha_0) * cos(Theta_1) * cos(Alpha_1) * cos(Theta_2)) -
(sin(Alpha_0) * sin(Alpha_1) * cos(Theta_2)) * cos(Alpha_2) * sin(Theta_3) -
(cos(Alpha_0) * cos(Theta_1) * sin(Alpha_1) + sin(Alpha_0) * cos(Alpha_1)) * sin(Alpha_2) * sin(Theta_3)));
S_z = ((sin(Alpha_0) * sin(Theta_1) * cos(Theta_2) + sin(Alpha_0) * cos(Theta_1) * cos(Alpha_1) * sin(Theta_2) +
cos(Alpha_0) * sin(Alpha_1) * sin(Theta_2)) * cos(Theta_3) +
(-sin(Alpha_0) * sin(Theta_1) * sin(Theta_2) + sin(Alpha_0) * cos(Theta_1) * cos(Alpha_1) * cos(Theta_2) +
cos(Alpha_0) * sin(Alpha_1) * cos(Theta_2)) * cos(Alpha_2) * sin(Theta_3) + ((-sin(Alpha_0) * cos(Theta_1) * sin(Alpha_1) + cos(Alpha_0) * cos(Alpha_1)) * sin(Alpha_2) * sin(Theta_3)));
n_x = (-(cos(Theta_1) * cos(Theta_2) - sin(Theta_1) * cos(Alpha_1) * sin(Theta_2)) * sin(Theta_3) -
(cos(Theta_1) * sin(Theta_2) + sin(Theta_1) * cos(Alpha_1) * cos(Theta_2)) * cos(Theta_2) * cos(Theta_3) +
sin(Alpha_1) * sin(Theta_1) * sin(Alpha_2) * cos(Theta_3));
n_y = (-(cos(Alpha_0) * sin(Theta_1) * cos(Theta_2) + cos(Alpha_0) * cos(Theta_1) * cos(Alpha_1) * sin(Theta_2) -
sin(Alpha_0) * sin(Alpha_1) * sin(Theta_2)) * sin(Theta_3) +
(-cos(Alpha_0) * sin(Theta_1) * sin(Theta_2) + cos(Alpha_0) * cos(Theta_1) * cos(Alpha_1) * cos(Theta_2) -
sin(Alpha_0) * sin(Alpha_1) * cos(Theta_2)) * cos(Theta_2) * cos(Theta_3) -
(cos(Alpha_0) * cos(Theta_1) * sin(Alpha_1) + sin(Alpha_0) * cos(Alpha_1)) * sin(Alpha_2) * cos(Theta_3));
n_z = (-(sin(Alpha_0) * sin(Theta_1) * cos(Theta_2) + sin(Alpha_0) * cos(Theta_1) * cos(Alpha_1) * sin(Theta_2) +
cos(Alpha_0) * sin(Alpha_1) * sin(Theta_2)) * sin(Theta_3) +
(-sin(Alpha_0) * sin(Theta_1) * sin(Theta_2) + sin(Alpha_0) * cos(Theta_1) * cos(Alpha_1) * cos(Theta_2) +
cos(Alpha_0) * sin(Alpha_1) * cos(Theta_2)) * cos(Alpha_2) * cos(Theta_3) +
(-sin(Alpha_0) * cos(Theta_1) * sin(Alpha_1) + cos(Alpha_0) * cos(Alpha_1)) * sin(Alpha_2) * cos(Theta_3));
a_x = ((cos(Theta_1) * sin(Theta_2) + sin(Theta_1) * cos(Alpha_1) * cos(Theta_2)) * sin(Alpha_2) +
sin(Alpha_1) * sin(Theta_1) * cos(Alpha_2));
a_y = ((-cos(Alpha_0) * sin(Theta_1) * sin(Theta_2) + cos(Alpha_0) * cos(Theta_1) * cos(Alpha_1) * cos(Theta_2) -
sin(Alpha_0) * sin(Alpha_1) * cos(Theta_2)) * sin(Alpha_2) -
(cos(Alpha_0) * cos(Theta_1) * sin(Alpha_1) + sin(Alpha_0) * cos(Alpha_1)) * cos(Alpha_2));
a_z = -(-sin(Alpha_0) * sin(Theta_1) * sin(Theta_2) + sin(Alpha_0) * cos(Theta_1) * cos(Alpha_1) * cos(Theta_2) -
cos(Alpha_0) * sin(Alpha_1) * cos(Theta_2)) * sin(Alpha_2) +
(-sin(Alpha_0) * cos(Theta_1) * sin(Alpha_1) + cos(Alpha_0) * cos(Alpha_1)) * cos(Alpha_2);
p_x = (cos(Theta_1) * sin(Theta_2) + sin(Theta_1) * cos(Alpha_1) * cos(Theta_2) * r_4 * sin(Alpha_2) +
sin(Alpha_1) * sin(Theta_1) * r_4 * cos(Alpha_2));
p_y = (-(-cos(Alpha_0) * sin(Theta_1) * sin(Theta_2) + cos(Alpha_0) * cos(Theta_1) * cos(Alpha_1) * cos(Theta_2) +
sin(Alpha_0) * sin(Alpha_1) * cos(Theta_2)) * r_4 * sin(Alpha_2) -
(cos(Alpha_0) * cos(Theta_1) * sin(Alpha_1) + sin(Alpha_0) * cos(Alpha_1)) * r_4 * cos(Alpha_2));
p_z = (-(-sin(Alpha_0) * sin(Theta_1) * sin(Theta_2) + sin(Alpha_0) * cos(Theta_1) * cos(Alpha_1) * cos(Theta_2) +
cos(Alpha_0) * sin(Alpha_1) * cos(Theta_2)) * r_4 * sin(Alpha_2) -
(sin(Alpha_0) * cos(Theta_1) * sin(Alpha_1) + cos(Alpha_0) * cos(Alpha_1)) * r_4 * cos(Alpha_2));
Serial.println(“The transformation matrix is :”);
float T_0_4[4][4] = {
{S_x, n_x, a_x, p_x},
{S_y, n_y, a_y, p_y},
{S_z, n_z, a_z, p_z},
{0, 0, 0, 1}
};
// Print the transformation matrix R_0_4
Serial.println(“The Transformation matrix T_0_4 is :”);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
Serial.print(T_0_4[i][j], 6);
if (j < 2) Serial.print(", ");
}
Serial.println();
}
Serial.print("S_x: "); Serial.println(S_x);
Serial.print("S_y: "); Serial.println(S_y);
Serial.print("S_z: "); Serial.println(S_z);
Serial.print("n_x: "); Serial.println(n_x);
Serial.print("n_y: "); Serial.println(n_y);
Serial.print("n_z: "); Serial.println(n_z);
Serial.print("a_x: "); Serial.println(a_x);
Serial.print("a_y: "); Serial.println(a_y);
Serial.print("a_z: "); Serial.println(a_z);
Serial.print("p_x: "); Serial.println(p_x);
Serial.print("p_y: "); Serial.println(p_y);
Serial.print("p_z: "); Serial.println(p_z);
//calculate delta
float delta = acos(a_z);
Serial.print(“delta:”); Serial.println(delta, 6);
//Calculate phi
float phi = atan2((S_z/ sin(delta)) , (n_z/ sin(delta)));
Serial.print(“phi :”); Serial.println(phi, 6);
//calculate gamma
float gamma = atan2((a_x/ sin(delta)), (-a_y/ sin(delta)));
Serial.print(“gamma :”); Serial.println(gamma, 6);
// Calculate the rotation matrix R_0_4
float R_0_4[3][3] = {
{cos(gamma) * cos(phi) - sin(gamma) * cos(delta) * sin(phi),
-cos(gamma) * sin(phi) - sin(gamma) * cos(delta) * cos(phi),
sin(delta) * sin(gamma)},
{sin(gamma) * sin(phi) + cos(gamma) * cos(delta) * cos(phi),
-sin(gamma) * cos(phi) + cos(gamma) * cos(delta) * sin(phi),
-sin(delta) * cos(gamma)},
{sin(delta) * sin(phi),
sin(delta) * cos(phi),
cos(delta)}
};
// Print the rotation matrix R_0_4
Serial.println("The Rotation matrix R_0_4 is :");
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
Serial.print(R_0_4[i][j], 6);
if (j < 2) Serial.print(", ");
}
Serial.println();
}
// calculating Inverse Kinematics
// Now we calculate for Theta2
theta2 = acos((cos(Alpha_0)*cos(delta) + sin(Alpha_0)*cos(gamma)*sin(delta) - cos(Alpha_1)*cos(Alpha_2))/(-sin(Alpha_1)*sin(Alpha_2)));
// Serial.print("The value of theta2 is: ");
// Serial.println(theta2);
theta2 = theta2 * (180 / PI);
Serial.print("The value of theta2 is: ");
Serial.println(theta2);
// Now we calculate for Theta1
float w = -pow((cos(Alpha_1)*sin(Alpha_2)*cos(theta2) + sin(Alpha_1)*cos(Alpha_2)), 2) - pow(sin(Alpha_2)*sin(theta2), 2);
float cos_theta1 = (cos(Alpha_1)*sin(Alpha_2)*cos(theta2) + sin(Alpha_1)*sin(Alpha_2))*(sin(Alpha_0)*cos(delta) - cos(Alpha_0)*cos(gamma)*sin(delta)) - sin(Alpha_2)*sin(theta2)*(sin(gamma)*sin(delta)) / w;
float sin_theta1 = -(cos(Alpha_1)*sin(Alpha_2)*cos(theta2) + sin(Alpha_1)*sin(Alpha_2))*(sin(gamma)*sin(delta)) - sin(Alpha_2)*sin(theta2)*(sin(Alpha_0)*cos(delta) - cos(Alpha_0)*cos(gamma)*sin(delta)) / w;
theta1 = atan2(sin_theta1, cos_theta1);
theta1 = theta1 * (180 / PI);
Serial.print("The value of theta1 is: ");
Serial.println(theta1);
// Now we calculate Theta3
float X = (-sin(Alpha_0)*sin(theta1)*sin(theta2) + sin(Alpha_0)*cos(theta1)*cos(Alpha_1)*cos(theta2) + cos(Alpha_0)*sin(Alpha_1)*cos(theta2))*cos(Alpha_2) + (-sin(Alpha_0)*cos(theta1)*sin(Alpha_1) + cos(Alpha_0)*cos(Alpha_1))*sin(Alpha_2);
float Y = sin(Alpha_0)*sin(theta1)*cos(theta2) + sin(Alpha_0)*cos(theta1)*cos(Alpha_1)*sin(theta2) + cos(Alpha_0)*sin(Alpha_1)*sin(theta2);
float cos_theta3 = (cos(phi)*X + sin(phi)*Y)/sin(delta);
float sin_theta3 = (sin(phi)*X - cos(phi)*Y)/sin(delta);
theta3 = atan2(sin_theta3, cos_theta3);
theta3 = theta3 * (180 / PI);
Serial.print("The value of theta3 is: ");
Serial.println(theta3);
double s[3] = {delta, phi, gamma};
double s2[3] = {-delta, phi + PI, gamma + PI};
Serial.println(s[3]);
Serial.println(s2[3]);
}
void loop() {
// Set the goal positions for each motor
dxl.setGoalPosition(DXL_ID1, theta1, UNIT_DEGREE); // Motor 1
dxl.setGoalPosition(DXL_ID2, theta2, UNIT_DEGREE); // Motor 2
dxl.setGoalPosition(DXL_ID3, theta3, UNIT_DEGREE); // Motor 3
// Monitor positions for each motor
// int present_position1 = 0, int present_position2 = 0, int present_position3 = 0;
float present_position1 = 0.0, present_position2 = 0.0, present_position3 = 0.0;
while ((abs(theta1 - present_position1) > 2.0) ||
(abs(theta2 - present_position2) > 2.0) ||
(abs(theta3 - present_position3) > 2.0))
{
// present_position1 = dxl.getPresentPosition(DXL_ID1, UNIT_DEGREE);
present_position2 = dxl.getPresentPosition(DXL_ID2, UNIT_DEGREE);
present_position3 = dxl.getPresentPosition(DXL_ID3, UNIT_DEGREE);
DEBUG_SERIAL.print("Motor 1 Position(degree): ");
DEBUG_SERIAL.println(present_position1);
DEBUG_SERIAL.print("Motor 2 Position(degree): ");
DEBUG_SERIAL.println(present_position2);
DEBUG_SERIAL.print("Motor 3 Position(degree): ");
DEBUG_SERIAL.println(present_position3);
delay(100);
}
delay(1000);
}