public void CollectAccelGyroData()
{
Console.WriteLine("Will collect AccelGyroData. Inside of CollectAccelGyroData().");
int fingerId, knuckleId;
Console.WriteLine(GlobalQueue.GetSize(GlobalQueue.CalibrationQueue));
while (GlobalQueue.GetSize(GlobalQueue.CalibrationQueue) > 1000)
{
calibrationData = GlobalQueue.Get(GlobalQueue.CalibrationQueue);
fingerId = (int)calibrationData.finger_id;
knuckleId = (int)calibrationData.knuckle_id;
/* Get acceleration data */
accel[fingerId, knuckleId].x += calibrationData.xAcc;
accel[fingerId, knuckleId].y += calibrationData.yAcc;
accel[fingerId, knuckleId].z += calibrationData.zAcc;
/* Get gyro data */
gyro[fingerId, knuckleId].x += calibrationData.xGyro;
gyro[fingerId, knuckleId].y += calibrationData.yGyro;
gyro[fingerId, knuckleId].z += calibrationData.zGyro;
/* Increment number of data points for this finger+knuckle */
numDataPoints[fingerId, knuckleId] += 1;
}
Console.WriteLine("Exiting CollectAccelGyroData().");
}
public static MotionData Get(ConcurrentQueue <byte> Q)
{
byte[] arr = new byte[20];
byte delim = new byte();
bool exit = false;
while (!exit)
{
Q.TryDequeue(out delim);
if (delim == 0xCD)
{
Q.TryDequeue(out delim);
if (delim == 0xAB)
{
for (int i = 0; i < 20; ++i)
{
Q.TryDequeue(out arr[i]);
}
exit = true;
}
}
}
GCHandle handle = GCHandle.Alloc(arr, GCHandleType.Pinned);
MotionData data = (MotionData)Marshal.PtrToStructure(handle.AddrOfPinnedObject(), typeof(MotionData));
handle.Free();
return(data);
}
public Calibrator()
{
/* Initialize all calibration data to 0 at first */
calibrationData = new MotionData();
/* Initialize data arrays */
accel = new Vector3[TOTAL_FINGER_REFS, TOTAL_KNUCKLE_REFS];
maxMag = new Vector3[TOTAL_FINGER_REFS, TOTAL_KNUCKLE_REFS];
minMag = new Vector3[TOTAL_FINGER_REFS, TOTAL_KNUCKLE_REFS];
gyro = new Vector3[TOTAL_FINGER_REFS, TOTAL_KNUCKLE_REFS];
/* Initialize offset arrays */
accelOffset = new Vector3[TOTAL_FINGER_REFS, TOTAL_KNUCKLE_REFS];
magOffset = new Vector3[TOTAL_FINGER_REFS, TOTAL_KNUCKLE_REFS];
gyroOffset = new Vector3[TOTAL_FINGER_REFS, TOTAL_KNUCKLE_REFS];
numDataPoints = new int[TOTAL_FINGER_REFS, TOTAL_KNUCKLE_REFS];
for (int i = 0; i < TOTAL_FINGER_REFS; i++)
{
if (i == (int)Finger.HAND)
{
accel[i, 1] = new Vector3();
maxMag[i, 1] = new Vector3();
minMag[i, 1] = new Vector3();
gyro[i, 1] = new Vector3();
accelOffset[i, 1] = new Vector3();
magOffset[i, 1] = new Vector3();
gyroOffset[i, 1] = new Vector3();
numDataPoints[i, 1] = 0;
}
else
{
for (int j = 0; j < TOTAL_KNUCKLE_REFS; j++)
{
accel[i, j] = new Vector3();
maxMag[i, j] = new Vector3();
minMag[i, j] = new Vector3();
gyro[i, j] = new Vector3();
accelOffset[i, j] = new Vector3();
magOffset[i, j] = new Vector3();
gyroOffset[i, j] = new Vector3();
numDataPoints[i, j] = 0;
}
}
}
}
private void setMinMag(int fingerId, int knuckleId, MotionData potentialMin)
{
if (minMag[fingerId, knuckleId].x > potentialMin.xMag)
{
minMag[fingerId, knuckleId].x = potentialMin.xMag;
}
if (minMag[fingerId, knuckleId].y > potentialMin.yMag)
{
minMag[fingerId, knuckleId].y = potentialMin.yMag;
}
if (minMag[fingerId, knuckleId].z > potentialMin.zMag)
{
minMag[fingerId, knuckleId].z = potentialMin.zMag;
}
}
private void setMaxMag(int fingerId, int knuckleId, MotionData potentialMax)
{
if (maxMag[fingerId, knuckleId].x < potentialMax.xMag)
{
maxMag[fingerId, knuckleId].x = potentialMax.xMag;
}
if (maxMag[fingerId, knuckleId].y < potentialMax.yMag)
{
maxMag[fingerId, knuckleId].y = potentialMax.yMag;
}
if (maxMag[fingerId, knuckleId].z < potentialMax.zMag)
{
maxMag[fingerId, knuckleId].z = potentialMax.zMag;
}
}
public void CollecMagData()
{
Console.WriteLine("Will collect MagData. Inside of CollectMagData().");
int fingerId, knuckleId;
while (GlobalQueue.GetSize(GlobalQueue.MagCalibrationQueue) > 1000)
{
calibrationData = GlobalQueue.Get(GlobalQueue.MagCalibrationQueue);
fingerId = (int)calibrationData.finger_id;
knuckleId = (int)calibrationData.knuckle_id;
/* Get mag data */
setMaxMag(fingerId, knuckleId, calibrationData);
setMinMag(fingerId, knuckleId, calibrationData);
}
Console.WriteLine("Exiting CollectMagData().");
}
public static void PrintAll(MotionData data)
{
Console.WriteLine("");
Console.WriteLine("Finger ID: " + data.finger_id);
Console.WriteLine("Knuckle ID: " + data.knuckle_id);
Console.WriteLine("xAcc: " + data.xAcc);
Console.WriteLine("yAcc: " + data.yAcc);
Console.WriteLine("zAcc: " + data.zAcc);
Console.WriteLine("xGyro: " + data.xGyro);
Console.WriteLine("yGyro: " + data.yGyro);
Console.WriteLine("zGyro: " + data.zGyro);
Console.WriteLine("xMag: " + data.xMag);
Console.WriteLine("yMag: " + data.yMag);
Console.WriteLine("zMag: " + data.zMag);
Console.ReadKey();
}
static void Main(string[] args)
{
Console.WriteLine("in main");
USBThread usb = new USBThread();
Calibrator calibrate = new Calibrator();
glove = new USB();
glove.Open(1152000, 1000, 1000);
string path = "C:\\Users\\sam_n\\Documents\\offsets.txt";
Console.WriteLine("Everything is initialized!");
_Calibrate();
calibrate.CollectAccelGyroData();
_MagCalibrate();
calibrate.CollecMagData();
calibrate.CalibrateOffsets();
using (StreamWriter w = new StreamWriter(path))
{
Console.WriteLine("Entering writer.");
for (int i = 0; i < TOTAL_FINGER_REFS; i++)
{
if (i == (int)Finger.HAND)
{
w.Write("Finger_ID: " + i + " \n"
+ "Knuckle_ID: " + 1 + " \n"
+ "Accel_Offset_x_y_z: " + calibrate.accelOffset[i, 1].x + " " + calibrate.accelOffset[i, 1].y + " " + calibrate.accelOffset[i, 1].z + " \n"
+ "Gyro_Offset_x_y_z: " + calibrate.gyroOffset[i, 1].x + " " + calibrate.gyroOffset[i, 1].y + " " + calibrate.gyroOffset[i, 1].z + " \n"
+ "Mag_offset_x_y_z: " + calibrate.magOffset[i, 1].x + " " + calibrate.magOffset[i, 1].y + " " + calibrate.magOffset[i, 1].z + " \n");
}
else
{
for (int j = 0; j < TOTAL_KNUCKLE_REFS; j++)
{
w.Write("Finger_ID: " + i + " \n"
+ "Knuckle_ID: " + j + " \n"
+ "Accel_Offset_x_y_z: " + calibrate.accelOffset[i, j].x + " " + calibrate.accelOffset[i, j].y + " " + calibrate.accelOffset[i, j].z + " \n"
+ "Gyro_Offset_x_y_z: " + calibrate.gyroOffset[i, j].x + " " + calibrate.gyroOffset[i, j].y + " " + calibrate.gyroOffset[i, j].z + " \n"
+ "Mag_offset_x_y_z: " + calibrate.magOffset[i, j].x + " " + calibrate.magOffset[i, j].y + " " + calibrate.magOffset[i, j].z + " \n");
}
}
}
Console.WriteLine("Exiting writer.");
Console.WriteLine("Done getting calibration data!");
}
bool end = false;
string path2 = "C:\\Users\\sam_n\\Documents\\lmao.txt";
Task check = new Task(() =>
{
using (StreamWriter w = new StreamWriter(path2))
{
Console.WriteLine("entering loop");
while (GlobalQueue.GetSize(GlobalQueue.Queue) > 1000)
{
MotionData data = GlobalQueue.Get(GlobalQueue.Queue);
w.Write(data.finger_id + " " + data.knuckle_id + " "
+ data.xAcc + " " + data.yAcc + " " + data.zAcc + " "
+ data.xGyro + " " + data.yGyro + " " + data.zGyro + " "
+ data.xMag + " " + data.yMag + " " + data.zMag + " " + "\n");
}
Console.WriteLine("done getting dummy data.");
glove.Close();
Console.ReadKey();
}
});
Task endTask = new Task(() =>
{
Console.WriteLine("DO SOME ACTION");
var startTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromMilliseconds(2 * WAIT_TIMER))
{
glove.Read(GlobalQueue.Queue);
}
});
endTask.Start();
endTask.Wait();
check.Start();
check.Wait();
}
