I've found the cause of my servo twitching, but...

Hey guys,

My current project has lights, sounds, and motion. The lights are leds on a shift register, and they’re updated occasionally in my main loop. The sounds are produced by a DAC, which I use hardware SPI to to write to at a samplerate of 30khz. And the motion is produced by a couple of servos, one of which has been twitching quite a bit.

I’ve narrowed the twitching problem down to the fact that my sound update interrupt is delaying the servo update interrupt ever so slightly. But I am at a loss for how to solve the problem.

I tried adding ISR_NOBLOCK to my DAC interrupt, but while that stopped the servos from twitching, the sound went crazy and the sketch crashed, presumably because a backlog of sound updates was occuring.

I was hoping to solve this problem on the servo side, so I took a look at the servo lib and tried to understand what it was doing.

It seems that what the servo lib does is set up a timer which updates once every 8 system clocks, and when that timer matches the compare reigster, it calls the servo ISR.

Meanwhile, the servo ISR sets the pin of a particular servo high, and then sets the compare register to tell the timer how long it should wait before triggering the ISR again, and when the ISR is next triggered, that servo’s pin is set low, and the pin of the next servo is set high, and the compare resister is set to a new value that tells the timer how long that servo’s pin must be high.

This means that the servo ISR isn’t really being called that often. It’s only being called once for each servo, every 20ms. And I only have two servos. So that’s one update every 10ms roughly.

So trying to optimize the servo ISR isn’t likely to help. The real problem it seems is that timer2, which the DAC is on, seems to be delaying timer1. I’m at a loss for how to fix this though.

Setting timer1 to NOBLOCK allows timer2 to interrupt it so it gets called at the proper time, but then timer1 continues executing and this seems to cause enough of a delay that a backlog then occurs.

Is there perhaps some way then that if timer1 needs to interrupt timer2 that timer2’s call could simply be aborted, thus skipping that update completely? Losing a sample here for the sound probably isn’t going to be a major issue since the sample I’m playing is noisy and I’m playing it on a piezo. I’m not going for high fidelity sound. But I do need the servos to stop twitching like crazy.

So, any ideas?

Here’s the code for the DAC update interrupt. I can optimize it a bit with direct port access I imagine, but I’m not sure that I won’t need to do anything else in the function so I’d like to solve the problem some other way than making the DAC update a little faster.

// This is called at x Hz to load the next sample.
ISR(TIMER2_COMPA_vect) { //, ISR_NOBLOCK) {

    static int bufferIndex = 0;
    unsigned int sample;
    byte dataHigh, dataLow;

    //static unsigned long lastToggle = 0; 
    //static int count = -1;
   

        
    /*
    if (sample >= sounddata_length) {
        if (sample == sounddata_length + lastSample) {
            stopPlayback();
        }
        else {
            // Ramp down to zero to reduce the click at the end of playback.
            OCR2A = sounddata_length + lastSample - sample;
        }
    }
    else {
        OCR2A = pgm_read_byte(&sounddata_data[sample]);
    }

    ++sample;
    */

        
    /*

      DAC data format:

      bit 15 A/B: DACA or DACB Select bit
        1 = Write to DACB
        0 = Write to DACA

      bit 14 BUF: VREF Input Buffer Control bit
        1 = Buffered
        0 = Unbuffered

      bit 13 GA: Output Gain Select bit
        1 = 1x (VOUT = VREF * D/4096)
        0 = 2x (VOUT = 2 * VREF * D/4096)

      bit 12 SHDN: Output Power Down Control bit
        1 = Output Power Down Control bit
        0 = Output buffer disabled, Output is high impedance

      bit 11-0 D11:D0: DAC Data bits
        12 bit number “D” which sets the output value. Contains a value between 0 and 4095.    

    */
    
    // Simulate square wave.  
    //  count++;
      
     
     /* // 5000 hz
     if (count > 5) { count = 0; }
     switch (count) {
       
       case 0:
       case 1:
       case 2:
         sample = 0;
         break;
         
       case 3:
       case 4:   
       case 5:
         sample = 4095;
         break;
         
     }   
     
     */ 
     /*
     // 7500hz
     if (count > 3) { count = 0; }
     switch (count) {
       
       case 0:
       case 1:
         sample = 0;
         break;
         
       case 2:
       case 3:   
         sample = 4095;
         break;
         
     }   
*/
    
     
     // if (sample == 0) { sample = 4095; } else { sample = 0; } 
      
      
    // Simulate noise.
      //sample = random(4096);

    // Determine next sample in wavetable to play.
    
      bufferIndex = bufferIndex + 1;// playbackSpeed; // Used a floating point index into buffer to ?
      //bufferIndex = bufferIndex - int(bufferIndex/soundBufferSize)*soundBufferSize; // Wrap around when we reach the end of the buffer.
      if (bufferIndex > 1023) { bufferIndex = 0; }
      
      sample = soundBuffer[int(bufferIndex)];
    
      //wavetablesample / wavetablesamplerate = 0.5 if halfway
      //0.5 * globalsamplerate = 

    // Transmit data:

      //sample = (float(sample) / 255.0) * 4095.0;   // Scale sample from 0..255 to 0..4095 to maximize volume.
      
      // Scale sample from 0..255 to 0..4095 to maximize volume.
      //sample = ((sample-127) << 4) + 2047; // Subtract 127 from sample to produce -127 to 127.  Multiply by 16 to produce -2047 to 2047.  Finally, add 2047 to convert to the 0 to 4097 the DAC needs.
      sample = sample << 4;
      
      // Take the SS pin low to select the DAC.
        //digitalWrite(pinSPI_SS, LOW);
        digitalLOW(pinSPI_SS);
        
      // Send the 16 bits needed to reconfigure the DAC.
      
        //dataHigh = B00110000 | (sample >> 8);
        //dataLow = sample & B11111111;
  
        SPI.transfer(B00110000 | (sample >> 8));
        SPI.transfer(sample & B11111111);
  
      // take the SS pin high to de-select the chip:
        //digitalWrite(pinSPI_SS, HIGH);    
        digitalHIGH(pinSPI_SS);
      
}

/* ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
  
---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

  void setupDACISR() {

    // Set up Timer 1 to send a sample every interrupt.
    // Set timer 1 to trigger 8000 times a second.
  
    // Disable interrupts:  
      cli();

    // Set Timer2 to mode 2 - Clear timer and trigger interrupt on match with OCR2A. (p.161):
      bitClear(TCCR2B, WGM22);
      bitSet(TCCR2A, WGM21);
      bitClear(TCCR2A, WGM20);
      
    // Set Timer2 prescalar to /32 - Trigger timer at a rate of 16mhz/32 :
//      bitClear(TCCR2B, CS22);
//      bitSet(TCCR2B, CS21);
//      bitSet(TCCR2B, CS20);

    // Set Timer2 prescalar to /256 - Trigger Timer2 at a rate of 16mhz/256 or 62500 times a second.
      bitSet(TCCR2B, CS22);
      bitSet(TCCR2B, CS21);
      bitClear(TCCR2B, CS20);
      
    // Set compare register.  This is how many times Timer2 must go off before it triggers an interrupt and clears itself.  OCR2A = 2 gives us 31250hz for our sampling rate. 
    // (One half of that is 15625hz which is the highest frequency tone we can produce.)
    
      //OCR2A = F_CPU / 32 / 8000; // For 8000hz 
      OCR2A = 1; // 1 for 31250hz, 3 for 15625
      
      // F_CPU is the speed of the CPU in MHZ as defined in the bootloader.  It is 16mhz on an Atmega328.
      // We divide F_CPU by 32 because our presclar is 32.  Our timer will tick at a rate of only 0.5mhz.  
      // We then divide the result by 8000, which is the number of times per second we want the interrupt to be called, and the number of times per second we must toggle our DAC output to generate a 4000hz tone.
      // At 16mhz, OCR2A should be 62.5, but the value will be truncated to 62 which means our interrupt will be called at a rate of around 8064.5hz.

    // Enable Timer2 compare match A interrupt when TCNT2 == OCR2A (p.163)
      bitSet(TIMSK2, OCIE2A);
    
    // Reenable interrupts.
      sei();

  }

Does the DAC accept the new value when it is de-selected?

Yes.

Excellent. One more question ... Are you using SPI for anything else?

Nope.

I’ve found a solution to my twitching problem. I don’t know if this is the best solution, or if this will affect my ability to play the highest frequency sounds, as I have not tested that yet, but it’s a very simple fix, and 99% of the servo twitching is gone and what remains is hardly a twitch at all.

// This is called at 31250Hz to load the next sample.
// Servo timer is /8 so for every 3806.25 ticks of servo timer, this interrupt is called once.
// Servo timer is updated at 2mhz.  And the /8 tells us that it potentially can trigger once every 8 system clock cycles.
// We could update servos less often.  That would reduce their resolution.  Ie, the nunber of discreete degrees they could be set to.  
// The twitching problem isn't caused by not updating the servos often enough, but by not updating them at the proper time causing the pulse width to be too long or short.

// We need to know how long our DAC interrupt takes to execute, then we need to compare that to the time remaining on the servo interrupt timer.  Then if the time remaining is less than our interrupt will take to execute we exit the interrupt early and skip that update.

// Problem with comparing OCR1A with TCNT1?
// One servo may trigger at TCNT1 = 3871.  The other at TCNT1 = 40000.  
// TCNT1 is counter.  OCR1A is count to trigger on. 
// If (TCNT1 + FREETIME) > OCR1A, and FREETIME is how long DAC ISR takes to execute, then servo ISR won't trigger in time if we execute ISR so we should return.

// Our ISR triggers with an effective clock divider of /512.  So this ISR _MUST_ be taking less than 512 clock cycles.  That means FREETIME, the time needed to complete one call of the DAC ISR, cannot be greater than that.  
// Also, as the sero timer is set to 8 clock cycles, and freetime is relative to the servo clock, we divide that 512 clock cycles by 8 to determine that freetime's value actually should be less than 64.  
// If we assume our ISR is using half the clock cycles it could possibly use then, then freetime should be set to 32.

ISR(TIMER2_COMPA_vect) { //, ISR_NOBLOCK) {

    const unsigned long FREETIME = 24; // Magic number is between 8 and 32.  Too high MAY affect max frequency dac can output.
  
    static int bufferIndex = 0;
    unsigned int sample;
    byte dataHigh, dataLow;
    
    if ((TCNT1+FREETIME) > OCR1A) { return; } // Check to see if we are running out of time to update the servos on time.  Note that servo timer1 is /8, so FREETIME of 1 is equivalent to 8 clock cycles.
      
    // Simulate noise.
      //sample = random(4096);

    // Determine next sample in wavetable to play.
    
      bufferIndex = bufferIndex + 1;// playbackSpeed; // Used a floating point index into buffer to ?
      //bufferIndex = bufferIndex - int(bufferIndex/soundBufferSize)*soundBufferSize; // Wrap around when we reach the end of the buffer.
      if (bufferIndex > 1023) { bufferIndex = 0; }
      
      sample = soundBuffer[int(bufferIndex)];
    
      //wavetablesample / wavetablesamplerate = 0.5 if halfway
      //0.5 * globalsamplerate = 

    // Transmit data:

      //sample = (float(sample) / 255.0) * 4095.0;   // Scale sample from 0..255 to 0..4095 to maximize volume.
      
      // Scale sample from 0..255 to 0..4095 to maximize volume.
      //sample = ((sample-127) << 4) + 2047; // Subtract 127 from sample to produce -127 to 127.  Multiply by 16 to produce -2047 to 2047.  Finally, add 2047 to convert to the 0 to 4097 the DAC needs.
      sample = sample << 4;
      
      // Take the SS pin low to select the DAC.

        //digitalWrite(pinSPI_SS, LOW);
        //digitalLOW(pinSPI_SS);
                
        PORTB &= ~B100; // Direct port access.  Bit 4 on port B is pin 10, which is the pin hardware SPI uses for slave select.  Note that this bitfield must be different for an Atmega1280.
        
      // Send the 16 bits needed to reconfigure the DAC.
      
        //dataHigh = B00110000 | (sample >> 8);
        //dataLow = sample & B11111111;
  
        SPI.transfer(B00110000 | (sample >> 8)); // These are inlined.
        SPI.transfer(sample & B11111111);
  
      // take the SS pin high to de-select the chip:
      
        //digitalWrite(pinSPI_SS, HIGH);    
        //digitalHIGH(pinSPI_SS);
        
        PORTB |= B100;
      
}

The secret is in the If statement at the start of my ISR. What I have done is peeked at the servo’s timer registers to see if I have enough time left to update my DAC ISR before the servo ISR needs to trigger again, or not. If not, I exit the function early, aborting the DAC update, thus allowing the servo timer to toggle the servo signal pins on time.

This should compromize my sound quality slightly, but with the test tones I am playing now, I can’t hear any difference. What it is really doing is probably lowing my sample rate by a little bit. That would affect my ability to play the highest frequency sounds, but as I mentioned, I have not yet tested that to see if it is in fact the case. I guess I will cross that bridge when I come to it, if it is in fact an issue. I suspect that all of my sound frequencies will be lowered by X amount though, so I will never actually notice if I hit the wall so to speak.

Good idea! I like it. Simple and effective.

In which case you probably don’t want to know what I had in mind … given that it is quite a bit more complicated.

Actually, I would like to know what you had in mind. Even if I don't use it it might come in handy later. :-)

Also I don't yet know for certain that my solution won't come with its own problems.

Hopefully I can put the idea to words... Basically, you put the bulk of the code in loop. In the ISR you do just three three things...

  1. Check a flag that's set when there is a pending write to the DAC. The flag is set in loop when the next value has been sent via SPI.

  2. Take the SS pin high to de-select the chip: digitalHIGH(pinSPI_SS);

  3. Clear the flag so the code in loop knows the stage the next value.

In loop, if the flag is clear, you do everything else that's in the ISR then set the flag. The code in loop is responsible for staging the next value. The code in the ISR is responsible for commiting the next value.

This solution has several advantages...

  • The ISR is extremely short. I think it can be reduced to a few machine instructions by using a naked ISR and a bit of inline assembly.

  • The "slow" code (running in loop) can be interrupted at any time so the effect on the servo ISR is dramatically reduced.

  • The code is essentially identical to what you have now. It's just executed in a different context.

Finally, is there a reason you have to use an interrupt? Can't you just use the blink-without-delay technique?

Is there a reason I have to use an interrupt for the DAC update?

Yes. I tried putting the code in my main loop and it wasn't updating nearly fast enough with all the led animation code and the code to check my resisitve touch switches. I put the DAC code in the ISR because it was of a much higher priority than everything else... except apparently the servo updates.

If I understand what you're suggesting correctly... you're saying I should send the bits to the dac in my main loop, and then the ISR simply sets the dac's select pin high when it's time for that data to be sent to it.

That wouldn't work for the reason I just outlined. The main loop couldn't set up the data nearly fast enough. I'm updating the dac at 31khz so I can output up to a 15khz tone. The very first thing I tried was updating the code in the main loop, and I think it could only output something like a 2-4khz tone.

It's an interesting idea though. And I wasn't aware that I could make my interrupt faster by making it naked. I thought the setup code was pretty much required. Unfortunately I suspect some of the setup code is still needed but I can't know what unless I actually look at the assembly code generated for my function and I don't have any idea how to go about doing that.

Scswitft,

thank you for posting the solution. This is a clever idea which might help me with with some servo jitter problem too.

Korman

If I understand what you're suggesting correctly... you're saying I should send the bits to the dac in my main loop, and then the ISR simply sets the dac's select pin high when it's time for that data to be sent to it.

Yes.

That wouldn't work for the reason I just outlined.

I agree.

It's an interesting idea though. And I wasn't aware that I could make my interrupt faster by making it naked.

You pay a price for naked ISRs. You, the developer, are solely responsible for saving and restoring every register touched by the ISR. You are also responsible for the status regsiter and the proper return (IRET).

I thought the setup code was pretty much required.

It is. Right now the compiler does it for you.

Unfortunately I suspect some of the setup code is still needed but I can't know what unless I actually look at the assembly code generated for my function and I don't have any idea how to go about doing that.

It's probably not worth it. There just isn't very much code left in your ISR. I suspect transferring the next value to the DAC is causing the unacceptable delay.

There is an "SPI Serial Transfer Complete" interrupt. If sending the next value really is the problem, an interrupt driven send would certainly help. You could start the transfer in the timer ISR then finish the transfer in an SPI ISR.

Have you tested the DAC for its SPI performance? Does the DAC's datahsheet mention SPI timing?

Coding: The DAC can run faster than the Arduino can supply data, so I'm running the SPI at /2.

On an unrelated note, I found one optimization for my sound code. Instead of using an if statement to check if I need to wrap my buffer index, I can simply do a bitwise and with 1023 and that will reset the value to 0 when it hits 1024. It'll only work so long as I am stepping through the buffer one sample at a time but I don't think I'll vary the pitch by altering how I step through the playback buffer. Rather I will do that in the portion of the code where I fill the buffer.

I haven't noticed that the optimization has helped with anything, but every little bit counts.