@cbenson @dialfonzo @cyberpalin
Sorry that trace is a little misleading here:
The trace is showing the trailing edge of the last part of the code that I call to make sure I have all of the servos found and configured:
void FindServos(void) {
g_count_servos_found = 0;
int32_t pos;
Serial.println("\nSearch for all servos");
// Initialize to no servos...
for (int i = 0; i < COUNT_SERVOS; i++) {
myLSS.setServoID(i);
pos = myLSS.getPosition();
if (myLSS.getLastCommStatus() == LSS_CommStatus_ReadSuccess) {
g_ids[g_count_servos_found++] = i;
Serial.print(" ");
Serial.print(i, DEC);
Serial.print(" - ");
Serial.println(pos, DEC);
}
}
// Now lets update the slots.
Serial1.printf("#254SLOTCOUNT%u\r", g_count_servos_found);
for (int i = 0; i < g_count_servos_found; i++) {
LSS_SERIAL.printf("#%uSLOT%d\r", g_ids[i], i);
LSS_SERIAL.printf("#%uG%u\r", g_ids[i], servo_gyre[g_ids[i]]);
}
Serial.println(" Done Slots and gyre updated");
}
The code that actually does the loop, with moving servos and query for voltage and temp:
void MoveAllServos(void) {
// first move all to center and on
AllServosCenter();
static int MIN_SERVO_POS = -300;
static int MAX_SERVO_POS = 300;
int servo_angle = 0;
int servo_increment = 50;
int voltages[COUNT_SERVOS];
int temps[COUNT_SERVOS];
uint32_t move_time = 250;
Serial.println("Move All servos: Enter any key to exit");
while (Serial.read() != -1);
while (!Serial.available()) {
elapsedMillis em = 0;
servo_angle += servo_increment;
if (servo_angle >= MAX_SERVO_POS) servo_increment = -50;
if (servo_angle <= MIN_SERVO_POS) servo_increment = 50;
#if WHEN_TO_CHEW == 2
// Lets ask for Voltage and Temp
Serial1.print("#254QV0QT0\r");
#endif
for (int j = 0; j < g_count_servos_found; j++) {
Serial1.printf("#%uD%dT%u", g_ids[j], servo_angle, move_time);
}
Serial1.print("\r"); // output terminator to execute the command.
char cmd_str[10];
uint8_t responses_remaining = g_count_servos_found * 2;
uint8_t servo_index = 0;
#if WHEN_TO_CHEW == 1
// Lets ask for Voltage and Temp
Serial1.print("#254QV0QT0\r");
#endif
#if WHEN_TO_CHEW > 0
while (responses_remaining && (em < 2 * move_time)) {
uint8_t servo_id;
int16_t servo_value = LSS_Read_Servo_s16(servo_id, cmd_str);
if (servo_id != g_ids[servo_index]) {
for (servo_index = 0; servo_index < g_count_servos_found; servo_index++) {
if (servo_id == g_ids[servo_index]) break;
}
}
if (servo_index < g_count_servos_found) {
if (strcmp(cmd_str, "QV") == 0) voltages[servo_index] = servo_value;
else if (strcmp(cmd_str, "QT") == 0) temps[servo_index] = servo_value;
responses_remaining--;
servo_index++;
if (servo_index == g_count_servos_found) servo_index = 0;
}
}
#endif
Serial.printf("servo_angle:%d QT: %u RR:%u\n", servo_angle, (uint32_t)em, responses_remaining);
while (em < (2 * move_time)) ;
}
}
Notice that the \r is only output after I write out all of the new positions.
But regardless: It should not matter if I tell each servo one at a time to start a move or tell all of the servos to start at once, I would believe that a goal would be that none of the messages should be garbled.
In the one I circled, my gut tells me there is a firmware issue. If you look at the overview trace, in most cases the blue traces (RX) look like it is responding one servo slot at a time, with both the QV and QT responses for that one servo… But in the circled area, the shorter blue one to the right of the circled only output the QV area and not the QT value. And then next thing corrupted.
My gut tells me that Servo started it’s response, and then became busy, maybe needing to compute and output the changes to the servo, and then it waited until next free time to start the rest of the response, but then the next servo also decided it was it’s time to output…
And I am seeing messages being corrupted or misinterpreted both on TX and RX pins…
RX mentioned above… TX - I am assuming TX error instead of servos on their own moving to wrong locations…
My next quick and dirty on this test case is to report how many of the responses to the commands
Appear to be wrong. Like: did not start with *, did not have a proper servo number, did not have one of the query command strings correct…
Note: the current library has very little support for this. That is you can ask for the next response to returned by calling something like:
static char * genericRead_Blocking_str(uint8_t id, const char * cmd);
But this code will fail if you pass in the wrong ID or the CMD is not what you passed in… So you have to know the exact order things will be returned and if your off, no real good way to know how to guess then what the next things you should pass in to try to get the next one…
Again note: the sketch is up on the github project. Will update at some time soon with changes I mentioned. Hopefully you should be able to replicate this with your own setup.
Kurt
EDIT: Should mention, that for each run, at the end I reset the servos and the like and scan for them and set up slots for each run, as to hopefully be able to fully recover for each pass through… Hope that makes sense.
)
