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