public float Fit()
{
float fit = 0;
for (int i = 0; i < templates.Count; i++)
{
float dot = Vector.Dot(templates[i], Skweezee.GetVector());
if (dot > fit)
{
fit = dot;
}
}
return(fit);
}
/*
* GetIMUAngle : Returns the angle around the z and x axis, useful for steering wheel applications
*/
public static float[] GetIMUAngle()
{
int[] IMU = Skweezee.GetIMU();
float[] IMUFloat = new float[6];
// Convert them all to floats
for (int i = 0; i < IMUFloat.Length; i++)
{
IMUFloat[i] = (1 / 0.75f) * 180 * ((float)IMU[i] - 127f) / 255f;
}
// Average out over history
hist.Add(IMUFloat);
IMUFloat = new float[6];
if (hist.Count > 10)
{
hist.RemoveAt(0); // Only use the last 10 samples
}
for (int i = 0; i < hist.Count; i++)
{
for (int index = 0; index < 6; index++)
{
IMUFloat[index] += hist[i][index];
}
}
for (int i = 0; i < 6; i++)
{
IMUFloat[i] /= hist.Count;
}
Vector3 acc = new Vector3(IMUFloat[0], IMUFloat[1], IMUFloat[2]);
acc.Normalize();
Vector3 gyro = new Vector3(IMUFloat[3], IMUFloat[4], IMUFloat[5]);
float az = 180 * Mathf.Acos(acc.x) / Mathf.PI * Mathf.Sign(acc.z); // Rotation around the Z-axis (pointed towards user)
float ay = 180 * Mathf.Acos(acc.z) / Mathf.PI * Mathf.Sign(acc.x);
return(new float[] { az, ay });
}
public void Record()
{
templates = new List <float[]>();
templates.Add(Skweezee.GetVector());
}
