/// <summary>
/// Make "Steering Angle Packe(for setting mode)" ready to send
/// </summary>
public void SetSteeringAngle(WheelPos ws, double angle)
{
int n;
byte[] buff;
n = g_SCI2_TX_LEN;
Tx_Buff[n++] = ( int )MessageType.SteerAngleChanged; //Type Of Data
Tx_Buff[n++] = 14; //Packet Length
buff = BitConverter.GetBytes(( int )ws);
Tx_Buff[n++] = buff[0]; //Set Wheel Pos
Tx_Buff[n++] = buff[1]; //Set Wheel Pos
Tx_Buff[n++] = buff[2]; //Set Wheel Pos
Tx_Buff[n++] = buff[3]; //Set Wheel Pos
buff = BitConverter.GetBytes(angle);
Tx_Buff[n++] = buff[0]; //Set Angle
Tx_Buff[n++] = buff[1]; //Set Angle
Tx_Buff[n++] = buff[2]; //Set Angle
Tx_Buff[n++] = buff[3]; //Set Angle
Tx_Buff[n++] = buff[4]; //Set Angle
Tx_Buff[n++] = buff[5]; //Set Angle
Tx_Buff[n++] = buff[6]; //Set Angle
Tx_Buff[n++] = buff[7]; //Set Angle
g_SCI2_TX_LEN = n;
}
// Return wheel index based on axle and position
public override int GetWheelIndex(int axle, WheelPos position = WheelPos.Default)
{
if (axle < 0 || axle > 3)
{
return(-1);
}
int wheel = (int)position;
if (wheel < 0)
{
wheel = 0;
}
else if (wheel >= 2)
{
wheel = 1;
}
return(wheel * 4 + axle);
}