// ------------------- forward kinematics -------------------------
public Coord3D getCoord()
{
int baseAngle = robotBase.getCurrAngle();
int shoulderAngle = shoulder.getCurrAngle();
int elbowAngle = elbow.getCurrAngle();
// wristAngle not needed
if (!robotBase.isInRange(baseAngle))
Console.WriteLine(" base angle (" + baseAngle + ") out of range");
if (!shoulder.isInRange(shoulderAngle))
Console.WriteLine(" shoulder angle (" + shoulderAngle + ") out of range");
if (!elbow.isInRange(elbowAngle))
Console.WriteLine(" elbow angle (" + elbowAngle + ") out of range");
double baseAng = baseAngle * Math.PI / 180.0; // to radians
double shoulderAng = shoulderAngle * Math.PI / 180.0;
double elbowAng = elbowAngle * Math.PI / 180.0;
int seAngle = 180 - (shoulderAngle + elbowAngle);
double seAng = seAngle * Math.PI / 180.0;
double radialDist = UPPER_ARM*Math.Sin(shoulderAng) +
LOWER_ARM*Math.Sin(seAng) + GRIPPER_LEN;
int x = (int)Math.Round( radialDist*Math.Sin(baseAng) );
int y = (int)Math.Round( radialDist*Math.Cos(baseAng) );
int z = (int)Math.Round( BASE_HEIGHT + UPPER_ARM*Math.Cos(shoulderAng) -
LOWER_ARM*Math.Cos(seAng) );
Coord3D pt = new Coord3D(x, y, z);
// Console.WriteLine("getCoord() pos: " + pt);
return pt;
}
// moves object from one coord to another
private static void shiftItem(RobotArm robotArm)
{
Coord3D fromPt = new Coord3D(150, 150, 65); // in mm
Coord3D toPt = new Coord3D(-150, 200, 65);
robotArm.moveItem(fromPt, toPt);
}
// ------------------- inverse kinematics -------------------------
public bool moveTo(Coord3D pt)
{
return moveTo(pt.X, pt.Y, pt.Z);
}
public bool moveToComposite(Coord3D pt)
{
{ return moveToComposite(pt.X, pt.Y,pt.Z); }
}
// -------------------------- item movement --------------------------
public void moveItem(Coord3D fromPt, Coord3D toPt)
{
bool hasMoved = moveTo(fromPt);
showAngles();
Console.WriteLine("From Coord: " + getCoord() );
if (hasMoved) {
openGripper(false);
turnByOffset(JointID.ELBOW, -30); // so off the floor
Console.WriteLine("Off-floor Coord: " + getCoord() );
moveTo(toPt); // in mm; no checking of result
showAngles();
Console.WriteLine("To Coord: " + getCoord() );
openGripper(true);
}
}
