private static void ProcessReceivedData(byte[] recvBuf)
{
for (int i = 0; i < 124; i += 4)
{
if (recvBuf[i + 3] == EndOfDataTrunk)
{
DataTrunk dta = new DataTrunk(recvBuf[i], recvBuf[i + 1], recvBuf[i + 2]);
switch (dta.type)
{
case DataType.ResetDataTag:
break;
case DataType.HandlebarDataTag:
break;
case DataType.WheelDataTag:
break;
case DataType.ButtonDataTag:
break;
default:
break;
}
}
}
}
void ReceiveData()
{
byte[] recvBuf = new byte[128];
try
{
port.Read(recvBuf, 0, 128);
for (int i = 0; i < 124; i += 4)
{
if (recvBuf[i + 3] == EndOfDataTrunk)
{
DataTrunk dta = new DataTrunk(recvBuf[i], recvBuf[i + 1], recvBuf[i + 2]);
switch (dta.type)
{
case DataType.ResetDataTag:
return;
case DataType.HandlebarDataTag:
{
short angle = (short)(dta.byte1 | (dta.byte2 << 8));
float val = MathEx.PiOver2 * (float)angle / 256f;
float lstval = MathEx.PiOver2 * (float)lastAngle / 256f;
if (lastAngle != angle)
{
Manager.OnHandlebarRotated(val, val - lstval);
lastAngle = angle;
}
float SlowSwitchAngle = MathEx.Degree2Radian(20);
float FastSwitchAngle = MathEx.Degree2Radian(45);
if (val > SlowSwitchAngle && val < FastSwitchAngle)
{
if (itemLeftCoolDown-- <= 0)
{
Manager.OnItemMoveRight();
itemLeftCoolDown = 30;
}
}
else if (val < -SlowSwitchAngle && val > -FastSwitchAngle)
{
if (itemRightCoolDown-- <= 0)
{
Manager.OnItemMoveLeft();
itemRightCoolDown = 30;
}
}
else if (val >= FastSwitchAngle)
{
if (lstval > SlowSwitchAngle && lstval < FastSwitchAngle)
{
itemLeftCoolDown = 0;
}
if (itemLeftCoolDown-- <= 0)
{
Manager.OnItemMoveRight();
itemLeftCoolDown = 10;
}
}
else if (val <= -FastSwitchAngle)
{
if (lstval < -SlowSwitchAngle && lstval > -FastSwitchAngle)
{
itemRightCoolDown = 0;
}
if (itemRightCoolDown-- <= 0)
{
Manager.OnItemMoveLeft();
itemRightCoolDown = 10;
}
}
else
{
itemLeftCoolDown = 0;
itemRightCoolDown = 0;
}
break;
}
case DataType.WheelDataTag:
if (dataDt > float.Epsilon)
{
ushort wheel = (ushort)(dta.byte1 | (dta.byte2 << 8));
if (lastWheel != wheel)
{
if (wheel < lastWheel)
{
lastWheel = 0;
wheel = 0;
}
float deltaS = 0.001f * (wheel - lastWheel) * 1.75f;
if (deltaS > 0)
{
float v = deltaS / dataDt;
float actV;
qVelocity.Enqueue(v);
if (qVelocity.Count > 15)
{
qVelocity.Dequeue();
}
if (qVelocity.Count > 0)
{
actV = 0;
foreach (float lean in qVelocity)
{
actV += lean;
}
actV /= qVelocity.Count;
}
else
{
actV = v;
}
v = actV;
Manager.OnWheelSpeedChanged(v, v - lastSpeed, false);
//if (v > 1f)
//{
// if (lastEnterTime > 2)
// {
// Manager.OnEnter();
// lastEnterTime = 0;
// }
//}
//else
//{
// lastEnterTime += dataDt;
//}
lastSpeed = v;
lastWheel = wheel;
}
}
dataDt = 0;
}
break;
case DataType.ButtonDataTag:
int hp = dta.byte2 & (1 << 5);
if (hp != lastHPulse)
{
Manager.OnHeartPulse();
lastHPulse = hp;
}
if ((dta.byte2 & (0x80)) == 1)
{
short angle = 0;
if (lastAngle != angle)
{
float val = MathEx.PiOver2 * (float)angle / 256f;
float lstval = MathEx.PiOver2 * (float)lastAngle / 256f;
Manager.OnHandlebarRotated(val, val - lstval);
lastAngle = angle;
}
}
if ((dta.byte2 & ButtonTLId) == 0)
{
if (!isEnterPressed)
{
Manager.OnEnter();
isEnterPressed = true;
}
}
else
{
isEnterPressed = false;
}
if ((dta.byte2 & ButtonTRId) == 0)
{
if (!isEscPressed)
{
Manager.OnEscape();
isEscPressed = true;
}
}
else
{
isEscPressed = false;
}
if ((dta.byte2 & ButtonMLId) == 0)
{
if (!isVPressed)
{
Manager.OnViewChanged();
isVPressed = true;
}
}
else
{
isVPressed = false;
}
if ((dta.byte2 & ButtonMRId) != 0)
{
if (!isResetPressed)
{
//port.Write(ResetDataTag);
Manager.OnReset();
isResetPressed = true;
}
}
else
{
isResetPressed = false;
}
break;
default:
return;
}
}
}
}
catch (TimeoutException)
{
}
}
