void Update()
{
// If myDigitalArmControl has not been wired up on Awake exit
if (myDigitalArmControl == null)
{
return;
}
// Make sure X never goes under 0 and Y under 1 (arm design constraints)
if (xPos < 0.1f)
{
xPos = 0.1f;
}
if (yPos < 1)
{
yPos = 1;
}
// Converts cartesian coordinates into servo Angles using a dedicated library
ArmAngles myArmAngles = RobotArm.CartesianToServo(xPos, yPos, armLength);
// Write servo angles in myDigitalArmControl script
myDigitalArmControl.baseServoAngle = myArmAngles.baseAngle;
myDigitalArmControl.jointServoAngle = myArmAngles.jointAngle;
}
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 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 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]);
}
public InterpOperation(ArmController arm, Vector3D worldtarg, double WristPitch, double WristYaw)
{
this.arm = arm;
CurrentState = arm.GetCurrentState();
EndState = arm.FindAnglesState(worldtarg, WristPitch, WristYaw);
EndPos = worldtarg;
CurrentPos = arm.FindPos(out StartWristPitch, out StartWristYaw);
//CurrentPos += arm.wristYaw.WorldMatrix.Up * arm.headHeight;
stepAmount = stepSize / Vector3D.Distance(CurrentPos, EndPos);
// StartWristPitch = arm.wristPitch.Angle + arm.shoulderPitch.Angle - arm.Elbow.Angle;
// StartWristYaw = arm.wristYaw.Angle - arm.shoulderYaw.Angle;
EndWristPitch = WristPitch;
EndWristYaw = WristYaw;
}
public Vector3D FindPos(ArmAngles ang)
{
double C = Math.Sqrt((2 * armLength * armLength) - (2 * armLength * armLength * Math.Cos(ang.elbowAng - Math.PI)));
double deltaAng = ang.deltaAng;
double WristPitch = ang.wristPitchAng + ang.shoulderPitchAng - ang.elbowAng - Math.PI / 2;
double WristYaw = ang.wristYawAng - ang.shoulderYawAng;
double WristPitch1 = WristPitch + extensionOffsetAngle;
double wristOffset_x = Math.Cos(WristYaw) * Math.Cos(WristPitch1) * headLength + Math.Cos(WristYaw) * Math.Cos(WristPitch) * headHeight;
double wristOffset_z = Math.Sin(WristYaw) * Math.Cos(WristPitch1) * headLength + Math.Sin(WristYaw) * Math.Cos(WristPitch) * headHeight;
double wristOffset_y = Math.Sin(WristPitch1) * headLength + Math.Sin(WristPitch) * headHeight;
Vector3D output = new Vector3D(0, 0, 0);
output.X = Math.Cos(-ang.shoulderYawAng) * Math.Cos(deltaAng) * C + wristOffset_x;
output.Z = Math.Sin(-ang.shoulderYawAng) * Math.Cos(deltaAng) * C + wristOffset_z;
output.Y = Math.Sin(deltaAng) * C + wristOffset_y;
return output;
}
