/********************************************************************************
Program to control Dynamixel of robotic arm by urko_18
********************************************************************************/
#include <ax12.h> //Include ArbotiX DYNAMIXEL library
int kop = 4; //Cuantity of dynamixel
int SERVO_ID[] = {1, 2, 3, 4}; // ID numbers
int regData1, regData2, regData3, regData4;
int turns = 0;
int turns2 = 0;
int turns3 = 0;
int turns4 = 0;
int l, i, repetitions;
double Speed;
/*****(r = 1; m = 420) (r = 2; m = 210) (r = 3; m = 140) (r = 4; m = 105)*****/ // maximun "m" for differents "r"
int m = 400; //columns
int r = 1; //rows
void setup()
{
dxlInit(1000000); //start dynamixel library at 1mbps to communicate with the servos
Serial.begin(9600); //start serial at 9600 for reporting data.
Relax(SERVO_ID[0]);
Relax(SERVO_ID[1]);
Relax(SERVO_ID[2]);
Relax(SERVO_ID[3]);
Serial.print("ID: ");
Serial.println(SERVO_ID[0]);
Serial.print ("ID: ");
Serial.println(SERVO_ID[1]);
Serial.print("ID: ");
Serial.println(SERVO_ID[2]);
Serial.print("ID: ");
Serial.println(SERVO_ID[3]);
delay(1000);
}
void loop()
{
int positionn1[r][m]; //Matrix of movements
int positionn2[r][m];
int positionn3[r][m];
int positionn4[r][m];
a:
Serial.println("How fast do you want to repeat the sequence?");
Serial.println(" 1 = Slow 2 = Normal 3 = Fast");
while (Serial.available() == 0) {}
Speed = Serial.parseInt();
while (Speed >=4){
Serial.println("You have entered a wrong value");
goto a;
}
while (Speed ==1){
Speed=1000;
}
while (Speed ==2){
Speed=700;
}
while (Speed ==3){
Speed=400;
}
Serial.print("Speed = ");
Serial.print(Speed, 0);
Serial.println(" microseconds");
delay(3000);
Serial.println("How many times do you want to repeat the sequence?");
while (Serial.available() == 0) {}
repetitions = Serial.parseInt();
Serial.print("repetitions = ");
Serial.println(repetitions);
delay(3000);
for (l = 0; l < r; l++) // structure to create rows
{
Serial.print("Positions vector ");
Serial.print(l + 1);
Serial.print(": [");
for (i = 0; i < m; i++) // structure to create columns
{
regData1 = dxlGetPosition(SERVO_ID[0]); //read and save the actual position
regData2 = dxlGetPosition(SERVO_ID[1]);
regData3 = dxlGetPosition(SERVO_ID[2]);
regData4 = dxlGetPosition(SERVO_ID[3]);
positionn1[l] = regData1; //Creation movements vector
positionn2[l] = regData2;
positionn3[l] = regData3;
positionn4[l] = regData4;
delay(20);
Serial.print(positionn1[l]); //Display the vector
Serial.print(", ");
Serial.print(positionn2[l]);
Serial.print(", ");
Serial.print(positionn3[l]);
Serial.print(", ");
Serial.print(positionn4[l]);
Serial.print("; ");
}
Serial.print("]\n");
if (l < (r - 1)) {
Serial.println("Next movement");
}
delay(2000);
}
delay(5000);
Serial.println("The servos will move to the initial position.");
/***The servos will move according to registered movements***/
for (int a = 0; a < repetitions; a++) //Repetition of the process (a = number of sequences)
{
Serial.print("SEQUENCE ");
Serial.println(a + 1);
int before_position_1, before_position_2, before_position_3, before_position_4;
int position1 = positionn1[0][0];
int position2 = positionn2[0][0];
int position3 = positionn3[0][0];
int position4 = positionn4[0][0];
for (l = 0; l < r; l++)
{
//First, the servos will move to initial position, in order to do the registered movements
int pos1_1 = dxlGetPosition(SERVO_ID[0]); //Actual servo position
int pos1_2 = dxlGetPosition(SERVO_ID[1]);
int pos1_3 = dxlGetPosition(SERVO_ID[2]);
int pos1_4 = dxlGetPosition(SERVO_ID[3]);
int pos2_1 = positionn1[l][0]; //Initial position of the movement (objective)
int pos2_2 = positionn2[l][0];
int pos2_3 = positionn3[l][0];
int pos2_4 = positionn4[l][0];
posiziora_joan(pos1_1, pos1_2, pos1_3, pos1_4, pos2_1, pos2_2, pos2_3, pos2_4); //Function that moves the robot to the initial position
if (l == 0) {
Serial.println("Now the servos will do the registered movements.");
}
delay(2000);
//Now, they will move through the registered positions
int initial1 = positionn1[l][0];
int initial2 = positionn2[l][0];
int initial3 = positionn3[l][0];
int initial4 = positionn4[l][0];
for (i = 0; i < m; i++)
{
//ID1
if (positionn1[l] < initial1)
{
while (positionn1[l] < initial1)
{
before_position_1 = initial1;
dxlSetGoalPosition(SERVO_ID[0], initial1);
initial1--;
delayMicroseconds(Speed);
if (initial1 != before_position_1)
{
if (initial1 == position1 + MX_MAX_POSITION_VALUE)
{
position1 = initial1;
turns++;
Serial.print("Turns engine 1: ");
Serial.println(turns);
}
else if (initial1 == position1 - MX_MAX_POSITION_VALUE)
{
position1 = initial1;
turns--;
Serial.print("Turns engine 1: ");
Serial.println(turns);
}
}
}
}
else if (positionn1[l] > initial1)
{
while (positionn1[l] > initial1)
{
before_position_1 = initial1;
dxlSetGoalPosition(SERVO_ID[0], initial1);
initial1++;
delayMicroseconds(Speed);
if (initial1 != before_position_1)
{
if (initial1 == position1 + MX_MAX_POSITION_VALUE)
{
position1 = initial1;
turns++;
Serial.print("Turns engine 1: ");
Serial.println(turns);
}
else if (initial1 == position1 - MX_MAX_POSITION_VALUE)
{
position1 = initial1;
turns--;
Serial.print("Turns engine 1: ");
Serial.println(turns);
}
}
}
}
####### THE SAME WITH THE OTHER MOTORS (ID2 ID3 ID4)#######
Serial.println(" ");
delay(1000); //Pause between movements
}
delay(300); //Pause between sequences
}
delay(7000);
/****REST POSITION****/
Serial.println("The robot will move to the resting position.");
int pos1_1 = dxlGetPosition(SERVO_ID[0]); //actual robot position
int pos1_2 = dxlGetPosition(SERVO_ID[1]);
int pos1_3 = dxlGetPosition(SERVO_ID[2]);
int pos1_4 = dxlGetPosition(SERVO_ID[3]);
int pos2_1 = 3820; //rest position of the robot (the positions of the 4 dynamixel)
int pos2_2 = 400;
int pos2_3 = 230;
int pos2_4 = 2395;
posiziora_joan(pos1_1, pos1_2, pos1_3, pos1_4, pos2_1, pos2_2, pos2_3, pos2_4);//function
delay(1000);
Relax(SERVO_ID[0]);
Relax(SERVO_ID[1]);
Relax(SERVO_ID[2]);
Relax(SERVO_ID[3]);
Serial.println("END!");