private void SetClockSource(MPU6050ClockSource source) { int value = _device.ReadAddressByte(MPU6050_REG_PWR_MGMT_1); value &= 0b11111000; value |= (int)source; _device.WriteAddressByte(MPU6050_REG_PWR_MGMT_1, (byte)value); }
void WriteAddressByte(int address, byte data, int maxRetryCount = 150) { bool success = false; for (int i = 0; i < maxRetryCount && !success; i++) { try { Device.WriteAddressByte(address, data); success = true; } catch (HardwareException e) when(e.ErrorCode == 121) { if (i == maxRetryCount - 1) { throw; } } } }
public void Init(int frequency = 60) { // SoftwareReset(); // Task.Delay(5).Wait(); //SetAllPwm(0, 0); _device.WriteAddressByte(MODE2, OUTDRV); _device.WriteAddressByte(MODE1, ALLCALL); Task.Delay(5).Wait(); // wait for oscillator var mode1 = _device.ReadAddressByte(MODE1); mode1 = (byte)(mode1 & ~SLEEP); // wake up (reset sleep) _device.WriteAddressByte(MODE1, mode1); Task.Delay(5).Wait(); // wait for oscillator SetPwmFreq(frequency); for (int i = 0; i < 16; i++) { SetServoPosition(i, SERVO_MIN + ((SERVO_MAX - SERVO_MIN) / 2)); } }
public void Start() { _device.WriteAddressByte(STATUS_REGISTER, 1); }