//Shift Array right -> neues Element ist null
public ArmAngles[] shiftAngleArray()
{
ArmAngles[] newAr = new ArmAngles[armAngle.Length];
for (int i = 1; i < armAngle.Length; i++)
{
newAr[i - 1] = armAngle[i];
}
return(newAr);
}
public Filter()
{
for (int i = 0; i < filtersize; i++)
{
armAngle[i] = new ArmAngles(Angles.inRadian(90),
Angles.inRadian(90),
Angles.inRadian(180),
Angles.inRadian(90),
Angles.inRadian(90),
Angles.inRadian(90),
Angles.inRadian(180),
Angles.inRadian(90));
}
}
public Filter()
{
for (int i = 0; i < filtersize; i++)
{
armAngle[i] = new ArmAngles(Angles.inRadian(90),
Angles.inRadian(90),
Angles.inRadian(180),
Angles.inRadian(90),
Angles.inRadian(90),
Angles.inRadian(90),
Angles.inRadian(180),
Angles.inRadian(90));
}
}
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);
}
//Neue Armposition hinzufuegen, alte verwerfen
public void addCurrentArmPos(ArmAngles currentAnglePos)
{
this.armAngle = shiftAngleArray();
armAngle[filtersize - 1] = currentAnglePos;
}
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]);
}
//Neue Armposition hinzufuegen, alte verwerfen
public void addCurrentArmPos(ArmAngles currentAnglePos)
{
this.armAngle = shiftAngleArray();
armAngle[filtersize - 1] = currentAnglePos;
}
//Shift Array right -> neues Element ist null
public ArmAngles[] shiftAngleArray()
{
ArmAngles[] newAr = new ArmAngles[armAngle.Length];
for (int i = 1; i < armAngle.Length; i++)
newAr[i - 1] = armAngle[i];
return newAr;
}
// This method will be called when the thread is started.
public void DoWork()
{
float shoulderPitch_Right = 0.0f;
float shoulderRoll_Right = 0.0f;
float elbowRoll_Right = 0.0f;
float elbowYaw_Right = 0.0f;
float shoulderPitch_Left = 0.0f;
float shoulderRoll_Left = 0.0f;
float elbowRoll_Left = 0.0f;
float elbowYaw_Left = 0.0f;
while (!_shouldStop)
{
if (currentSkeleton == null)
{
continue;
}
//Hier werden Berechnungen mit CURRENTSKELETON (EVTL PUFFER!!!) getätigt
//BERECHNE WINKEL a-d
//updateAngles
//Sleep
//Zuverlässigkeit der Joints
//filter.addCurrentArmPos()
//Right
shoulderPitch_Right = AngleCalculation.getShoulderPitch_Right(currentSkeleton);
shoulderRoll_Right = AngleCalculation.getShoulderRoll_Right(currentSkeleton);
elbowRoll_Right = AngleCalculation.getElbowRoll_Right(currentSkeleton);
elbowYaw_Right = AngleCalculation.getElbowYaw_Right(currentSkeleton);
//Left
shoulderPitch_Left = AngleCalculation.getShoulderPitch_Left(currentSkeleton);
shoulderRoll_Left = AngleCalculation.getShoulderRoll_Left(currentSkeleton);
elbowRoll_Left = AngleCalculation.getElbowRoll_Left(currentSkeleton);
elbowYaw_Left = AngleCalculation.getElbowYaw_Left(currentSkeleton);
filter.addCurrentArmPos(
new ArmAngles(
shoulderPitch_Right,
shoulderRoll_Right,
elbowRoll_Right,
elbowYaw_Right,
shoulderPitch_Left,
shoulderRoll_Left,
elbowRoll_Left,
elbowYaw_Left)
);
ArmAngles filtered_values = filter.getArmValue();
//skeletonAngleHandler.updateAngles(shoulderPitch_Right, shoulderRoll_Right, elbowRoll_Right, elbowYaw_Right, shoulderPitch_Left, shoulderRoll_Left, elbowRoll_Left, elbowYaw_Left);
skeletonAngleHandler.updateAngles(
filtered_values.shoulderPitch_Right, filtered_values.shoulderRoll_Right, filtered_values.elbowRoll_Right, filtered_values.elbowYaw_Right,
filtered_values.shoulderPitch_Left, filtered_values.shoulderRoll_Left, filtered_values.elbowRoll_Left, filtered_values.elbowYaw_Left);
Thread.Sleep(100);
}
Console.WriteLine("Angle Calculation shutted down");
}
