You're right, I should put more information, sorry, it's my first posts here!
Regarding the protocol, I do not know what you mean, so I can not answer you, I'm sorry.
The library that I use is the <ax12.h>, and the link is the following:
https://github.com/vanadiumlabs/arbo...Bioloid/ax12.h
This morning I tried to do the following:
Code:
ISR(PCINT1_vect) {
interrupt_state = (PINB & 0x1E) >> 1;
Serial.println("EMERGENCY BUTTON!");
TorqueOn(SERVO_ID[0]);
TorqueOn(SERVO_ID[1]);
TorqueOn(SERVO_ID[2]);
TorqueOn(SERVO_ID[3]);
int pos_em_1 = dxlGetPosition(SERVO_ID[0]); //Actual servo position
int pos_em_2 = dxlGetPosition(SERVO_ID[1]);
int pos_em_3 = dxlGetPosition(SERVO_ID[2]);
int pos_em_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;
go_position_EMERGENCY(pos_em_1, pos_em_1, pos_em_1, pos_em_1, pos2_1, pos2_2, pos2_3, pos2_4);//function
while(1);
}
I think that's what you told me to do, right? I register the positions when I give the button, and then send the robot to the new position (rest position), (go_position_EMERGENCY is a funciton that moves the robot to the rest position), here is the function:
Code:
//The following function will move the robot to the initial position and to the resting position when you have presed the EMERGENCY BUTTON!!
void go_position_EMERGENCY(int pos_em_1, int pos_em_2, int pos_em_3, int pos_em_4, int pos2_1, int pos2_2, int pos2_3, int pos2_4)
{
while (pos_em_1 != pos2_1 || pos_em_2 != pos2_2 || pos_em_3 != pos2_3 || pos_em_4 != pos2_4)
{
//ID1
if (pos2_1 < pos_em_1)
{
dxlSetGoalPosition(SERVO_ID[0], pos_em_1);
pos_em_1--;
delayMicroseconds(2000);
}
else if (pos2_1 > pos_em_1)
{
dxlSetGoalPosition(SERVO_ID[0], pos_em_1);
pos_em_1++;
delayMicroseconds(2000);
}
//ID2
if (pos2_2 < pos_em_2)
{
dxlSetGoalPosition(SERVO_ID[1], pos_em_2);
pos_em_2--;
delayMicroseconds(2000);
}
else if (pos2_2 > pos_em_2)
{
dxlSetGoalPosition(SERVO_ID[1], pos_em_2);
pos_em_2++;
delayMicroseconds(2000);
}
//ID3
if (pos2_3 < pos_em_3)
{
dxlSetGoalPosition(SERVO_ID[2], pos_em_3);
pos_em_3--;
delayMicroseconds(2000);
}
else if (pos2_3 > pos_em_3)
{
dxlSetGoalPosition(SERVO_ID[2], pos_em_3);
pos_em_3++;
delayMicroseconds(2000);
}
//ID4
if (pos2_4 < pos_em_4)
{
dxlSetGoalPosition(SERVO_ID[3], pos_em_4);
pos_em_4--;
delayMicroseconds(2000);
}
else if (pos2_4 > pos_em_4)
{
dxlSetGoalPosition(SERVO_ID[3], pos_em_4);
pos_em_4++;
delayMicroseconds(2000);
}
}
}
But the robot firstly moves very fast to other position (i don´t know why) and then it returns slowly to the rest position, so i don´t understand why the robot do that.
Also, in this program the Serial.println("EMERGENCY BUTTON!") didn´t print at all while i press the button... other time only is printing "EM"...
I don´t think that there is a power drain that happens which causes the voltage to drop too low and the board resets, or some bad call, that is resetting the program or, because after that the program continues running ok.
So, i don´t know what can be the problem...
Bookmarks