I am attempting to insert the `print6Axis();` function within the servo algorithms. However, I am uncertain about where I should place the command within the servo function, as I am not familiar with the servo function responsible for moving the head or legs.
For instance, let's consider the function called `servomove`. In pseudo-code, the function might resemble the following:
servomove loop{
targethead.setPWN(70); //Example command. I assume this is how it works, right? if printgyro == 1: // When it received 'V' from a user.
print6Axis(); // Add the function to prevent it from halt when servo moves
}I'm trying to insert the `print6Axis();` function in the servo function so that way, it wouldn't stop servo function from printing gyro.
Do you have any idea where I can find it?
The servos are controlled by calibratedPWM(). In every loop() of gait, only one servo is controlled. If you want the gyro to update after all the servos are controlled, you can add it in skill.h->perform() in the condition:
if (frame >= abs(period)) { frame = 0; print6Axis(); }If you only control a few joints independently, you can add it in reaction.h:
if ((token == T_INDEXED_SEQUENTIAL_ASC || token == T_INDEXED_SIMULTANEOUS_ASC) && (nonHeadJointQ || lastToken != T_SKILL)) { // printToAllPorts(token); transform(targetFrame, 1, transformSpeed); skill->convertTargetToPosture(targetFrame); print6Axis(); }In general, you can look for transform() or calibratedPWM() to insert the print6Axis() function.