public void TestMedianWithNull()
{
Filter filter = new Filter();
ArmAngles[] armAngle = new ArmAngles[3];
armAngle[0] = new ArmAngles(1.5f, 0f, 0f, 0f, 0f, 0f, 0f, 0f);
armAngle[1] = new ArmAngles(1.0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f);
filter.addCurrentArmPos(armAngle[0]);
filter.addCurrentArmPos(armAngle[1]);
ArmAngles median = filter.getArmValue();
Assert.AreEqual(1.0f, median.shoulderPitch_Right);
}
public void addCurrentArmPos()
{
Filter filter = new Filter();
ArmAngles[] armAngle = new ArmAngles[3];
armAngle[0] = new ArmAngles(1.5f, 0f, 0f, 0f, 0f, 0f, 0f, 0f);
armAngle[1] = new ArmAngles(1.0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f);
armAngle[2] = new ArmAngles(10.0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f);
filter.addCurrentArmPos(armAngle[0]);
Assert.AreEqual(armAngle[0].shoulderPitch_Right, filter.armAngle[2].shoulderPitch_Right);
Assert.AreEqual(null, filter.armAngle[1]);
Assert.AreEqual(null, filter.armAngle[0]);
filter.addCurrentArmPos(armAngle[1]);
Assert.AreEqual(armAngle[0].shoulderPitch_Right, filter.armAngle[1].shoulderPitch_Right);
Assert.AreEqual(armAngle[1].shoulderPitch_Right, filter.armAngle[2].shoulderPitch_Right);
Assert.AreEqual(null, filter.armAngle[0]);
filter.addCurrentArmPos(armAngle[2]);
Assert.AreEqual(armAngle[0].shoulderPitch_Right, filter.armAngle[0].shoulderPitch_Right);
Assert.AreEqual(armAngle[1].shoulderPitch_Right, filter.armAngle[1].shoulderPitch_Right);
Assert.AreEqual(armAngle[2].shoulderPitch_Right, filter.armAngle[2].shoulderPitch_Right);
}
public void TestshiftAngleArray()
{
Filter filter = new Filter();
ArmAngles[] armAngle = new ArmAngles[3];
armAngle[0] = new ArmAngles(1.5f, 0f, 0f, 0f, 0f, 0f, 0f, 0f);
armAngle[1] = new ArmAngles(1.0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f);
armAngle[2] = new ArmAngles(10.0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f);
filter.armAngle = armAngle;
ArmAngles[] shifted = filter.shiftAngleArray();
Assert.AreEqual(1.0f, shifted[0].shoulderPitch_Right);
Assert.AreEqual(10.0f, shifted[1].shoulderPitch_Right);
Assert.AreEqual(null, shifted[2]);
}
