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);
}
