public static FTPoint At(double time, double real_tunable_param, FTData input, FTData expectedOutput)
{
FTPoint pt;
pt.time = time;
pt.real_tunable_param = real_tunable_param;
pt.input = input;
pt.expectedOutput = expectedOutput;
return(pt);
}
void BackgroundExecThread()
{
int force_scale = 1000000; // Divider scale for newtons
int torque_scale = 1000000; // Divider scale for newton-meters
// Spec calls for this specific header
UInt16 command_header = 0x3412;
UInt16 command = 0x0200; // Request a single measurement
UInt32 sample_count = 0x01000000;
Byte[] datagram = new Byte[8];
Byte[] received = new Byte[32];
//Populate the outgoing datagram. See the F/T manual for more info
Array.Copy(BitConverter.GetBytes(command_header), 0, datagram, 0, 2);
Array.Copy(BitConverter.GetBytes(command), 0, datagram, 2, 2);
Array.Copy(BitConverter.GetBytes(sample_count), 0, datagram, 4, 4);
IPEndPoint ep = null;
long ticksPerSec;
long loop_start_time;
long loop_time;
QueryPerformanceFrequency(out ticksPerSec);
while (running)
{
QueryPerformanceCounter(out loop_start_time);
client.Send(datagram, 8); // Send UDP packet
//Receive and unpack data
try
{
received = client.Receive(ref ep);
Array.Reverse(received);
lock (wr)
{
wr[0] = (double)(BitConverter.ToInt32(received, 20)) / force_scale;
wr[1] = (double)(BitConverter.ToInt32(received, 16)) / force_scale;
wr[2] = (double)(BitConverter.ToInt32(received, 12)) / force_scale;
wr[3] = (double)(BitConverter.ToInt32(received, 8)) / torque_scale;
wr[4] = (double)(BitConverter.ToInt32(received, 4)) / torque_scale;
wr[5] = (double)(BitConverter.ToInt32(received, 0)) / torque_scale;
}
}
catch (Exception e)
{
//Console.WriteLine(e.Message);
}
if (recording_data == 1)
{
FTData o = new FTData();
o.time = (loop_start_time - record_start_time);
o.ft_data = wr;
try
{
_FTDataStreamBroadcaster.AsyncSendPacket(o, () => { });
}
catch { }
//string wr_line = "";
//wr_line += (loop_start_time - record_start_time) / (ticksPerSec / 1000);
//for (int n = 0; n < 6; n++)
// wr_line += ", " + wr[n];
//sw.WriteLine(wr_line);
}
QueryPerformanceCounter(out loop_time);
long tMillisec = (loop_time - loop_start_time) / (ticksPerSec / 1000);
while (tMillisec < 10)
{
QueryPerformanceCounter(out loop_time);
tMillisec = (loop_time - loop_start_time) / (ticksPerSec / 1000);
}
}
}
