public AnglePositions(AnglePositions angles)
{
LeftShoulderAlong = angles.LeftShoulderAlong;
LeftElbowOut = angles.LeftElbowOut;
LeftShoulderOut = angles.LeftShoulderOut;
LeftElbowAlong = angles.LeftElbowOut;
RightElbowAlong = angles.RightElbowAlong;
RightElbowOut = angles.RightElbowOut;
RightShoulderAlong = angles.RightShoulderAlong;
RightShoulderOut = angles.RightShoulderOut;
}
/// <summary>
/// Convert the AngleSet object to an AnglePositions object.
/// </summary>
/// <returns>The AnglePositions object</returns>
public AnglePositions toAnglePositions()
{
AnglePositions angles = new AnglePositions();
angles.RightElbowOut = (int)rightElbow2;
angles.RightElbowAlong = (int)rightElbow1;
angles.RightShoulderOut = (int)rightShoulder2;
angles.RightShoulderAlong = (int)rightShoulder1;
angles.LeftElbowOut = (int)leftElbow2;
angles.LeftElbowAlong = (int)leftElbow1;
angles.LeftShoulderOut = (int)leftShoulder2;
angles.LeftShoulderAlong = (int)leftShoulder1;
return angles;
}
public void kinectAngles(AnglePositions angles)
{
//if (!ready)
// return;
double[] prevAngles = convert(CollisionRestrictor.INSTANCE.getRealAngles());
prevAngles = convert(lastReturned);
Point3D[] target = computeKeyPositions(convert(angles));
double[] fds = new double[8];
for (int x = 0; x < 8; x++)
fds[x] = finiteDifference(prevAngles, x, target);
double lSquared = 0;
for (int x = 0; x < 8; x++)
lSquared += fds[x] * fds[x];
if (lSquared == 0)
{
CollisionRestrictor.INSTANCE.commitAngles(null); //signal nothing to change
return;
}
lSquared = -Math.Sqrt(lSquared);
for (int x = 0; x < 8; x++)
fds[x] = prevAngles[x] + fds[x] * INCREMENT / lSquared;
double[] perturbation = perturb(target, prevAngles);
double pError = computeError(perturbation, target);
double fdError = computeError(fds, target);
double prevError = computeError(prevAngles, target);
if (fdError <= prevError && fdError <= pError)
{
commit(fds);
}
else if (pError <= prevError)
{
commit(perturbation);
}
else
{
CollisionRestrictor.INSTANCE.commitAngles(null); //send nothing
}
}
public void SetAngles(AnglePositions ap)
{
SetAngle(RightArm.ShoulderAlong, ap.RightShoulderAlong);
SetAngle(RightArm.ShoulderOut, ap.RightShoulderOut);
SetAngle(RightArm.ElbowAlong, ap.RightElbowAlong);
SetAngle(RightArm.ElbowOut, ap.RightElbowOut);
SetAngle(RightArm.ShoulderAlong, ap.RightShoulderAlong);
SetAngle(RightArm.ShoulderOut, ap.RightShoulderOut);
SetAngle(RightArm.ElbowAlong, ap.RightElbowAlong);
SetAngle(RightArm.ElbowOut, ap.RightElbowOut);
}
/// <summary>
/// Return the angle position object represented by the dummy robot's current position.
/// (So thid does the conversion for you)
/// </summary>
/// <returns>The angle positions object</returns>
public AnglePositions getAngles()
{
AnglePositions angles = new AnglePositions();
angles.LeftShoulderAlong = (int)getMotor(MotorManager.LEFT_SHOULDER_MOTOR1);
angles.LeftShoulderOut = (int)getMotor(MotorManager.LEFT_SHOULDER_MOTOR2);
angles.LeftElbowAlong = (int)getMotor(MotorManager.LEFT_ELBOW_MOTOR1);
angles.LeftElbowOut = (int)getMotor(MotorManager.LEFT_ELBOW_MOTOR2);
angles.RightShoulderAlong = (int)getMotor(MotorManager.RIGHT_SHOULDER_MOTOR1);
angles.RightShoulderOut = (int)getMotor(MotorManager.RIGHT_SHOULDER_MOTOR2);
angles.RightElbowAlong = (int)getMotor(MotorManager.RIGHT_ELBOW_MOTOR1);
angles.RightElbowOut = (int)getMotor(MotorManager.RIGHT_ELBOW_MOTOR2);
return angles;
}
/// <summary>
/// Set the angles of the robot using an AnglesPosition object instead
/// </summary>
/// <param name="angles">The angles object you wish to use (null values will be ignored)</param>
public void setAngles(AnglePositions angles)
{
if (angles.LeftShoulderAlong != null) setMotor(MotorManager.LEFT_SHOULDER_MOTOR1, angles.LeftShoulderAlong.Value);
if (angles.LeftShoulderOut != null) setMotor(MotorManager.LEFT_SHOULDER_MOTOR2, angles.LeftShoulderOut.Value);
if (angles.LeftElbowAlong != null) setMotor(MotorManager.LEFT_ELBOW_MOTOR1, angles.LeftElbowAlong.Value);
if (angles.LeftElbowOut != null) setMotor(MotorManager.LEFT_ELBOW_MOTOR2, angles.LeftElbowOut.Value);
if (angles.RightShoulderAlong != null) setMotor(MotorManager.RIGHT_SHOULDER_MOTOR1, angles.RightShoulderAlong.Value);
if (angles.RightShoulderOut != null) setMotor(MotorManager.RIGHT_SHOULDER_MOTOR2, angles.RightShoulderOut.Value);
if (angles.RightElbowAlong != null) setMotor(MotorManager.RIGHT_ELBOW_MOTOR1, angles.RightElbowAlong.Value);
if (angles.RightElbowOut != null) setMotor(MotorManager.RIGHT_ELBOW_MOTOR2, angles.RightElbowOut.Value);
}
public AnglePositions MakeSafe(AnglePositions angles)
{
return angles;
}
private static AnglePositions newAnglePositions()
{
AnglePositions a = new AnglePositions();
a.LeftElbowAlong = 0;
a.LeftElbowOut = 0;
a.LeftShoulderAlong = 0;
a.LeftShoulderOut = 0;
a.RightElbowAlong = 0;
a.RightElbowOut = 0;
a.RightShoulderAlong = 0;
a.RightShoulderOut = 0;
return a;
}
private AnglePositions convert(double[] angles)
{
AnglePositions ap = new AnglePositions();
ap.LeftShoulderAlong = round(angles[0]);
ap.LeftShoulderOut = round(angles[1]);
ap.LeftElbowAlong = round(angles[2]);
ap.LeftElbowOut = round(angles[3]);
ap.RightShoulderAlong = round(angles[4]);
ap.RightShoulderOut = round(angles[5]);
ap.RightElbowAlong = round(angles[6]);
ap.RightElbowOut = round(angles[7]);
return ap;
}
private double[] convert(AnglePositions angles)
{
return new double[]{
convert(angles.LeftShoulderAlong),
convert(angles.LeftShoulderOut),
convert(angles.LeftElbowAlong),
convert(angles.LeftElbowOut),
convert(angles.RightShoulderAlong),
convert(angles.RightShoulderOut),
convert(angles.RightElbowAlong),
convert(angles.RightElbowOut)
};
}
private void commit(double[] angles)
{
list.AddLast(convert(angles));
if (list.Count > 5)
list.RemoveFirst();
lastReturned=newAnglePositions();
try
{
foreach (AnglePositions a in list)
{
lastReturned.LeftElbowAlong += a.LeftElbowAlong;
lastReturned.LeftElbowOut += a.LeftElbowOut;
lastReturned.LeftShoulderAlong += a.LeftShoulderAlong;
lastReturned.LeftShoulderOut += a.LeftShoulderOut;
lastReturned.RightElbowAlong += a.RightElbowAlong;
lastReturned.RightElbowOut += a.RightElbowOut;
lastReturned.RightShoulderAlong += a.RightShoulderAlong;
lastReturned.RightShoulderOut += a.RightShoulderOut;
}
lastReturned.LeftElbowAlong /= list.Count;
lastReturned.LeftElbowOut /= list.Count;
lastReturned.LeftShoulderAlong /= list.Count;
lastReturned.LeftShoulderOut /= list.Count;
lastReturned.RightElbowAlong /= list.Count;
lastReturned.RightElbowOut /= list.Count;
lastReturned.RightShoulderAlong /= list.Count;
lastReturned.RightShoulderOut /= list.Count;
CollisionRestrictor.INSTANCE.commitAngles(lastReturned);
}
catch (InvalidOperationException e)
{
//Abort this attempt
CollisionRestrictor.INSTANCE.commitAngles(null); //signal nothing to change
return;
//CollisionRestrictor.INSTANCE.commitAngles(lastReturned);
}
}