Add for multiple joints

[alesof]

I have tested three zeroerr erob in daisy chain. I'll add the code briefly.

[alesof]

@bryangd34 would you be able to test it out on your zero err setup?

[bryangd34]

@alesof sure I should be able to take a look this week. Coincidentally, I have it working now too with 3 drives.