public void CanGetMovmentFromLeftFrontLeg()
{
Leg leg = new Leg(1,1)
{
Side = Side.Left,
Position = Position.Front,
Offset = 00,
X = 5.7,
Y = 10.4,
Z = -9.8726896031426,
Coxa = new ServoBase(3.8, 60, 1, 0, 0, Side.Left),
Femur = new ServoBase(7.6, -90, 3, 0, 0, Side.Left),
Tibia = new ServoBase(10.4, -70, 5, 0, 0, Side.Left)
};
Leg.CalculateNewPosition(leg, 0,1);
MovmentComandAX12[] movement = leg.GetMovements();
List<byte> movmentInBytes;
//coxa
movmentInBytes = (List<byte>)movement[0].ToByte();
Assert.AreEqual(1, movmentInBytes[0]); //ServoId
Assert.AreEqual(0x1FF, BitConverter.ToInt16(movmentInBytes.ToArray(), 1));
Assert.AreEqual(0x050, BitConverter.ToInt16(movmentInBytes.ToArray(), 3));
//femur
movmentInBytes = (List<byte>)movement[1].ToByte();
Assert.AreEqual(3, movmentInBytes[0]); //ServoId
Assert.AreEqual(0x1FF, BitConverter.ToInt16(movmentInBytes.ToArray(), 1));
Assert.AreEqual(0x050, BitConverter.ToInt16(movmentInBytes.ToArray(), 3));
//Coxa
movmentInBytes = (List<byte>)movement[2].ToByte();
Assert.AreEqual(5, movmentInBytes[0]); //ServoId
Assert.AreEqual(0x1BB, BitConverter.ToInt16(movmentInBytes.ToArray(), 1));
Assert.AreEqual(0x050, BitConverter.ToInt16(movmentInBytes.ToArray(), 3));
}
