/// <summary>
/// コントローラーのステータスを全体へ通知
/// </summary>
/// <param name="data"></param>
/// <returns></returns>
private void 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,
};
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;
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();
}
}
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 responce = new ForceSensorResponceSingle()
{
Sensor1 = new ForceSensorData()
{
Fx = sensor1ForceVector.X,
Fy = sensor1ForceVector.Y,
Fz = sensor1ForceVector.Z,
Mx = sensor1MomentVector.X,
My = sensor1MomentVector.Y,
Mz = sensor1MomentVector.Z,
},
Time = data.Time,
};
var resStr = JsonConvert.SerializeObject(responce);
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,
};
var sensor1ForceVector = new Vector3(sensor1.Fx, sensor1.Fy, sensor1.Fz);
var sensor1MomentVector = new Vector3(sensor1.Mx, sensor1.My, sensor1.Mz);
var responce = new ForceSensorResponceSingle()
{
Sensor1 = new ForceSensorData()
{
Fx = sensor1ForceVector.X,
Fy = sensor1ForceVector.Y,
Fz = sensor1ForceVector.Z,
Mx = sensor1MomentVector.X,
My = sensor1MomentVector.Y,
Mz = sensor1MomentVector.Z,
},
Time = data.Time,
};
var resStr = JsonConvert.SerializeObject(responce);
// Console.WriteLine($"updated: {ii}\n{data.StatusCode}");
await SendMessageToAllAsync(resStr, AppConst.NON_BOM_UTF8_ENCORDING);
Thread.Sleep(10);
}
public ForceSensorTestHandlerSingle(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 ForceSensorResponceSingle
{
Sensor1 = data,
Time = Convert.ToUInt32((DateTime.Now - s).TotalMilliseconds),
};
var resStr = JsonConvert.SerializeObject(res);
await SendMessageToAllAsync(resStr, AppConst.NON_BOM_UTF8_ENCORDING);
System.Threading.Thread.Sleep(10);
}
});
}
