Optimized Servotor32 for faster servo position update
The impact of the below listed changes to current PoCoMo-controlled Hexy is minimal. You will most probably appreciate this information if you are trying to transfer more control into Servotor32 and you are trying to squeeze some CPU cycles.
I found that when I’m communicating with Servotor32 with my binary protocol, the serial link buffer can get overloaded (and Servotor32 halted) if I send more than about 20 packets per second (20 x 19 servos). 20 FPS is definitely sufficient speed because of mechanical servo speed limit (they cannot move that fast). I personally use 10 FPS. However, I found that the buffer overload is caused by inefficient way of updating the servo position registers one-by-one. This is due to the reason that the original (ASCII) communication sends position update for each servo separately, while my binary communication send 19 servo positions at once.
So before the Servotor32 code looked something like:
if (binary) {
switch(inChar){
case '\r':
case '\n':
binary = false;
changeServo(5,inAllPos[0]);
changeServo(6,inAllPos[1]);
changeServo(7,inAllPos[2]);
changeServo(9,inAllPos[3]);
changeServo(10,inAllPos[4]);
changeServo(11,inAllPos[5]);
changeServo(13,inAllPos[6]);
changeServo(14,inAllPos[7]);
changeServo(15,inAllPos[8]);
changeServo(16,inAllPos[9]);
changeServo(17,inAllPos[10]);
changeServo(18,inAllPos[11]);
changeServo(20,inAllPos[12]);
changeServo(21,inAllPos[13]);
changeServo(22,inAllPos[14]);
changeServo(24,inAllPos[15]);
changeServo(25,inAllPos[16]);
changeServo(26,inAllPos[17]);
changeServo(31,inAllPos[18]);
break;
default:
inAllPos[numCount] = byte(inChar)*10;
numCount++;
break;
}
}
else {
switch(inChar){
...
which called changeServo() function 19 times. changeServo() calls update_registers_fast() which, for a given servo, removes the servo from a sorted list and adds it back based on the new servo position. And then updates shift registers. 19 times for 19 servos.
Hence I made a new function update_all_registers_fast() which build this sorted list for all servos only once and then updates the shift registers:
void Servotor32::update_all_registers_fast(){
while(update_reg_flag == 0){ // wait for the servos to stop pulsing before updating the timing arrays
delayMicroseconds(10);
}
// ----- delete all ------
for(byte group=0; group<GROUPS; group++){
servos_active_in_group[group] = 0;
active_servos_hex[group] = 0;
for(byte i=0; i<SERVOS_PER_GROUP; i++){
servos_sorted[group][i] = -1;
}
}
// ----- add all active servos ------
for(byte i=0; i<SERVOS; i++){
if (servo_positions[i] != -1) {
servos_sorted[i/SERVOS_PER_GROUP][servos_active_in_group[i/SERVOS_PER_GROUP]] = i; //index of active servo
servos_active_in_group[i/SERVOS_PER_GROUP] += 1;
active_servos_hex[i/SERVOS_PER_GROUP] |= pin_2_num[i%SERVOS_PER_GROUP];
}
}
// ----- bubble sort servos_sorted ------
// sort the array by servos position, the ones with pos = -1 will stay at the end
boolean sorted = false;
byte j = 0;
short temp = 0;
for(byte group=0; group<GROUPS; group++){ // for each group separately
sorted = false;
j = 0;
while (!sorted){ // continue sorting the array until is sorted.
sorted = true; // expecting the the array sorted
for (byte i=0; i<servos_active_in_group[group]-1-j; i++){ // go through the active servo list (they are at the front)
if (servo_positions[servos_sorted[group][i]] > servo_positions[servos_sorted[group][i+1]]){ //if wrong order of two consecutive, swap them
temp = servos_sorted[group][i];
servos_sorted[group][i] = servos_sorted[group][i+1];
servos_sorted[group][i+1] = temp;
sorted = false; // no, it was not sorted yet
}
}
j++;
}
}
// ----- create timing idicies from servo/group data ------- (no change from here on)
// clear the timing arrays for fresh start
for(uint8_t i=0; i<MAX_TIMINGS; i++){
servo_timings[i] = 0;
shift_output[i] = 0xFF;
shift_latch[i] = 0xFF;
}
uint8_t counter_index=0;
uint8_t current_timing=0;
uint8_t current_shift_output=0;
for(byte group=0; group<GROUPS; group++){ //go through each group
if(servos_active_in_group[group] > 0){ // skip it if the group is active, otherwise:
servo_timings[counter_index] = group_offsets[group];
shift_output[counter_index] = active_servos_hex[group];
shift_latch[counter_index] = (1<<group_latches[group]);
counter_index +=1;
//create additional timings
for(byte i=0; i<servos_active_in_group[group]; i++){ //create the timings for each servo after that, using the previous output
if(servo_positions[servos_sorted[group][i]] == servo_positions[servos_sorted[group][i-1]]){ // if this servo's time is the same as the last's
if(i != 0){
counter_index -= 1; //reverse the index count-up
}
else{
current_shift_output = shift_output[counter_index-1];
servo_timings[counter_index] = servo_positions[servos_sorted[group][i]]+ group_offsets[group];
shift_latch[counter_index] = (1<<group_latches[group]);
}
}
else{
current_shift_output = shift_output[counter_index-1];
servo_timings[counter_index] = servo_positions[servos_sorted[group][i]]+ group_offsets[group];
shift_latch[counter_index] = (1<<group_latches[group]);
}
//subtract the current servo from the shift register output
current_shift_output &= ~pin_2_num[servos_sorted[group][i]-group*SERVOS_PER_GROUP];
shift_output[counter_index] = current_shift_output;
counter_index +=1;
}
}
}
}
And the processChar() function changed to:
// short inAllPos[19]; - removed
uint8_t servosInPacket[] = {5,6,7, 9,10,11, 13,14,15, 16,17,18, 20,21,22, 24,25,26, 31};
void Servotor32::processChar(char inChar){
if (binary) {
switch(inChar){
case '\r':
case '\n':
binary = false;
update_all_registers_fast();
break;
default:
inPos = byte(inChar);
if ((inPos >= 50) && (inPos <= 250)){
servo_positions[servosInPacket[numCount]] = inPos;
}
if (inPos == 255){
servo_positions[servosInPacket[numCount]] = -1;
}
numCount++;
break;
}
}
else {
switch(inChar){
...
The original function update_registers_fast() stays in the code. My function update_all_registers_fast() is called only when binary packet is received. This way backward compatibility is preserved.
Now, updating all 19 servos is about 10x faster (more efficient) than it was. The impact of these changes is that Servotor32 does not get halted when you send binary packets at high rate.
As I mentioned at the beginning, this function might be also helpful to you if you are working on autonomous Hexy and therefore you want to update more servos at once as well (with or without serial communication).
If you don’t know how to import can you tell me how to fix your PoMoCo? I’m not him. 
