Driving 256 servos

Status
Not open for further replies.
O

orbitronics

Well-known member
This is a follow up from a previous post that asked questions on how to talk to 256 steppers.

I'm now deciding that servo's are much better for my application and am very keen on using a teensy 3.6 to control 256 servo's. Hence i need 256 GPIOs.

I know i can use 32 shift registers, which the 3.6 can accommodate - but will have issues given shift registers are digital and i need to drive the servos with a PWM signal.

The servos are the parallax standard servos. The datasheet says i need states 0.75–2.25 ms high pulses, 20 ms intervals.

Does anyone have any suggestions on what type of GPIO expander i can use? I have looked at ShiftPWM but not sure it is what i want.

EDIT: I have since seen this PWM expander

https://www.adafruit.com/product/815

It's designed for LEDs and I'm currently reading through the datasheet, but in the interim would love to know what you wise people think?
 
Last edited:
Wow, that's a *lot* of servo motors. Whatever you're building, I hope you share photos or video!

Probably the simplest solution would use these boards:

https://www.adafruit.com/product/815

If electronic hardware cost is your biggest concern, and you're up for some DIY building, maybe you could use 74HCT164 shift registers with the PulsePosition library.

https://www.pjrc.com/teensy/td_libs_PulsePosition.html

But even that only gets you half way there, since PulsePosition can have at most 8 outputs, each with 16 signals encoded. With some significant software work, maybe you could create a 2nd copy of PulsePosition which uses the FTM3 timer instead of FTM1, for 8 more outputs.

Using more than one Teensy might also be worth considering. Especially if the motors and other power supplies are distributed over some distance, running TTL-level signals over such long distances can be problematic. CAN bus or RS-485 serial are highly robust ways to communicate between controllers. That does add extra cost, but sometimes keeping groups of parts and wires into modular sections can end up saving more money in wires/connectors/hardware/maintenance than the cost of extra electronics.
 
Honestly Paul, i think that chip on the adafruit breakout is the best solution, i've been cross checking with the parallax motor for compatibility and i am under the impression that it'd work at a 60hz refresh rate. Please let me know if i'm overseeing something. The main requirement is that the motors all move at once (if i tell motor 5 and motor 200 to move, then need to perform the commands simultaneously, within the tolerances and constraints of the motor of course).

I'm doing the electronics for a surface that reflects ultrasonic signals - and you can be certain that i share the final thing with you - it's for an academic group that will showcase the thing at chi. Paul your software and hardware eco system is the best.
 
Another thought is the Pololu Maestro USB servers that can do USB, serial, or internal scripting. I've not used these, but I see them when I buy stuff from Pololu:

Another alternative, particularly if you are using multiple Teensys is get a Teensy/Feather adapter, and mount several of the feather servo shields stacked on the Teensy:

Note, with 16 servo controllers on an i2c bus, you probably need to think about physical restrictions (wire length, etc.) of the i2c layout. With some amount of modifying the controller library, I imagine you might be able to split that into 4 separate i2c buses on a Teensy 3.6 (or 3 buses on a 3.5 or 2 on a 3.2). At some point the i2c protocol overhead might be an issue.
 
It has been several years since I have done anything with RC servos.

Back then the Servo controller I have used in the past was the Lynxmotion one: https://www.robotshop.com/en/lynxmotion-ssc-32u-usb-servo-controller.html
Can be driven by TTL signals, or can plug into USB or...

One of the nice things of the old Lynxmotion board, is that you can also let the board do some position interpolation for you. That is you can say, I want to move these N servos from their current position to a new position and I want that move to take time X, and the controller will figure out how much each servo should move per time period, which I believe is the 20ms or 50 times per second...

I remember some other people used some servo controllers from Pololu: https://www.pololu.com/category/12/rc-servo-controllers If I remember correctly these controllers also had some ability to move some of the smarts to the controller.

I can imagine that there could be some timing issues, for example if you are trying to update all 256 servos 50 times a second, it would be interesting to see how fast you need to be able to drive the I2C buss... My guess is that the Adafruit board will take care of the servo refresh issue, so then you only need to tell servos a new pulse width when it changes... But again don't know your requirements here, on how often each servo will actually change positions.

It will also be interesting to see how you power all of these servos.

good luck, will be fun to follow your progress!
 
Thanks Kurt and Michael.

Please accept my apologies in advance for my cluttered response.

Cost isn't an issue.

I don't think i can realistically work with the PulsePosition library as i'm not a wizard programmer.

Right now the requirement is, if i want to move servo's 1 and 100, they'll need to move in unison (both begin moving with as little latency as possible).
At this point, the servos moving fast or responding super fast isn't much of a requirement - they'll connect to an arm that moves a surface up and down - so all i need is a bunch of servos to move in unison, to change a surface's shape. doing it super fast = not a requirement.

I see the usb maestro servers go up to 24 channels, but i don't know how realistic it is to get them to control 256 in unison.

With the power supply, i thought about using those server PSU's that can handle a lot of current, or derivative of that switching PSU. The servoes (in a worst case) require about 49 amps. I have thought that using a number of x Amp PSU's with a common ground should do the trick - would it not?

