private void SetPwmFreq(int freq_hz) { _frequency = freq_hz; var prescaleval = 25000000.0; // 25MHz prescaleval /= 4096.0; // 12-bit prescaleval /= ((double)freq_hz * 0.9); //Compensation: https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11 prescaleval -= 1.0; var prescale = (byte)Math.Floor(prescaleval + 0.5); var oldmode = _device.ReadAddressByte(MODE1); var newmode = (oldmode & 0x7F) | 0x10; // sleep _device.WriteAddressWord(MODE1, (byte)newmode); // go to sleep _device.WriteAddressWord(PRESCALE, prescale); _device.WriteAddressWord(MODE1, oldmode); Task.Delay(5).Wait(); // wait for oscillator _device.WriteAddressByte(MODE1, (byte)(oldmode | 0x80)); }