USARBot.Gripper

class Gripper extends Effecter;

var config float MaxAngle;
var config float MinAngle;
var config string LeftFinger, RightFinger; 
var int idLF, idRF;
var int idLF2, idRF2;
var KRobot robot;

function Init(String SName, Actor parent, vector position, rotator direction, KVehicle platform, name mount)
{
    local int i;
    local array<int> children;
    
    Super.Init(SName,parent,position,direction,platform,mount);
    robot = KRobot(platform);
    if (robot!=None) {
        if (LeftFinger=="" || RightFinger=="") {
            // find the left and right fingers
            for (i=0;i<robot.Joints.length;i++) {
                if (robot.Parents[i]==parent) {
                    if (robot.JointParts[i].ParentPos.Y<0 && LeftFinger=="")
                        idLF = i;
                    else if (robot.JointParts[i].ParentPos.Y>0 && RightFinger=="")
                        idRF = i;
                }
            }
        }
        if (idLF==-1)       
            idLF = robot.FindJointPartId(LeftFinger);
        if (idRF==-1)
            idRF = robot.FindJointPartId(RightFinger);
    }
    if (idLF==-1 || idRf==-1)
        log("Error LeftFinger/RightFinger name or incorrect Robot class");
    else {
        children = robot.FindChildren(idLF);
        if (children.length>0)
            idLF2 = children[0];
        children = robot.FindChildren(idRF);
        if (children.length>0)
            idRF2 = children[0];
        log("Use "$robot.JointParts[idLF].PartName$"/"$robot.JointParts[idRF].PartName$" as Left/Right Finger;");
    }
}

function string Set(String opcode, String args)
{
    local float angle;
    
    if (idLF==-1 || idRf==-1)
        return "Failed";

    if (Caps(opcode)=="OPEN") {
        angle = float(args);
        if (angle>MaxAngle)
            angle = MaxAngle;
        else if (angle<MinAngle)
            angle = MinAngle;
        robot.JointsControl[idLF].state = 1; // new command
        robot.JointsControl[idLF].steer = 0;
        robot.JointsControl[idLF].order = 0;
        if (converter!=None)
            robot.JointsControl[idLF].value = /*KDHinge(robot.Joints[idLF]).KCurrentAngle*/ converter.AngleToUU(angle/2);
        else
            robot.JointsControl[idLF].value = /*KDHinge(robot.Joints[idLF]).KCurrentAngle*/ angle/2; 
        robot.JointsControl[idRF].state = 1; // new command
        robot.JointsControl[idRF].steer = 0;
        robot.JointsControl[idRF].order = 0;
        if (converter!=None)
            robot.JointsControl[idRF].value = /*KDHinge(robot.Joints[idRF]).KCurrentAngle +*/ converter.AngleToUU(angle/2);
        else
            robot.JointsControl[idRF].value = /*KDHinge(robot.Joints[idRF]).KCurrentAngle +*/ angle/2;
        if (idLF2>=0 && idRF2>=0) {
            robot.JointsControl[idLF2].state = 1; // new command
            robot.JointsControl[idLF2].steer = 0;
            robot.JointsControl[idLF2].order = 0;
            robot.JointsControl[idLF2].value = -1*robot.JointsControl[idLF].value;
            robot.JointsControl[idRF2].state = 1; // new command
            robot.JointsControl[idRF2].steer = 0;
            robot.JointsControl[idRF2].order = 0;
            robot.JointsControl[idRF2].value = -1*robot.JointsControl[idRF].value;
        }
        robot.bNewCommand = true;
        
        return "OK";
    }
    return "Failed";
}

function String GetConfData()
{
    local string outstring;
    outstring = Super.GetConfData();
    outstring = outstring@"{MaxAngle "$MaxAngle$"}";
    return outstring;
}

defaultproperties
{
//Scaled with 4.762 at Mon Sep 25 14:21:51 EDT 2006
    idLF=-1
    idRF=-1
    idLF2=-1
    idRF2=-1
    ItemType="Gripper"
    MaxAngle=1.57
    MinAngle=0
    drawscale=0.4762
}