The control for this project (the electronics) and wiring will all be tightly packed together, IF i use the adafruit design, the I2C wires wouldn't be longer than the length of the boards stacked above / next to each other (as close as i can get them together). So i don't think that would pose much of an issue as i won't be having the controllers / servos in position A, then one a meter away etc etc.

I do wonder if the I2C bus might pose problems for 12 servo controllers.

I'm considering chaining 4 of adafruits carrier boards to each of the four i2c buses to decrease the latency. Or do you think a deriviative of this idea with multiple teensys would be better? If so, I'd imagine i need 1 master teensy?

I've had a serious look at the Lynxmotion controller - i thought about connecting it via serial, but noticed i'd need 8 serial ports for 256 servo's. i could do this but keeping them all timed together might make it easier to stick to the i2c solution.

The data stream that'll determine what the servo's actually do will be output from a computer (either usb, spi etc).

Taking all of this on board: 1 teensy 3.6 can talk to 4 16 channel servo controllers - using 1 i2c bus per 16 channels should keep that '60 hz refresh rate' as stated on adafruits site (please tell me if i'm wrong). Using all four buses, means one teensy 3.6 can control 64 servos at 60 hz (ish). Now i can have this setup x4, so four teensy 3.6's, each with 4 16 channel controllers. that means i can individually control my 256 servo's. To have each teensy send commands in unison (this is the iffy bit i'd appreciate feedback on), i can have 1 master teensy, that sends out a common 'latch' signal. The idea would be to have either an interrupt driven, or polled system on the slave teensys, that only send out data when a latch is high. The data to send to the motors could be determined (again, input here would be appreciated) by the master teensy (protocol yet undecided, i thought SPI but there's 3 spi ports on the teensy 3.6). The data to send out could alternatively be sent out to each teensy through one common spi port. At this point i'm a bit hazy. I know if i'm aiming for 60 hz refresh rate, the higher SPI data rates should suffice.

If i haven't completely lost you, then may i ask your thoughts?

EDIT: This might be much much easier, if i infact stick to the original comment above, and use 4 chained 16 channel controllers to each of the four I2C buses IF the latency isn't too great. I could alternatively throw the towel in and use a high GPIO count MCU or FPGA (FPGA will bust my balls).
 
Last edited:
Again there are probably lots of factors here that may influence your latency and the like, which again may influence what solution may work for you.

Taking a quick look at the Adafruit board. I think it is a pretty simple controller. It will simply generate pwm signals. That is for each channel you send it 4 bytes (2 2 byte values 0-4095). The time that the signal is high and the time the signal is low... So if you are changing all 16 channels and assuming you are running on T3.6 maybe 120mhz bus speed 60mhz, I believe that the Teensy can run I2C at about 938khz. I believe the Adafruit chip maybe can support up to 1mhz...

So to output 64 bytes would take some time, could calculate... Something like: (640)/938000 seconds? So maybe in the nature of .68ms probably closer to 1ms... Add in Addressing, delays...

So if you chained 4 of these controllers. you are talking about like 4ms per cycle... Or maybe 25 cycles per second...

BUT: you have the 4 different I2C Busses. Now if you use default Wire library, then this may not gain anything, as I believe the Wire.beginTransmission, Wire.write()... Wire.endTransmission() is
synchronous. That is the endTransmission will wait until your transfer completes.

So I assume you will need to use i2c_t3, which I believe has DMA support.

Again I don't know what your requirement is for example moving Servo 1 and 100 in unison. Are these two independent servos that as part of some move you wish to get some effect, or are these two servos that are used together as to double up on some joint to give extra strength? Solutions may be different depending... And again depending on need.

As for SSC-32u and needing 8 of them... Again may depend on your complete setup... That is if this is all controlled by some PC or the like, than not sure how well it would work to plug 8 of these into USB of the host? Never tried... Actually have not done much with the USB part, mostly used the earlier versions. With the Pololu they say that you can daisy chain several of these on the same Serial buss. Not sure how well? Also I believe I read that they are 5v so would be issue for T3.6...
 
KurtE said:
...
Click to expand...

Is individual pin PWM not really a done thing with an MCU via shift registers? I've seen an FPGA do it but not quite sure just yet why it's unpopular.

4 bytes per channel is workable, the PCA9685 from the adafruit board states it's a 'fast mode family plus' device, communicating up to 1 MHz. The teensy 3.6 should easily handle this, I think it should also handle it fine for the four buses in unison.

Up to 10ms refresh rate (time between sending 256 servo move commands to them being completed) will be great.

Moving two or more things in unison; what i mean is if i send a command to move some of these servos, then they should move pretty much when they're commanded to from a datastream - the idea is if the servo's are reflecting ultrasonic waves that are levitating something, and we change the reflecting surface (servo's) then the surface should change with minimal latency, so as to not disrupt the ultrasonic waves that are levitating said thing. If that makes any sense.

To make this clearer, the servo's are attached to some square surface that the servo moves up and down, by having a matrix of these surfaces we reflect an matrix of ultrasonic modules. Each servo independently moves about 8 grams of weight, it never increases, and the speed at which it moves isn't much of an issue.

I asked pololu what the refresh rate would be like if i daisy chained the maestro boards and their response was along the lines of 'yes they can be daisy chained', and given the simplicity of these adafruit boards, I think i'll try and make these part of the solution.

Speaking of adafruit, i did ask them here about their opinion on what we've been discussing, and it's got the attention of two mods. They've given a couple of interesting tidbits of information.
 
Sounds great.

As I mentioned, I think you can run Wire at the 1mhz rate more or less (depends on bus speed on the actual rate you can get but I believe should get close to that.

As for being able to drive all 4 I2C busses at the same time, this is more of a coding issue...
That is if you use the standard Wire library with code that looks something like:
Code: 
    Wire.beginTransmission(device1)
    for (i=0;i<64;i++) Wire.write(...);
    Wire.endTransmission();

    Wire1.begin(...
    ...
    Wire1.endTransmission()
Then only one buss at a time will be active as the code will hang in endTransmission until the data has been output.
However you can do this with DMA or interrupts... Probably using the i2c_t3 library (https://forum.pjrc.com/threads/21680-New-I2C-library-for-Teensy3)
 
It's easy to lose perspective when concentrating on timing specs.

Consider that most video uses 30 Hz refresh rate, and most movies are shot with 24 Hz frame rate. 60 Hz video is becoming more commonly used and it is considered to make a difference for watching fast action sports events, and of course it's pretty much a necessity competitive first person shooter video games where fast reaction is a matter of virtual life & death!

RC servo motors aren't that fast. Nearly all of them are meant to be updated at 20 Hz, where even 20 times per second is overkill when you consider their actual mechanical speed.

Sure, you can put a tremendous amount of work into achieving 100 Hz update rate (10 ms) to all these motors. But what is the point?
 
However, looking at Adafruit's code, it looks like each motor requires 6 bytes of I2C communication. With stop, ack and stop bits, at the default 100 kHz I2C speed that works out to be about 0.6 ms per motor.

That'll be too slow for 256 motors! Even at 400 kHz I2C, still too slow. :(

1 Mbit speed probably isn't viable for so many boards and lengthy wires.

You could use i2c_t3 to get 4 I2C ports running simultaneously. That would mean rewriting Adafruit's library, or creating your own code to talk to their boards. Perhaps that's not as bad as it sounds, since that library code is fairly simple.

Another option using more hardware but less software work (no edits to Adafruit's lib) could look like using Teensy LC boards to drive small groups sets of servo motors, with serial communication from Teensy 3.6 to the Teensy LC boards. As long as you send messages that fit into the serial buffers, you can write several messages and all will transmit simultaneously.

Still, there's a point of diminishing returns. Obviously you don't want to take 0.15 seconds to update all 256 motors, but a tremendous amount of work to get the all-motor update to 0.01 seconds isn't worthwhile when the factor in the actual mechanical capability of the motors.
 
And then there is the notion that if you make it modular, it can be easier to replace parts and debug things. I can just imagine the fun of trying to track down a broken solder connection or wire on 256 servos.
 
Last edited:
All good points.

I've done some testing and with adafruits library, the only bit i'm getting a bit caught up on is setting the right pulse values for SERVOMIN and SERVOMAX, and then making a function that basically sets them accordingly so that one could just use a function to set the servo to any angle.

I had been getting very poor results, but i set the Wire rate to 1 MHz and it sorted things out well, there's some slowdown to the RPM if i daisy chain 4x16 channels, but it's still acceptable.

I have daisy chained 8x16 channel boards, in the fast video I am just sending commands to sweep the 10 servo's. In the slow video, I am sending commands to sweep all 256 servos (only physically have 10 hooked up).

slow

fast

It was as slow as it is in the slow video when i issued commands to 128 servos with 4 boards daisy chained, the change to 1 MHz sorted that out.

There's something that's majorly slowing this down though - on the adafruit forum i linked where i'm asking similar questions, Bill did mention a big slow down might be coming from the wait for ACK. I'm not quite sure and I'm thinking of busting out my Saleae logic analyser.

I think it was always the way to use i2c_t3, and the case of simultaneously driving all four I2C ports was a question i wanted to ask. Is there anything i should take into consideration when porting the code from Wire to I2C_t3 to allow simultaneous I2C bus communication? I have already ported over to i2c_t3 but nothing else other than 'Wire.h' to 'i2c_t3.h', and 'TwoWire' objects to 'i2c_t3'. Looking deeper at i2c_t3, is it 'Wire.sendTransmission' that i want? So pretty much remove begin and end transmission and instead use 'Wire.sendTransmission', 'Wire1.sendTransmission' .. to .. 'Wire3.sendTransmission'? Lastly, I can't tell if using the DMA operational mode will improve things, and if it's a matter of setting I2C_OP_MODE_DMA.

The multiple hardware option was always on the table, but I'll try and go for using all four I2C ports given four daisychained boards driving 64 servo's rotates at an acceptable RPM. I still need to figure out why more servo's on the chain = slower RPM, and not same RPM but latency between issuing command to servo 0 and servo n (they currently move in unison, as seen in the video).

In the final thing i will most definitely keep this modular, debugging something like this would be like trying to replace a bust LED inside a 8x8x8 LED cube! Teensy With the adafruit boards is probably the way, I may make a motherboard so they're fully modular, with only the wires for the servo's, datastream (SPI or USB) and power.

with serial communication from Teensy 3.6 to the Teensy LC boards. As long as you send messages that fit into the serial buffers, you can write several messages and all will transmit simultaneously.
Click to expand...
Is this 8 bits?
 
Last edited:
orbitronics said:
In the final thing i will most definitely keep this modular, debugging something like this would be like trying to replace a bust LED inside a 8x8x8 LED cube! Teensy With the adafruit boards is probably the way, I may make a motherboard so they're fully modular, with only the wires for the servo's, datastream (SPI or USB) and power.

with serial communication from Teensy 3.6 to the Teensy LC boards. As long as you send messages that fit into the serial buffers, you can write several messages and all will transmit simultaneously.
Click to expand...

Is this 8 bits?
Click to expand...
Each serial line transmits 8 bits(1 byte) at a time in a sequential fashion. You can't transmit less than 8 bits at a time.

Quoting from https://www.pjrc.com/teensy/td_uart.html:
  • On Teensy 3.2, 3.5, 3.6, Serial1 and Serial2 have 8 byte transmit and receive FIFOs, which allow for higher speed baud rates, even when other libraries create interrupt latency.
  • All serial ports on all Teensy boards use interrupt-based transmit and receive buffering, to allow your program to avoid waiting when writing short messages, and to allow data to be reliably received even if your program must spend time performing other tasks.
  • On Teensy 3.6 when running faster than 120 MHz, writing to EEPROM may momentarily interfere with Serial1 and Serial2 baud rates. Reading EEPROM does not interfere. Serial3 to Serial6 are not affected.
Click to expand...
 
It is hard to give complete answers without knowing really what it is you have changed... Or see the actual code.

Wire vs i2c_t3: Yes I do believe that you would use Wire.sendTransmission call instead of endTransmission(), to start up a non-blocking call... See the example sketch: advanced_master.ino

Is this the way to solve it? Good question, it may depend on how organized your calling code is. That is can you set up your code to start output to each of the 4 different wire busses and know and keep them busy (if appropriate?) Something like have lists of servos to update by which Wire object and then check to see if a wire bus is free and then output next update...

Again if you are using the Adafruit PWMServoDriver library for this, looks like again it is only updating one servo per call, wire transmission... i.e. this function:
Code: 
void Adafruit_PWMServoDriver::setPWM(uint8_t num, uint16_t on, uint16_t off) {
#ifdef ENABLE_DEBUG_OUTPUT
  Serial.print("Setting PWM "); Serial.print(num); Serial.print(": "); Serial.print(on); Serial.print("->"); Serial.println(off);
#endif

  _i2c->beginTransmission(_i2caddr);
  _i2c->write(LED0_ON_L+4*num);
  _i2c->write(on);
  _i2c->write(on>>8);
  _i2c->write(off);
  _i2c->write(off>>8);
  _i2c->endTransmission();
}
So again only update one servo with 6 bytes transferred (i2c address byte, Address of first register to update, 4 bytes of register values).
Which may be fine if you are only updating a small subset of your servos...

But again if you are updating all 16 servos, than might be better to setup to write all servos at once. Which would only require the 2 byte overhead plus 64 bytes...

As for using Serial objects instead... Yes minimal output is 8 bit outputs...

But I think the idea for non blocking using SerialX objects is. if you do something like Serial1.write(buffer, length) and the length is greater than how many bytes that will fit in the output queue, the write statement will wait until there is enough space to fit the data before it returns.

To potentially get around this, you can do stuff like, call off to Serial1.availableForWrite(), which returns you a count of how many bytes you can now write to the queue before it would block...
That way can setup your code output a portion of your output buffer to the queue as room is available...
 
So I've set all the ON values to 0 once, that's removed 2 of the 6 original byte transfers.

Now if my understanding of sendTransmission is correct, I'm trying to use it to send commands to each slave, then before repeating that loop, wait for the transmission (and servo movement) to finish.


Here's an example of using sendTransmission to two slaves on two buses, to set 1 angle, it works well:
Code: 
    long temp = degrees_to_duty_cycle(180);
    Wire.beginTransmission(0x40);
    Wire.write(8 + (4 * 0));
    Wire.write(temp);
    Wire.write(temp >> 8 );
    Wire.sendTransmission();
    Wire1.beginTransmission(0x40);
    Wire1.write(8 + (4 * 0));
    Wire1.write(temp);
    Wire1.write(temp >> 8 );
    Wire1.sendTransmission();
    Wire.finish();
    Wire1.finish();

Here's an example of working sweep using sendTransmission for each slave, set to only work on the first slave of each bus exclusively:
Code: 
    for (uint8_t angle = 0; angle < 180; angle++ ) {
    long temp = degrees_to_duty_cycle(angle);
    for (uint8_t i = 0; i < 16; i++ ) {
      for (uint8_t j = 0; j < 1; j++ ) {
        Wire.beginTransmission(0x40 + j);
        Wire.write(8 + (4 * i));
        Wire.write(temp);
        Wire.write(temp >> 8 );
        Wire.sendTransmission();
        Wire1.beginTransmission(0x40 + j);
        Wire1.write(8 + (4 * i));
        Wire1.write(temp);
        Wire1.write(temp >> 8 );
        Wire1.sendTransmission();
      }
      Wire.finish();
      Wire1.finish();
    }
    delay(10);
  }

When i change the value of j to above 1, nothing moves - no doubt an issue in the way i'm using sendTransmission

Changing the previous code to put the finish's inside the third nested loop makes things work:
Code: 
  for (uint8_t angle = 0; angle < 180; angle++ ) {
    long temp = degrees_to_duty_cycle(angle);
    for (uint8_t i = 0; i < 16; i++ ) {
      for (uint8_t j = 0; j < 2; j++ ) {
        Wire.beginTransmission(0x40 + j);
        Wire.write(8 + (4 * i));
        Wire.write(temp);
        Wire.write(temp >> 8 );
        Wire.sendTransmission();
        Wire1.beginTransmission(0x40 + j);
        Wire1.write(8 + (4 * i));
        Wire1.write(temp);
        Wire1.write(temp >> 8 );
        Wire1.sendTransmission();
        Wire.finish();
        Wire1.finish();
      }
    }
    delay(10);
  }

Any idea on how i might better utilise sendTransmission ?

(on a seperate note, It may be linked to the previous issue: I've done the second code example by using sendTransmission for each slave then waiting to finish, instead of reordering the loops, so I'm actually sendTransmission for each channel on a single device then wait, as I'm assuming sendTransmission doesn't use a buffer, and I'd just be overwriting the previous commands if i try to use sendTransmission on the same bus, before the previous sendTransmission has finished, example I am not doing this because i don't think it'll work (swapping i and j)):

Code: 
  for (uint8_t angle = 0; angle < 180; angle++ ) {
    long temp = degrees_to_duty_cycle(angle);
    for (uint8_t i = 0; i < 4; i++ ) {
      for (uint8_t j = 0; j < 16; j++ ) {
        Wire.beginTransmission(0x40 + j);
        Wire.write(8 + (4 * i));
        Wire.write(temp);
        Wire.write(temp >> 8 );
        Wire.sendTransmission();
        Wire1.beginTransmission(0x40 + j);
        Wire1.write(8 + (4 * i));
        Wire1.write(temp);
        Wire1.write(temp >> 8 );
        Wire1.sendTransmission();
      }
      Wire.finish();
      Wire1.finish();
      delay(500);
    }
  }
)
 
Last edited:
It is not surprising to me that you can only talk to one conversation (which also implies one device) at at a time per I2C transmission...

Other things you can try: as I mentioned in previous answer. If you are updating most/all of the servos each time, than having your output function output all 64 bytes per transmission may improve things.
 
KurtE said:
It is not surprising to me that you can only talk to one conversation (which also implies one device) at at a time per I2C transmission...

Other things you can try: as I mentioned in previous answer. If you are updating most/all of the servos each time, than having your output function output all 64 bytes per transmission may improve things.
Click to expand...

I tried something like that (if i understand what you mean) by writing:

startTransmission
write to all LEDOFF registers
endTRansmission

issue is, there's always two bytes of registers (LEDON) between the set of high and low LEDOFF registers, i could just write 2 bytes of 0s, not sure if it'd negate the optimisation of doing everything in a clean sweep as you suggest.
 
Well it seams like it would be easy to try it out and see?

That is setup your 16 values for one of the servo controllers. Try looping to do the 16 values as individual calls and try it with one call writing all in one write.

Again something like
Code: 
    // Single outputs
    uint32_t time_start = millis();
    for (uint8_t j = 0; j < 16; j++ ) {
        Wire.beginTransmission(0x40);    //NOT SURE OF ADDRESS HERE You say J, but that is inner loop?
        Wire.write(8 + (4 * j));
        Wire.write(temp);
        Wire.write(temp >> 8 );
        Wire.sendTransmission();
        Wire.finish();
    }
    Serial.println(millis()-time_start);

    // try for one output
    uint32_t time_start = millis();
    Wire.beginTransmission(0x40);
    Wire.write(8);
    for (uint8_t j = 0; j < 16; j++ ) {
        Wire.write(temp);
        Wire.write(temp >> 8 );
        Wire.write(0);
        Wire.write(0);
        Wire.sendTransmission();
        Wire.finish();
    }
    Serial.println(millis()-time_start);
Again not sure of this code on what your indexes are for which servo and which board and... But hopefully you get the idea.

As for writing two extra bytes 0's My guess is the later will be faster as yes writing two extra 0s, but NOT writing two extra bytes (board address and byte number start), which you output once

Obviously if the above works, than you can incorporate with multiple I2C objects doing the writes at the same time... But again this assume all servos are updated. If you are doing smaller subsets? than what code would work best may change. Example if you hold onto the current values for each of the servos and only update those who actually change and IF that is a small subset of the total servos than your solution might be different.
 
Thanks a lot for your help and advice, I'm planning to have the datastream come from a powerful computer that would output two byte packets, [0] = servo number, [1] = pulse value.

I think i should keep the previous values local to the teensy so i can simply iterate over all 256 servos, i know that leaves some redundancy when sending a byte for the servo address (number), but at least i could setup one for loop that iterates and writes only new values, with say a second task (in some RTOS) fetching and storing new values / buffering.

Regarding your code examples, I used adafruits PWM writes, the four byte writes (only writing off positions, using sendTransmission and finish, so i can write multiple buses at a time then wait), and i tried the solution you proposed that writes all appropriate values t o the 16 channels (four bytes per channel).

i used elapsedMicros instead since the millis was always around 1.

The final example ('try for one output') hangs the i2c bus.

Anyway here's the stuff:

Code: 
  // Single outputs
  elapsedMicros time_start;
  for (uint8_t j = 0; j < 16; j++ ) {
    Wire.beginTransmission(0x40);    //NOT SURE OF ADDRESS HERE You say J, but that is inner loop?
    Wire.write(8 + (4 * j));
    Wire.write(440);
    Wire.write(440 >> 8 );
    Wire.sendTransmission();
    Wire1.beginTransmission(0x40);    //NOT SURE OF ADDRESS HERE You say J, but that is inner loop?
    Wire1.write(8 + (4 * j));
    Wire1.write(440);
    Wire1.write(440 >> 8 );
    Wire1.sendTransmission();
    Wire1.finish();
    Wire1.finish();
  }
  Serial.print("Writing 2 at a time: ");
  Serial.println(time_start);
  delay(1000);    // wait for the servo to move
  digitalWrite(13, HIGH);

  // adafruit lib outputs
  time_start = 0;
  for (uint8_t j = 0; j < 16; j++ ) {
    pwm_A_1.setPWM(j, 0, 250);
    pwm_B_1.setPWM(j, 0, 250);
  }
  Serial.print("Adafruit library: ");
  Serial.println(time_start);
  delay(1000);

  // try for one output
  time_start = 0;
  Wire.beginTransmission(0x40);
  Wire.write(6);
  for (uint8_t j = 0; j < 16; j++ ) {
    Wire.write(0);
    Wire.write(0);
    Wire.write(85);
    Wire.write(85);
    Wire.sendTransmission();
    Wire.finish();
  }
  Serial.print("Single output all write: ");
  Serial.println(time_start);
  Serial.println();
  delay(1000);

The last doesn't actually do anything, though it doesn't hang the i2c bus.

I tried changing the last one to something i would have thought would be the correct version:

Code: 
  Wire.beginTransmission(0x40);
  Wire.write(6);
  for (uint8_t j = 0; j < 16; j++ ) {
    Wire.write(0);
    Wire.write(0);
    Wire.write(85);
    Wire.write(85);
  }
  Wire.sendTransmission();
  Wire.finish();

This still does nothing:

Anyway I setup the final one (non working) to equal the first two to get a proper benchmark:

Code: 
  Wire.beginTransmission(0x40);
  Wire.write(6);
  Wire1.beginTransmission(0x40);
  Wire1.write(6);
  for (uint8_t j = 0; j < 16; j++ ) {
    Wire.write(0);
    Wire.write(0);
    Wire.write(85);
    Wire.write(85);
    Wire1.write(0);
    Wire1.write(0);
    Wire1.write(85);
    Wire1.write(85);
  }
  Wire.sendTransmission();
  Wire1.sendTransmission();
  Wire.finish();
  Wire1.finish();

And the benchmarks are:

Code: 
Writing 2 at a time: 1073
Adafruit library: 2765
Single output all write: 877

In micro seconds, it seems like a change in ~0.2ms to write all at a time (and figure out why it's not working now) is maybe not worth it?

EDIT: For fun i decided to benchmark the 'first' solution, but using endTransmission instead of sendTrans:

Code: 
  for (uint8_t j = 0; j < 16; j++ ) {
    Wire.beginTransmission(0x40);    //NOT SURE OF ADDRESS HERE You say J, but that is inner loop?
    Wire.write(8 + (4 * j));
    Wire.write(440);
    Wire.write(440 >> 8 );
    Wire.endTransmission();
    Wire1.beginTransmission(0x40);    //NOT SURE OF ADDRESS HERE You say J, but that is inner loop?
    Wire1.write(8 + (4 * j));
    Wire1.write(440);
    Wire1.write(440 >> 8 );
    Wire1.endTransmission();
  }

It performs at 1937 us, that's almost x2 longer than using sendTransmission.

EDIT 2:

The final optimisation that i've not sat down and played with yet is the following that was suggested by adafruit:

The key is the OCH bit of the MODE2 register. See Table 6 in datasheet:
https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf
In the default mode "Outputs change on STOP command". The STOP command is part of the I2C comms. Unfortunately, the way the current driver works, this feature isn't supported, since it does not do repeated starts:
https://github.com/adafruit/Adafruit-PW ... r.cpp#L124
and will send a STOP command after updating each channel.
Click to expand...

I'm currently reading through the datasheet and trying to figure out how updating on an ack instead of a stop command would change things, and how i'd need to code things differently.
 
Last edited:
As for if the .2ms is worth it or not... is obviously totally up to you....

As for why things are not working? With code like:
Code: 
Wire.beginTransmission(0x40);
  Wire.write(6);
  for (uint8_t j = 0; j < 16; j++ ) {
    Wire.write(0);
    Wire.write(0);
    Wire.write(85);
    Wire.write(85);
  }
  Wire.sendTransmission();
  Wire.finish();
First I am assuming the code parts included some of the init stuff like setting the pwm rates... Like in their example app
Code: 
 pwm.begin();
  
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates

But in your example, outputting a low time of 0x8585 is probably bad data data... From my quick looking through the code, I think the valid ranges
are closer to 500us pulse (0x7a 0x00), 1500us (0x70, 0x1), and 2500 (0x66 0x2)

My guess is your 0x8585 might be taken as 0x585? which I believe would probably try to generate a pulse width of something like 5750us, which I don't believe any of the servos will like...
 
I don't know where you got those hex values from, i calculated (and by observation) noted 85 is 0 degrees, and 445 is 180 degrees, but noticing each servo stops at slightly different angles (maybe +- 5 degrees) with no load. I know these numbers are not really exactly what you'd calculate if you go off what the datasheet says for the duty cycle of 0.75 ms to 2.25 ms, but if i set that (something like 150 to a number higher than 450) then i started getting about 150 degrees per full rotation, and the gears were audibly grinding. It's really annoying it doesn't match the datasheet, but 85 to 445 seem to be very close to 180 degrees from 0 degrees, and there are no grinding gears.

I do use the adafruit library to set things up, here's the complete code:

Code: 
/***************************************************************
   LIBRARIES
*/

#include <Adafruit_PWMServoDriver.h>
#include <i2c_t3.h>
#include <LiquidCrystal_I2C.h>
#include <Encoder.h>
#include <Bounce.h>
#include <EEPROM.h>

/***************************************************************
    DEFINES
*/
#define DEFAULT_HEIGHT  150 * 4
#define CALIBRATE_BUTTON 2
#define SERVO_UP_BUT 32
#define SERVO_DOWN_BUT 31
#define SERVO_SET_BUT 30

#define u8 uint8_t
#define u16 uint16_t

/***************************************************************
   GLOBALS
*/
u16 SERVOMIN = 85;
u16 SERVOMAX[256]; // around the max value, calibrate to actual value

Adafruit_PWMServoDriver pwm_A_1 = Adafruit_PWMServoDriver(&Wire, 0x40);
Adafruit_PWMServoDriver pwm_A_2 = Adafruit_PWMServoDriver(&Wire, 0x41);
Adafruit_PWMServoDriver pwm_A_3 = Adafruit_PWMServoDriver(&Wire, 0x42);
Adafruit_PWMServoDriver pwm_A_4 = Adafruit_PWMServoDriver(&Wire, 0x43);
Adafruit_PWMServoDriver pwm_B_1 = Adafruit_PWMServoDriver(&Wire1, 0x40);
Adafruit_PWMServoDriver pwm_B_2 = Adafruit_PWMServoDriver(&Wire1, 0x41);
Adafruit_PWMServoDriver pwm_B_3 = Adafruit_PWMServoDriver(&Wire1, 0x42);
Adafruit_PWMServoDriver pwm_B_4 = Adafruit_PWMServoDriver(&Wire1  , 0x43);
Adafruit_PWMServoDriver* servo_A[4] = {&pwm_A_1, &pwm_A_2, &pwm_A_3, &pwm_A_4};
Adafruit_PWMServoDriver* servo_B[4] = {&pwm_B_1, &pwm_B_2, &pwm_B_3, &pwm_B_4};


/***************************************************************
    SETUP
*/
void setup() {
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);
  for (u16 i = 0; i < 16; i++) {
    u16 eeprom = 200;// + (u16)EEPROM.read(i);
    if (eeprom > 445)
      SERVOMAX[i] = 445;
    if (eeprom < 400)
      SERVOMAX[i] = 445;
  }
  Serial.begin(115200);

  pwm_A_1.begin();
  delay(15);
  pwm_A_2.begin();
  delay(15);
  pwm_B_1.begin();
  delay(15);
  pwm_B_2.begin();
  delay(15);

  reset_servos();

  delay(500);


/***************************************************************
    FUNCTIONS
*/
/**
   Reset the servos to the zeroth position
*/
void reset_servos(void) {

  for (u8 i = 0; i < 2; i++ ) {
    for (u8 j = 0; j < 16; j++ ) {
      Wire.beginTransmission(0x40 + i);
      Wire.write(6 + (4 * j));
      Wire.write(85);
      Wire.write(85);
      Wire.write(85);
      Wire.write(85);
      Wire.sendTransmission();
      Wire1.beginTransmission(0x40 + i);
      Wire1.write(6 + (4 * j));
      Wire1.write(85);
      Wire1.write(85);
      Wire1.write(85);
      Wire1.write(85);
      Wire1.sendTransmission();
      Wire.finish();
      Wire1.finish();
    }
  }
  delay(100);
  Serial.println("Reset!");
}

/**
   Enter number in degrees, outputs duty cycle
*/
long degrees_to_duty_cycle(u8 degree, u8 servo) {

  return map(degree, 0, 180, SERVOMIN, SERVOMAX[servo]);
}

/***************************************************************
   LOOP
*/

void loop() {

  // Single outputs
  elapsedMicros time_start;
  // using i2c_t3 to write simultanously to different i2c buses on a teensy
  for (u8 i = 0; i < 2; i++ ) {
    for (u8 j = 0; j < 16; j++ ) {
      Wire.beginTransmission(0x40 + i);
      Wire.write(8 + (4 * j));
      Wire.write(445);
      Wire.write(445 >> 8 );
      Wire.sendTransmission();
      Wire1.beginTransmission(0x40 + i);
      Wire1.write(8 + (4 * j));
      Wire1.write(445);
      Wire1.write(445 >> 8 );
      Wire1.sendTransmission();
      Wire1.finish();
      Wire1.finish();
    }
  }
  Serial.print("Writing 2 at a time: ");
  Serial.println(time_start);
  delay(1000);    // wait for the servo to move

  // adafruit lib outputs
  time_start = 0;
  for (u8 j = 0; j < 16; j++ ) {
    pwm_A_1.setPWM(j, 0, 85);
    pwm_B_1.setPWM(j, 0, 85);
    pwm_A_2.setPWM(j, 0, 85);
    pwm_B_2.setPWM(j, 0, 85);
  }
  Serial.print("Adafruit library: ");
  Serial.println(time_start);
  Serial.println();
  delay(1000);
}

I made some modifications to the adafruit library but only to one function, in addition to going from Wire.h to i2c_t3.h, i change 'begin' to:

Code: 
/**************************************************************************/
/*! 
    @brief  Setups the I2C interface and hardware
*/
/**************************************************************************/
void Adafruit_PWMServoDriver::begin(void) {
  _i2c->begin();
  _i2c->setOpMode(I2C_OP_MODE_DMA);
  _i2c->resetBus();

  _i2c->setClock(1000000);

  reset();
  // set a default frequency
  setPWMFreq(50);
}
 
Last edited:
As for the numbers I showed, the 0x8585 was from your code, although I did get it wrong:
Wire.write(85);
Wire.write(85);
So the actual value: given is 85*256=85=21845 = 0x5555 I believe only the low 3 bits is use of high byte: so this is 0x555 or 1365...

As for pulse width:
If you go to the libraries example program servo.ino you see:
Code: 
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;
  
  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= 60;   // 60 Hz
  Serial.print(pulselength); Serial.println(" us per period"); 
  pulselength /= 4096;  // 12 bits of resolution
  Serial.print(pulselength); Serial.println(" us per bit"); 
  pulse *= 1000000;  // convert to us
  pulse /= pulselength;
  Serial.println(pulse);
  pwm.setPWM(n, 0, pulse);
}

Again I was assuming 60hz as per their code...
So if you pass in .0015 as the pulse width which internally multiply by 1000000 = 1500
and you divide this by pulselength= (1000000/60)/4096 = 4.0690104166666666666666666666667
so 1500/4.06... = 368 then you need to convert to bytes...

Now with 50hz instead of 60, I would suspect a value of about 307 (same calculations substitute 50 for 60...)

As for +-5 degrees. A lot of the may depend on what servos you are using? How much slop...

If you are doing 50 hz and the range of valid values is 0-4095, than a change of 1 in the output will change the pulse width by about 5us (4.8...)
But lets assume that your servos have a valid range of output of 500(-90 degrees) 1500(0) 2500(90 degrees), I would suspect that the valid data to
pass as pulse widths is: 102 - 512 So about a difference of 410 so 180 degrees/410=.439 degrees per unit...

But this really depends on your servos. Also not all are linear... So yes you may need to use your own observations to do the valid conversions. Also if you have different brands/model numbers, the conversion may be different for each of them.

Warning it has probably been 5 years since I did much of anything with RC servos!
 
orbitronics said:
Honestly Paul, i think that chip on the adafruit breakout is the best solution, i've been cross checking with the parallax motor for compatibility and i am under the impression that it'd work at a 60hz refresh rate. Please let me know if i'm overseeing something. The main requirement is that the motors all move at once (if i tell motor 5 and motor 200 to move, then need to perform the commands simultaneously, within the tolerances and constraints of the motor of course).

I'm doing the electronics for a surface that reflects ultrasonic signals - and you can be certain that i share the final thing with you - it's for an academic group that will showcase the thing at chi. Paul your software and hardware eco system is the best.
Click to expand...

Is this like a Keck telescope mirror system ? If so VErY cool project. !
 
orbitronics, How many of those servos could you control off a single teensy 3.6 given it's capable of low latency controlling them? (not using adafruit board)
 
Status
Not open for further replies.
Back
Top