USARBot.EncoderSensor

class EncoderSensor extends sensor config (USARBot);

/**
    This class simulates the encoder sensor.

    The added config params are:
      Resolution:   The resoulution of the encoder defined in UU.

    The returned value is:
      Tick: The data range is (-65535/uuResolution)~(65535/uuResolution).
            Positive value means clockwise direction. It's the controller's
            resposible to count how many circles the part has rotated.
*/

var USARRemoteBot rBot;

var config float Resolution;

var int uuResolution;
var string tickData;
var int myTick;
var vector axis1,axis2;
var int lastAng, curAng;
var int lastBaseAng, curBaseAng;
var float totalAng, myAng, baseAng;
var Actor parBase;

function Init(String SName, Actor parent, vector position, rotator direction, KVehicle platform, name mount)
{
    Super.Init(SName, parent, position, direction, platform, mount);

    axis1 = vector(direction); // the turning axis
    axis2 = vector(direction+rot(16384,0,0));

    if (parent.Owner!=None)
        parBase = parent.Owner;
    else if (parent.Base!=None)
        parBase = parent.Base;
    else
        parBase = None;
}

function ConvertParam(USARConverter converter)
{
    if (converter!=None)
        uuResolution = converter.AngleToUU(Resolution);
    else
        uuResolution = Resolution;
}

function int getTick()
{
    local vector newAxis1,newAxis2,lastAxis;
    local Quat relQ, curQ;
    local float difAng,difCos,difSign;

    // my angle
    curQ = base.KGetRBQuaternion();
    newAxis1 = QuatRotateVector(curQ,axis1);
    newAxis2 = QuatRotateVector(curQ,axis2);

    relQ = QuatFindBetween(newAxis1,axis1);
    lastAxis = QuatRotateVector(relQ,newAxis2);

    difCos = lastAxis Dot axis2;
    difCos = int((difCos*10000+5))/10000.0;
    if (difCos>1.0) difCos = 1.0;
    if (difCos<-1.0) difCos = -1.0;

    difSign = (lastAxis Cross axis2) Dot axis1;
    if (difSign<0) difSign=-1.0;
    else difSign=1.0;

    curAng = difSign * ACos(difCos)*32768/PI;
    difAng = curAng - lastAng;
    lastAng = curAng;
    if (difAng>=32768)
        difAng = difAng - 65535;
    else if (difAng<-32768)
        difAng = difAng + 65535;

    myAng += difAng;

    // base's angle
    if (parBase!=None) {
        curQ = parBase.KGetRBQuaternion();
        newAxis1 = QuatRotateVector(curQ,axis1);
        newAxis2 = QuatRotateVector(curQ,axis2);

        relQ = QuatFindBetween(newAxis1,axis1);
        lastAxis = QuatRotateVector(relQ,newAxis2);

        difCos = lastAxis Dot axis2;
        difCos = int((difCos*10000+5))/10000.0;
        if (difCos>1.0) difCos = 1.0;
        if (difCos<-1.0) difCos = -1.0;

        difSign = (lastAxis Cross axis2) Dot axis1;
        if (difSign<0) difSign=-1.0;
        else difSign=1.0;

        curBaseAng = difSign * ACos(difCos)*32768/PI;
        difAng = curBaseAng - lastBaseAng;
        lastBaseAng = curBaseAng;
        if (difAng>=32768)
            difAng = difAng - 65535;
        else if (difAng<-32768)
            difAng = difAng + 65535;

        baseAng += difAng;
}

    totalAng = normalAngle(myAng - baseAng);
    if (totalAng!=(myAng - baseAng)) {
        myAng = normalAngle(myAng);
        baseAng = normalAngle(baseAng);
    }

    //log(ItemName@"dif:"$myAng@baseAng@totalAng@base.Rotation@base.Owner.Rotation);

    totalAng += RandRange(-Noise, Noise)*totalAng;
    myTick = int(normalAngle(totalAng)/uuResolution);
    return myTick;
}

function float normalAngle(float ang) {
    if (ang>65536) ang-=65536;
    if (ang<-65536) ang+=65536;
    return ang;
}

function string Set(String opcode, String args)
{
    if(Caps(opcode)=="RESET") {
        myAng = 0;
        baseAng = 0;
        totalAng = 0;
        myTick = 0;
        return "OK";
    }
    return "Failed";
    }

function String GetData()
{
    local string outstring;

    outstring = "{Name "$ItemName$" Tick "$getTick()$"}";

    return outstring;
}

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

defaultproperties
{
//Scaled with 4.762 at Mon Sep 25 14:21:52 EDT 2006
    bUseGroup=True
    ItemType="Encoder"
    HiddenSensor=false
    Resolution=0.01745 // in rad. It equals 1 deg
    DrawScale=0.0047620004
    DrawScale3D=(X=0.001)
    StaticMesh=StaticMesh'USARSim_VehicleParts_Meshes.Sensors.Sensor'
    Noise=0.001
    Mass=0.001
}