I have not used the wire library, but I had a similar problem with my servos.
The twitching is caused by the servo timer interrupt being called too late. Say the servo wants a 1hz signal to stay at 90 degrees. 1hz signal would require the interrupt to be called once every 1/1000th of a second. If it's called 200 nanoseconds too late, then the servo thinks it should be at a different angle and starts to move to it, but when the next update ends up being on time it moves back.
In the last circuit I built, I was having this problem because I was using an interrupt to send serial data to a DAC over hardware SPI. This hardware interrupt couldn't be interrupted by the servo interrupt so if the time came to update the servos and it was still in the DAC interrupt the servo update would be late.
To solve this, I modified the DAC interrupt to first look at the clock register that triggers the servo interrupt. If I detected that there was not enough time to complete the DAC interrupt's duties before the servo interrupt needed to be triggered, I would exit the interrupt immediately and skip that update. This probably caused a slight variation in the frequencies of sound being output, but I couldn't detect it.
So basically, to fix the problem you have to modify the interrupt in the wire library to look at that servo interrupt's counter.
Here's the code I used in my circuit to do this:
/*
This is called at 31250Hz to load the next sample.
*/
ISR(TIMER2_COMPA_vect) {
const unsigned long FREETIME = 24; // Magic number is between 8 and 32. Too high a value may affect max frequency dac can output.
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.
// Determine next sample in wavetable to play.
soundBufferIndex = soundBufferIndex + 1;// playbackSpeed; // Used a floating point index into buffer to ?
soundBufferIndex = soundBufferIndex & 1023; // if (soundBufferIndex > 1023) { soundBufferIndex = 0; } // Wrap around when we reach the end of the buffer.
sample = soundBuffer[int(soundBufferIndex)];
//wavetablesample / wavetablesamplerate = 0.5 if halfway
//0.5 * globalsamplerate =
// Simulate noise.
//sample = random(4096);
// Scale sample from 0..255 to 0..4095 to maximize volume.
//sample = (float(sample) / 255.0) * 4095.0;
//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; // Can we multiply a negative number by left shifting? Sample will be -127 to 127 I think. We may need to add 127.
// Transmit data:
// 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;
}
/*
*/
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();
}
If you can't figure out what you need to do from that I probably won't be able to help much; it's been forever since I wrote that code so I've forgotten how it works exactly and I don't know what interrupts the I2C lib uses. But if you look in the datasheet for the atmega328 for those interrupt variables I put in caps you should find the relevant section needed to figure out how to modify it to work with the wire lib.