private void Quaterniones(float[] q, MadgwickAHRS updatevalues)
{
q[0] = updatevalues.Quaternion[0];
q[1] = updatevalues.Quaternion[1];
q[2] = updatevalues.Quaternion[2];
q[3] = updatevalues.Quaternion[3];
}
private void button7_Click(object sender, EventArgs e)
{
foreach (var series in chart1.Series)
{
series.Points.Clear();
}
foreach (var series in chart2.Series)
{
series.Points.Clear();
}
foreach (var series in chart3.Series)
{
series.Points.Clear();
}
i = 0;
i = 0;
nmin = 0;
nmax = 200;
chart1.ChartAreas[0].AxisX.Minimum = nmin;
chart1.ChartAreas[0].AxisX.Maximum = nmax;
chart1.ChartAreas[0].AxisY.Maximum = 3;
chart2.ChartAreas[0].AxisX.Minimum = nmin;
chart2.ChartAreas[0].AxisX.Maximum = nmax;
chart2.ChartAreas[0].AxisY.Maximum = 3;
chart3.ChartAreas[0].AxisX.Minimum = nmin;
chart3.ChartAreas[0].AxisX.Maximum = nmax;
chart3.ChartAreas[0].AxisY.Maximum = 3;
updatevalues = new MadgwickAHRS(395.5f, float.Parse(textBox8.Text));
}
// static void Main (string[] args) {
//
// var lsm9DS1 = new LSM9DS1 (false);
// lsm9DS1.GyroChanged += (sender, vector3) =>
// {
// Console.WriteLine($"Gyro X: {vector3.X} Y: {vector3.Y} Z: {vector3.Z}");
// };
// lsm9DS1.AccelerometerChanged += (sender, vector3) => {
// Console.WriteLine ($"Accelerometer X: {vector3.X} Y: {vector3.Y} Z: {vector3.Z}");
// };
//
// lsm9DS1.StartInternalPolling ();
//
// Thread.Sleep (Timeout.Infinite);
// }
// static void Main(string[] args)
// {
// var lsm9DS1 = new LSM9DS1();
// lsm9DS1.Begin();
// while (true)
// {
// var acc = lsm9DS1.ReadAccelerometer();
// var gyro = lsm9DS1.ReadGyroscope();
// var mag = lsm9DS1.ReadMagnetometer();
// Console.WriteLine($"Accelerometer x {acc.X:N3} y {acc.X:N3} z {acc.Z:N3}");
// Console.WriteLine($"Gyroscope x {gyro.X:N3} y {gyro.X:N3} z {gyro.Z:N3}");
// Console.WriteLine($"Magnetometer x {mag.X:N3} y {mag.X:N3} z {mag.Z:N3}");
// Console.WriteLine();
// Thread.Sleep(50);
// }
// Thread.Sleep(Timeout.Infinite);
// }
static async Task Main(string[] args)
{
// This switch must be set before creating the GrpcChannel/HttpClient.
AppContext.SetSwitch(
"System.Net.Http.SocketsHttpHandler.Http2UnencryptedSupport", true);
var channel = GrpcChannel.ForAddress("http://192.168.0.40:5000");
var client = new Location.LocationClient(channel);
var degreesToRadiansFactor = Math.PI / 180;
var radiansToDegreesFactor = 180.0f / Math.PI;
var samplePeriod = TimeSpan.FromMilliseconds(50);
// var ahrs = new MahonyAHRS(samplePeriod, 5f);
var ahrs = new MadgwickAHRS((float)samplePeriod.TotalSeconds, 0.1f);
var lsm9DS1 = new LSM9DS1();
lsm9DS1.Begin();
while (true)
{
// read fresh sensor data
var acc = lsm9DS1.ReadAccelerometer();
var gyro = lsm9DS1.ReadGyroscope();
var mag = lsm9DS1.ReadMagnetometer();
// convert gyro readings from dps to rad/s
var gx = (float)(gyro.X * degreesToRadiansFactor);
var gy = (float)(gyro.Y * degreesToRadiansFactor);
var gz = (float)(gyro.Z * degreesToRadiansFactor);
// update ahrs
ahrs.Update(gx, gy, gz,
acc.X, acc.Y, acc.Z,
mag.X, mag.Y, mag.Z
);
ahrs.ComputeAngles();
// output rol, pitch and yaw
var rollDegrees = ahrs.Roll * radiansToDegreesFactor;
var pitchDegrees = ahrs.Pitch * radiansToDegreesFactor;
var yawDegrees = ahrs.Yaw * radiansToDegreesFactor;
client.UpdateLocationAsync(
new NewLocation {
Roll = rollDegrees, Pitch = pitchDegrees, Yaw = yawDegrees
}
);
Console.WriteLine($"Roll {rollDegrees:N3} Pitch {pitchDegrees:N3} Yaw {yawDegrees:N3}");
Console.WriteLine();
Thread.Sleep(samplePeriod);
}
Thread.Sleep(Timeout.Infinite);
}
public void AHRS_Test()
{
MadgwickAHRS AHRS = new MadgwickAHRS(0.00000001f);
AHRS.Update(0f, 0f, 0f, 0f, 1f, 0f);
List <float> q = AHRS.Quaternion.ToList();
Assert.AreEqual(1, Math.Sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]));
}
public GearVRController(ulong bluetoothAddress)
{
_bluetoothAddress = bluetoothAddress;
_ahrs = new MadgwickAHRS(
samplePeriod: 1 / 68.84681583453657f, // Madgwick is sensitive to this
beta: 0.352f
);
//_ahrs = new MadgwickAHRS(1f / GEARVR_HZ, 0.01f);
// start MonotorThread
_monitorThread = new Thread(MonitorThreadWorker);
_monitorThread.IsBackground = true;
_monitorThread.Start();
}
public float[] getEuler(float[][] matGyr, float[][] matAcc)
{
//float[] matOut={0,0,0};
float[] euler = new float[] { 0, 0, 0 };
//float seamplePeriod = 1 / 256f;
float[, ][] R = new float[3, 3][];
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
R [i, j] = new float[matGyr.Length];
for (int k = 0; k < matGyr.Length; k++)
{
R [i, j] [k] = 0f;
}
}
}
MadgwickAHRS ahrs = new MadgwickAHRS(1 / 256f);
for (int i = 0; i < matGyr.Length; i++)
{
float imp = (float)3.14 / 180;
//gyro in grade
float gx = (matGyr [i] [0] * imp);
float gy = matGyr [i] [1] * imp;
float gz = matGyr [i] [2] * imp;
float ax = matAcc [i] [0];
float ay = matAcc [i] [1];
float az = matAcc [i] [2];
ahrs.Update(gx, gy, gz, ax, ay, az);
euler = ahrs.QuaternToEuler();
}
return(euler);
}
public static async Task Main(string[] args)
{
//Load in data from sensor
MLContext mlContext = new MLContext();
IDataView dataView = mlContext.Data.LoadFromTextFile <SensorData>(path: linAccOriginal, hasHeader: true, separatorChar: ',');
var accelerations = mlContext.Data.CreateEnumerable <SensorData>(dataView, reuseRowObject: false).ToList();
var time = accelerations.Select(x => x.Time).ToList();
//load data from python export
IDataView counts = mlContext.Data.LoadFromTextFile <linnaccpy>(path: counts_path, hasHeader: true);
var acc_counts = mlContext.Data.CreateEnumerable <linnaccpy>(counts, reuseRowObject: false).ToList();
//load data from wheel export
IDataView wheelView = mlContext.Data.LoadFromTextFile <WheelD>(path: wheel_path, hasHeader: true, separatorChar: ',');
var wheeldata = mlContext.Data.CreateEnumerable <WheelD>(wheelView, reuseRowObject: false).ToList();
//Apply filter
List <double> difference = new List <double>();
for (int k = 1; k < accelerations.Count; k++)
{
difference.Add(accelerations[k].Time - accelerations[k - 1].Time);
}
var average = difference.Sum(x => x) / difference.Count();
var filteredAcceleration = Butterworth(accelerations.Select(x => x.Acceleration_x).ToArray(), average, 0.5);
var jerk = Derivative(filteredAcceleration.ToList(), average);
List <CustomDouble> accelerationRoots = new List <CustomDouble>();
bisection(1, filteredAcceleration.ToList().Count - 1, filteredAcceleration.ToList());
accelerationRoots = foundRoots.ToList();
exportdata(filteredAcceleration.ToList(), "versnelling_filtered", "m/s^2", true, foundRoots.Select(x => x.countInArray).ToList());
List <CustomDouble> max = new List <CustomDouble>();
for (int k = 0; k < foundRoots.Count; k++)
{
//Find all max: //f'(t+h) < 0 and f'(t-h) > 0
int h = 5;
int tminh = foundRoots[k].countInArray - h;
int tplush = foundRoots[k].countInArray + h;
if (foundRoots[k].countInArray - h < 0)
{
tminh = foundRoots[k].countInArray;
}
if (foundRoots[k].countInArray + h > foundRoots.Count - 1)
{
tplush = foundRoots[k].countInArray + h;
}
if (jerk[tminh] > 0 && jerk[tplush] < 0)
{
max.Add(foundRoots[k]);
}
}
difference = new List <double>();
for (int k = 1; k < wheeldata.Count; k++)
{
difference.Add(wheeldata[k].Time - wheeldata[k - 1].Time);
}
var averageWheelPeriod = difference.Sum(x => x) / difference.Count();
MadgwickAHRS AHRS = new MadgwickAHRS((float)averageWheelPeriod, 1f);
List <string> eulerAngles = new List <string>(); //Z,Y,X
for (int i = 0; i < wheeldata.Count - 2; i++)
{
var k = wheeldata[i];
float[] a = { (float)k.Accy / (float)9.81, (float)k.Accz / (float)9.81, (float)k.Accx / (float)9.81 };
float[] g = { (float)k.GyroY, (float)k.GyorZ, (float)k.GyorX };
float[] B = { (float)k.By, (float)k.Bz, (float)k.Bx };
var e = new MagData(g, a, B);
AHRS.Update((e.Gyroscope[0]), (e.Gyroscope[1]), (e.Gyroscope[2]), e.Accelerometer[0], e.Accelerometer[1], e.Accelerometer[2]);
var z = (new QuaternionData(AHRS.Quaternion)).ConvertToConjugate().ConvertToEulerAnglesCSVstring();
eulerAngles.Add(z);
}
System.Text.StringBuilder sb = new System.Text.StringBuilder();
sb.AppendLine($"Z,Y,X");
foreach (var item in eulerAngles)
{
sb.AppendLine(item.ToString());
}
string eulerAnglespath = Path.Combine("/D/", "acc_export", $"euler_angles.csv");
System.IO.File.WriteAllText(
eulerAnglespath,
sb.ToString());
//rotatie om y-as dus...
//load data from wheel export
IDataView eulerView = mlContext.Data.LoadFromTextFile <EulerData>(path: eulerAnglespath, hasHeader: true, separatorChar: ',');
var AnglesData = mlContext.Data.CreateEnumerable <EulerData>(eulerView, reuseRowObject: false).ToList(); // in deg
exportdata(AnglesData.Select(x => x.Z).ToList(), "z-theta", "deg", true);
exportdata(AnglesData.Select(x => x.Y).ToList(), "y-theta", "deg", true);
exportdata(AnglesData.Select(x => x.X).ToList(), "x-theta", "deg", true);
//perform bisection
var y2 = Butterworth(wheeldata.Select(x => x.GyroY).ToArray(), averageWheelPeriod, 0.5);
List <double> angleTimes = new List <double>();
for (int k = 0; k < AnglesData.Count; k++)
{
angleTimes.Add(k * averageWheelPeriod);
}
bisection(1, y2.ToList().Count - 1, y2.ToList());
exportdata(y2.ToList(), "gyro", "rad/s", true, foundRoots.Select(x => x.countInArray).ToList());
bisection(1, jerk.ToList().Count - 1, jerk.ToList());
exportdata(jerk.ToList(), "jerk", "m/s^3", true);
List <CustomBool> maxOrMins = new List <CustomBool>();
for (int j = 0; j < foundRoots.Count; j++)
{
//f'(t-h) >0 en f'(t+h) < 0 , waar h een klein getal is. (maxima)
int h = 5;
int k = foundRoots[j].countInArray;
bool IsMax = false;
int indexForPlus = k + h;
int indexForMin = k - h;
if (k + h > jerk.Count)
{
indexForPlus = k;
}
else if (k - h < 0)
{
indexForMin = k;
}
var jj = jerk[indexForPlus];
if (jerk[indexForMin] > 0 && jerk[indexForPlus] < 0)
{
IsMax = true;
}
maxOrMins.Add(new CustomBool {
index = k, Max = IsMax
});
}
exportdata(filteredAcceleration.ToList(), "acc", "m/s^2", true, maxOrMins.Select(x => (int)x.index).ToList());
List <double> t = new List <double>();
for (int k = 0; k < accelerationRoots.Count - 1; k++)
{
double averageXroot = (accelerationRoots[k + 1].countInArray + accelerationRoots[k].countInArray) / 2.0;
var nearestMaxima = maxOrMins.OrderBy(x => Math.Abs((int)x.index - (int)averageXroot)).First();
if (nearestMaxima.Max)
{
var dt = (accelerationRoots[k + 1].countInArray - accelerationRoots[k].countInArray) * average;
t.Add(dt);
}
}
var countsmin = 5413.7; // 7.242048 kilometers per hour/TO CHANGE WHEN WE GET WRIST ACCELERATIONS!
double m = 80; // 80 kg
double r = 0.35; // wheel radius 35 cm
//https://www.nature.com/articles/sc201533.pdf
double EE = (0.0022 * countsmin + 3.13) / 1000; // V02 (L min-1 kg-1)
double EEm = EE * m * 5 * (t.Sum() / 60.0);
Console.WriteLine("Number of revolutions: " + (int)(foundRoots.Count / 2.0));
Console.WriteLine("Distance traveled: " + (int)((foundRoots.Count / 2.0) * 2 * Math.PI * r) + " m");
Console.WriteLine("Burned kcal: " + EEm);
}
private void Start()
{
ahrs = GameObject.Find("Hand").GetComponent <MadgwickAHRS>();
}
private void Awake()
{
_ahrs = new MadgwickAHRS(1 / 256f, .1f);
_lastQ = Quaternion.identity;
}
