public ForceSensorTestHandlerDouble(WebSocketObjectHolder webSocketObjectHolder) : base(webSocketObjectHolder)
{
DateTime s = DateTime.Now;
this.testLoop = Task.Run(async() => {
this.isStop = false;
var random = new Random();
var seed = 2.0;
while (!this.isStop)
{
var randomFArray = Enumerable.Range(0, 6).Select(i => Convert.ToSingle(seed * random.NextDouble() - seed / 2.0)).ToArray();
var data = new ForceSensorData
{
Fx = randomFArray[0],
Fy = randomFArray[1],
Fz = randomFArray[2],
Mx = randomFArray[3],
My = randomFArray[4],
Mz = randomFArray[5],
};
var res = new ForceSensorResponceDouble
{
Sensor1 = data,
Sensor2 = data,
TimeStamp = Convert.ToUInt32((DateTime.Now - s).TotalMilliseconds),
};
var resStr = JsonConvert.SerializeObject(res);
await SendMessageToAllAsync(resStr, AppConst.NON_BOM_UTF8_ENCORDING);
System.Threading.Thread.Sleep(10);
}
});
}
/// <summary>
/// コントローラーのステータスを全体へ通知
/// </summary>
/// <param name="data"></param>
/// <returns></returns>
private void ForceSensor_SensorDataUpdated(ForceSensorUpdateArgs data)
{
ForceSensorData sensor1 = (data.Sensor1Data == null) ? new ForceSensorData() : data.Sensor1Data;
ForceSensorData sensor2 = (data.Sensor2Data == null) ? new ForceSensorData() : data.Sensor2Data;
if (sensorCount < meanTimes)
{
meanbuf1fx[sensorCount] = sensor1.Fx;
meanbuf1fy[sensorCount] = sensor1.Fy;
meanbuf1fz[sensorCount] = sensor1.Fz;
meanbuf1mx[sensorCount] = sensor1.Mx;
meanbuf1my[sensorCount] = sensor1.My;
meanbuf1mz[sensorCount] = sensor1.Mz;
meanbuf2fx[sensorCount] = sensor2.Fx;
meanbuf2fy[sensorCount] = sensor2.Fy;
meanbuf2fz[sensorCount] = sensor2.Fz;
meanbuf2mx[sensorCount] = sensor2.Mx;
meanbuf2my[sensorCount] = sensor2.My;
meanbuf2mz[sensorCount] = sensor2.Mz;
sensorCount++;
if (sensorCount >= meanTimes)
{
sensor1Offset.Fx = meanbuf1fx.Average();
sensor1Offset.Fy = meanbuf1fy.Average();
sensor1Offset.Fz = meanbuf1fz.Average();
sensor1Offset.Mx = meanbuf1mx.Average();
sensor1Offset.My = meanbuf1my.Average();
sensor1Offset.Mz = meanbuf1mz.Average();
sensor2Offset.Fx = meanbuf2fx.Average();
sensor2Offset.Fy = meanbuf2fy.Average();
sensor2Offset.Fz = meanbuf2fz.Average();
sensor2Offset.Mx = meanbuf2mx.Average();
sensor2Offset.My = meanbuf2my.Average();
sensor2Offset.Mz = meanbuf2mz.Average();
}
}
else
{
var sensor1ForceVector = new Vector3(sensor1.Fx - sensor1Offset.Fx, sensor1.Fy - sensor1Offset.Fy, sensor1.Fz - sensor1Offset.Fz);
var sensor1MomentVector = new Vector3(sensor1.Mx - sensor1Offset.Mx, sensor1.My - sensor1Offset.My, sensor1.Mz - sensor1Offset.Mz);
var sensor2ForceVector = new Vector3(sensor2.Fx - sensor2Offset.Fx, sensor2.Fy - sensor2Offset.Fy, sensor2.Fz - sensor2Offset.Fz);
var sensor2MomentVector = new Vector3(sensor2.Mx - sensor2Offset.Mx, sensor2.My - sensor2Offset.My, sensor2.Mz - sensor2Offset.Mz);
q = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), (float)Math.PI);
// Quaternion q = new Quaternion(new Vector3(0, 1, 0), 180.0f);
// Y軸周りに180°回転
sensor2ForceVector = Vector3.Transform(sensor2ForceVector, q);
sensor2MomentVector = Vector3.Transform(sensor2MomentVector, q);
var combindForce = sensor1ForceVector + sensor2ForceVector;
var combindMoment = sensor1MomentVector + sensor2MomentVector;
var responce = new ForceSensorResponceDouble()
{
Sensor1 = new ForceSensorData()
{
Fx = sensor1ForceVector.X,
Fy = sensor1ForceVector.Y,
Fz = sensor1ForceVector.Z,
Mx = sensor1MomentVector.X,
My = sensor1MomentVector.Y,
Mz = sensor1MomentVector.Z,
},
Sensor2 = new ForceSensorData()
{
Fx = sensor2ForceVector.X,
Fy = sensor2ForceVector.Y,
Fz = sensor2ForceVector.Z,
Mx = sensor2MomentVector.X,
My = sensor2MomentVector.Y,
Mz = sensor2MomentVector.Z,
},
Combined = new ForceSensorData()
{
Fx = combindForce.X,
Fy = combindForce.Y,
Fz = combindForce.Z,
Mx = combindMoment.X,
My = combindMoment.Y,
Mz = combindMoment.Z,
},
//TimeStamp = data.MeasureTime,
TimeStamp = 0,
};
var resStr = JsonConvert.SerializeObject(responce);
// Console.WriteLine($"updated: {ii}\n{data.StatusCode}");
SendMessageToAllAsync(resStr, AppConst.NON_BOM_UTF8_ENCORDING);
}
}
/// <summary>
/// コントローラーのステータスを全体へ通知
/// </summary>
/// <param name="data"></param>
/// <returns></returns>
private async Task ForceSensor_SensorDataUpdated(ForceSensorUpdateArgs data)
{
ForceSensorData sensor1 = (data.Sensor1Data == null)? new ForceSensorData() : new ForceSensorData
{
Fx = data.Sensor1Data.Fx,
Fy = data.Sensor1Data.Fy,
Fz = data.Sensor1Data.Fz,
Mx = data.Sensor1Data.Mx,
My = data.Sensor1Data.My,
Mz = data.Sensor1Data.Mz,
};
ForceSensorData sensor2 = (data.Sensor2Data == null)? new ForceSensorData() : new ForceSensorData
{
Fx = data.Sensor2Data.Fx,
Fy = data.Sensor2Data.Fy,
Fz = data.Sensor2Data.Fz,
Mx = data.Sensor2Data.Mx,
My = data.Sensor2Data.My,
Mz = data.Sensor2Data.Mz,
};
var sensor1ForceVector = new Vector3(sensor1.Fx, sensor1.Fy, sensor1.Fz);
var sensor1MomentVector = new Vector3(sensor1.Mx, sensor1.My, sensor1.Mz);
var sensor2ForceVector = new Vector3(sensor2.Fx, sensor2.Fy, sensor2.Fz);
var sensor2MomentVector = new Vector3(sensor2.Mx, sensor2.My, sensor2.Mz);
Quaternion q = new Quaternion(new Vector3(0, 1, 0), (float)Math.PI);
// Y軸周りに180°回転
sensor2ForceVector = Vector3.Transform(sensor2ForceVector, q);
sensor2MomentVector = Vector3.Transform(sensor2MomentVector, q);
var combindForce = sensor1ForceVector + sensor2ForceVector;
var combindMoment = sensor1MomentVector + sensor2MomentVector;
var responce = new ForceSensorResponceDouble()
{
Sensor1 = new ForceSensorData()
{
Fx = sensor1ForceVector.X,
Fy = sensor1ForceVector.Y,
Fz = sensor1ForceVector.Z,
Mx = sensor1MomentVector.X,
My = sensor1MomentVector.Y,
Mz = sensor1MomentVector.Z,
},
Sensor2 = new ForceSensorData()
{
Fx = sensor2ForceVector.X,
Fy = sensor2ForceVector.Y,
Fz = sensor2ForceVector.Z,
Mx = sensor2MomentVector.X,
My = sensor2MomentVector.Y,
Mz = sensor2MomentVector.Z,
},
Combined = new ForceSensorData()
{
Fx = combindForce.X,
Fy = combindForce.Y,
Fz = combindForce.Z,
Mx = combindMoment.X,
My = combindMoment.Y,
Mz = combindMoment.Z,
},
//TimeStamp = data.Time,
//TimeStamp = DateTime.Now.ToFileTime(),
TimeStamp = DateTime.Now.ToFileTime(),
};
var resStr = JsonConvert.SerializeObject(responce);
// Console.WriteLine($"updated: {ii}\n{data.StatusCode}");
await SendMessageToAllAsync(resStr, AppConst.NON_BOM_UTF8_ENCORDING);
}