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)); }