internal short[] ReadMagData() //TODO : When using the INT pin, this does not work at all. { byte[] rawData = { 0, 0, 0, 0, 0, 0 }; // x/y/z gyro register data stored here //WriteByte(_ak8975A, AK8975A_CNTL, 0x01); // toggle enable data read from magnetometer, no continuous read mode! //var w = WaitMs(20); _mpu9150.Write(INT_PIN_CFG, 0x02); var w = WaitMs(10); _ak8975A.Write(0x0A, 0x01); w = WaitMs(10); //var rb = ReadByte(_ak8975A, AK8975A_ST1); //Only accept a new magnetometer data read if the data ready bit is set and // if there are no sensor overflow or data read errors //if (ReadByte(_ak8975A, AK8975A_ST1) & 0x01) > 0)) //So the return for read byte should be a byte? //{ // wait for magnetometer data ready bit to be set _ak8975A.Read(6, AK8975A_XOUT_L, out rawData); // Read the six raw data registers sequentially into data array var magData = new short[3]; magData[0] = (short)((rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value magData[1] = (short)((rawData[3] << 8) | rawData[2]); magData[2] = (short)((rawData[5] << 8) | rawData[4]); //} return(magData); }
/// <summary> /// Testing detecting vibration when a foot contacts an object. /// </summary> /// <param name="channel"></param> internal void Start(int channel) { Task.Factory.StartNew(() => { var avg = new double[10]; int i = 0; while (true) { var config = Enums.Ads1115.Config.CQUE_NONE | Enums.Ads1115.Config.CLAT_NONLAT | Enums.Ads1115.Config.CPOL_ACTVLOW | Enums.Ads1115.Config.CMODE_TRAD | Enums.Ads1115.Config.DR_128SPS | Enums.Ads1115.Config.MODE_SINGLE; var newconfig = ((uint)config) | ((uint)Enums.Ads1115.Gain.Sixteen) | ((uint)Enums.Ads1115.Config.MUX_SINGLE_0) | ((uint)Enums.Ads1115.Config.OS_SINGLE); var r = _ads1115.Write(new byte[] { 0x01, (byte)(newconfig >> 8), (byte)(newconfig & 0xff) }); _stopwatch.Restart(); while (_stopwatch.ElapsedMilliseconds < 4) { } r = _ads1115.Write(new byte[] { 0x00, 0x00 }); byte[] toRead; r = _ads1115.Read(2, out toRead); var val = BitConverter.ToUInt16(toRead, 0); if (val == 256) { val = 0; } avg[i] = val; if (i == 9) { i = 0; var outVal = avg.Sum() / 10; if (outVal > 30000) //When a foot hits the floor, you get some value above this. { Debug.WriteLine("Value " + outVal); } continue; } i++; } }, TaskCreationOptions.LongRunning); }
internal bool WriteBit(I2CDevice device, byte regAddr, byte bitNum, byte data) { byte[] b; device.Write(new[] { regAddr }); device.Read(1, out b); if (data != 0) { b[0] = (byte)(1 << bitNum); } else { b[0] = (byte)(b[0] & (byte)(~(1 << bitNum))); } return(device.Write(new[] { regAddr, b[0] })); }
internal bool WriteBits(I2CDevice device, byte regAddr, byte bitStart, byte length, byte data) { // 010 value to write // 76543210 bit numbers // xxx args: bitStart=4, length=3 // 00011100 mask byte // 10101111 original value (sample) // 10100011 original & ~mask // 10101011 masked | value byte[] b; device.Write(new[] { regAddr }); if (device.Read(regAddr, out b)) { var mask = (byte)(((1 << length) - 1) << (bitStart - length + 1)); data <<= (bitStart - length + 1); // shift data into correct position data &= mask; // zero all non-important bits in data b[0] &= (byte)(~(mask)); // zero all important bits in existing byte b[0] |= data; // combine data with existing byte return(device.Write(new[] { regAddr, b[0] })); } return(false); }
private int GetFifoCount(I2CDevice device) { if (!device.Write(new[] { FIFO_COUNTH })) { return(0); } byte[] buffer; var r = device.Read(2, out buffer); if (r) { return((buffer[0] << 8) | buffer[1]); //Get byte count } return(0); }
internal async Task InitializeHardware() { try { _ioController = GpioController.GetDefault(); _interruptPin = _ioController.OpenPin(17); _interruptPin.Write(GpioPinValue.Low); _interruptPin.SetDriveMode(GpioPinDriveMode.Input); _interruptPin.ValueChanged += Interrupt; _mpu9150 = new I2CDevice((byte)Mpu9150Setup.Address, I2cBusSpeed.FastMode); await _mpu9150.Open(); await Task.Delay(5); // power up _mpu9150.Write((byte)Mpu9150Setup.PowerManagement1, 0x80);// reset the device await Task.Delay(100); _mpu9150.Write((byte)Mpu9150Setup.PowerManagement1, 0x2 ); _mpu9150.Write((byte)Mpu9150Setup.UserCtrl, 0x04);//reset fifo _mpu9150.Write((byte)Mpu9150Setup.PowerManagement1, 1 ); // clock source = gyro x _mpu9150.Write((byte)Mpu9150Setup.GyroConfig, 0); // +/- 250 degrees sec, max sensitivity _mpu9150.Write((byte)Mpu9150Setup.AccelConfig, 0); // +/- 2g, max sensitivity _mpu9150.Write((byte)Mpu9150Setup.Config, 1);// 184 Hz, 2ms delay _mpu9150.Write((byte)Mpu9150Setup.SampleRateDiv, 19); // set rate 50Hz _mpu9150.Write((byte)Mpu9150Setup.FifoEnable, 0x78); // enable accel and gyro to read into fifo _mpu9150.Write((byte)Mpu9150Setup.UserCtrl, 0x40); // reset and enable fifo _mpu9150.Write((byte)Mpu9150Setup.InterruptEnable, 0x1); } catch (Exception ex) { Debug.WriteLine(ex); } }
internal void SetPwm(byte channel, ushort on, ushort off) { _pca9685.Write((byte)(Registers.LED0_ON_L + 4 * channel), (byte)(on & 0xFF)); _pca9685.Write((byte)(Registers.LED0_ON_H + 4 * channel), (byte)(on >> 8)); _pca9685.Write((byte)(Registers.LED0_OFF_L + 4 * channel), (byte)(off & 0xFF)); _pca9685.Write((byte)(Registers.LED0_OFF_H + 4 * channel), (byte)(off >> 8)); }
internal async Task InitHardware() { try { _ioController = GpioController.GetDefault(); _interruptPin = _ioController.OpenPin(17); _interruptPin.Write(GpioPinValue.Low); _interruptPin.SetDriveMode(GpioPinDriveMode.Input); _interruptPin.ValueChanged += _interruptPin_ValueChanged; _mpu9150 = new I2CDevice((byte)Mpu9150Setup.Address, I2cBusSpeed.FastMode); await _mpu9150.Open(); await Task.Delay(100); // power up _mpu9150.Write((byte)Mpu9150Setup.PowerManagement1, 0x80); // reset the device await Task.Delay(100); _mpu9150.Write((byte)Mpu9150Setup.PowerManagement1, 0x2); _mpu9150.Write((byte)Mpu9150Setup.UserCtrl, 0x04); //reset fifo _mpu9150.Write((byte)Mpu9150Setup.PowerManagement1, 1); // clock source = gyro x _mpu9150.Write((byte)Mpu9150Setup.GyroConfig, 0); // +/- 250 degrees sec, max sensitivity _mpu9150.Write((byte)Mpu9150Setup.AccelConfig, 0); // +/- 2g, max sensitivity _mpu9150.Write((byte)Mpu9150Setup.Config, 1); // 184 Hz, 2ms delay _mpu9150.Write((byte)Mpu9150Setup.SampleRateDiv, 19); // set rate 50Hz _mpu9150.Write((byte)Mpu9150Setup.FifoEnable, 0x78); // enable accel and gyro to read into fifo _mpu9150.Write((byte)Mpu9150Setup.UserCtrl, 0x40); // reset and enable fifo _mpu9150.Write((byte)Mpu9150Setup.InterruptEnable, 0x1); } catch (Exception ex) { Debug.WriteLine(ex); } }
private async void Interrupt(GpioPin sender, GpioPinValueChangedEventArgs args) { await Task.Delay(10); if (_mpu9150 == null) { return; } int interruptStatus = _mpu9150.ReadRegisterSingle((byte)Mpu9150Setup.InterruptStatus); if ((interruptStatus & 0x10) != 0) { _mpu9150.Write((byte)Mpu9150Setup.UserCtrl, 0x44);// reset - enable fifo } if ((interruptStatus & 0x1) == 0) { return; } var ea = new MpuSensorEventArgs(); ea.Status = (byte)interruptStatus; ea.SamplePeriod = 0.02f; var l = new List <MpuSensorValue>(); int count = _mpu9150.ReadUshort((byte)Mpu9150Setup.FifoCount); while (count >= SensorBytes) { _mpu9150.Write((byte)Mpu9150Setup.FifoReadWrite); byte[] buffer; _mpu9150.Read(SensorBytes, out buffer); count -= SensorBytes; var xa = (short)(buffer[0] << 8 | buffer[1]); var ya = (short)(buffer[2] << 8 | buffer[3]); var za = (short)(buffer[4] << 8 | buffer[5]); var xg = (short)(buffer[6] << 8 | buffer[7]); var yg = (short)(buffer[8] << 8 | buffer[9]); var zg = (short)(buffer[10] << 8 | buffer[11]); var sv = new MpuSensorValue { AccelerationX = xa / 16384d, AccelerationY = ya / 16384d, AccelerationZ = za / 16384d, GyroX = xg / 131d, GyroY = yg / 131d, GyroZ = zg / 131d }; l.Add(sv); var gain = 0.00875; var xRotationPerSecond = sv.GyroX * gain; //xRotationPerSecond is the rate of rotation per second. var loopPeriod = 0.015; //loop period - 0.02 _gyroXangle += xRotationPerSecond * loopPeriod; var radToDeg = 57.29578; var accXangle = (Math.Atan2(sv.AccelerationY, sv.AccelerationZ) + Math.PI) * radToDeg; var complementaryFilterConstant = 0.98; _cFangleX = complementaryFilterConstant * (_cFangleX + xRotationPerSecond * loopPeriod) + (1 - complementaryFilterConstant) * accXangle; //Debug.WriteLine("X: " + sv.GyroX + ", Y: " + sv.GyroY + ", Z: " + sv.GyroZ); //Debug.WriteLine("CFangleX: " + _cFangleX); //Debug.WriteLine("AccelX: " + sv.AccelerationX + ", AccelY: " + sv.AccelerationY + ", AccelZ: " + sv.AccelerationZ); } ea.Values = l.ToArray(); if (SensorInterruptEvent == null) { return; } if (ea.Values.Length > 0) { SensorInterruptEvent(this, ea); } }
private int GetFifoCount(I2CDevice device) { if (!device.Write(new[] {FIFO_COUNTH})) return 0; byte[] buffer; var r = device.Read(2, out buffer); if (r) return (buffer[0] << 8) | buffer[1]; //Get byte count return 0; }
internal bool WriteBits(I2CDevice device, byte regAddr, byte bitStart, byte length, byte data) { // 010 value to write // 76543210 bit numbers // xxx args: bitStart=4, length=3 // 00011100 mask byte // 10101111 original value (sample) // 10100011 original & ~mask // 10101011 masked | value byte[] b; device.Write(new[] { regAddr }); if (device.Read(regAddr, out b)) { var mask = (byte)(((1 << length) - 1) << (bitStart - length + 1)); data <<= (bitStart - length + 1); // shift data into correct position data &= mask; // zero all non-important bits in data b[0] &= (byte)(~(mask)); // zero all important bits in existing byte b[0] |= data; // combine data with existing byte return device.Write(new[] { regAddr, b[0] }); } return false; }
internal bool WriteBit(I2CDevice device, byte regAddr, byte bitNum, byte data) { byte[] b; device.Write(new[] { regAddr }); device.Read(1, out b); if (data != 0) { b[0] = (byte)(1 << bitNum); } else { b[0] = (byte)(b[0] & (byte)(~(1 << bitNum))); } return device.Write(new[] { regAddr, b[0] }); }
private void ConfigureMpr121() { _i2CDevice.Write(new byte[] { SoftReset, 0x63 }); Task.Delay(10).Wait(); _i2CDevice.Write(new byte[] { ECR, 0x00 }); SetThresholds(4, 3); _i2CDevice.Write(new byte[] { MHD_R, 0x01 }); _i2CDevice.Write(new byte[] { NHD_R, 0x01 }); _i2CDevice.Write(new byte[] { NCL_R, 0x0E }); _i2CDevice.Write(new byte[] { FDL_R, 0x00 }); _i2CDevice.Write(new byte[] { MHD_F, 0x01 }); _i2CDevice.Write(new byte[] { NHD_F, 0x05 }); _i2CDevice.Write(new byte[] { NCL_F, 0x01 }); _i2CDevice.Write(new byte[] { FDL_F, 0x00 }); _i2CDevice.Write(new byte[] { NHD_T, 0x00 }); _i2CDevice.Write(new byte[] { NCL_T, 0x00 }); _i2CDevice.Write(new byte[] { FDL_T, 0x00 }); _i2CDevice.Write(new byte[] { Debounce, 0x00 }); _i2CDevice.Write(new byte[] { Config1, 0x10 }); _i2CDevice.Write(new byte[] { Config2, 0x20 }); _i2CDevice.Write(new byte[] { ECR, 0x03 }); //_i2CDevice.Write(new byte[] { Mpr121.GPIO_EN, 0xff }); //_i2CDevice.Write(new byte[] { Mpr121.GPIO_DIR, 0xff }); //_i2CDevice.Write(new byte[] { Mpr121.GPIO_CTRL0, 0xff }); //_i2CDevice.Write(new byte[] { Mpr121.GPIO_CTRL1, 0xff }); //_i2CDevice.Write(new byte[] { Mpr121.GPIO_CLEAR, 0xff }); _gpioPin.ValueChanged += Pin_ValueChanged; }