USARBot.Soryu

class Soryu extends SkidSteeredRobot config(USARBot);

defaultproperties
{
    Wheels(0)=(Number=0,PowerType=Right_Powered);
    Wheels(1)=(Number=1,PowerType=Left_Powered);
    Wheels(2)=(Number=4,PowerType=Right_Powered);
    Wheels(3)=(Number=5,PowerType=Left_Powered);
    Wheels(4)=(Number=8,PowerType=Right_Powered);
    Wheels(5)=(Number=9,PowerType=Left_Powered);
    
    //--Other Robot's Properties
    bDebug=false
    StaticMesh=StaticMesh'USARSim_Vehicles_Meshes.Soryu.SoryuCar'
    DrawScale=0.23810   // 4.762*0.05
    DrawScale3D=(X=1.0,Y=1.0,Z=1.0)

    ChassisMass=0.5
    
    // Configuration Parameters. Note: these variables need to be correspond to your robot's model.
    WheelRadius=0.122           // Wheel Radius, in unreal units. Note: Value is in meters
    Dimensions=(X=1.210,Y=0.145,Z=0.122)    // X=Length=1.210m, Y=Width=0.145m, Z=Height=0.122m
    maxSpinSpeed=3.14           // Unknown. Maximum wheel's spin speed is 3.14 rad/sec = 180 deg/sec
    MaxNetUpdateInterval=5.0    // 0.01 in default

    TireRollFriction=10.000000
    TireLateralFriction=10.000000
    TireRollSlip=0.0600
    TireLateralSlip=0.0600000
    TireMinSlip=0.001000
    TireSlipRate=0.00050000
    TireSoftness=0.000020
    TireAdhesion=0.000000
    TireRestitution=0.000000

    MaxTorque=100.000000

    Begin Object Class=KarmaParamsRBFull Name=KParams0
        KInertiaTensor(0)=0.010000
        KInertiaTensor(3)=0.02000
        KInertiaTensor(5)=0.03000
        KCOMOffset=(X=0.000000,Z=0.000000)
        KLinearDamping=0.500000
        KAngularDamping=50.00000
        KStartEnabled=True
        bHighDetailOnly=False
        bClientOnly=False
        bKDoubleTickRate=True
        KFriction=2.500000
        Name="KParams0"
        KMass=2.8
    End Object
    KParams=KarmaParamsRBFull'USARBot.Soryu.KParams0'
    //--
}

var bool gotBDInfo;
var KDHinge FrontCarYaw, FrontCarPitch, RearCarYaw, RearCarPitch;

function ProcessCarInput()
{
    local int i;
    local float FrontCarPitchAng, CarPitchAngLimit, RearCarPitchAng;
    local float FrontCarYawAng, CarYawAngLimit, RearCarYawAng;
    local int FCP, FCY, RCP, RCY;   // FC stands for FrontCar, P is pitch and Y is Yaw.

    Super.ProcessCarInput();
    
    // number of parts as described in USARBot.ini
    FCY = 2;
    FCP = 3;
    RCY = 6;
    RCP = 7;
    
    // this block is executed just once
    if( !gotBDInfo ){
        for( i=0; i<Parts.length; i++ ){
            Log("Parts[" $i$ "]=" $ GetItemName(string(Parts[i])) );
            JointParts[i].BrakeTorque = 1000;   // set torque of every part very high in order to mimick a real Soryu
        }

        FrontCarYaw = KDHinge(Joints[FCY]);
        FrontCarPitch = KDHinge(Joints[FCP]);
        RearCarYaw = KDHinge(Joints[RCY]);
        RearCarPitch = KDHinge(Joints[RCP]);

        gotBDInfo = true;
    }
    
    // acquire pitch angles of FrontCar and RearCar
    FrontCarPitchAng = FrontCarPitch.KCurrentAngle;
    RearCarPitchAng = RearCarPitch.KCurrentAngle;
    FrontCarYawAng = FrontCarYaw.KCurrentAngle;
    RearCarYawAng = RearCarYaw.KCurrentAngle;

    // 45 degree is a physical limit of car pitch angle
    CarPitchAngLimit = converter.AngleToUU(pi/4);
    // 30 degree is a physical limit of car yaw angle
    CarYawAngLimit = converter.AngleToUU(pi/6);

    // constrain a moving range of a car joint physically
    // if an operator tries angle-control (position-control) then
    if( JointsControl[FCP].order== 0 ){
        if( JointsControl[FCP].value > CarPitchAngLimit )
            JointsControl[FCP].value = CarPitchAngLimit;
        else if( JointsControl[FCP].value < -CarPitchAngLimit )
            JointsControl[FCP].value = -CarPitchAngLimit;
    }
    if( JointsControl[FCY].order== 0 ){
        if( JointsControl[FCY].value > CarYawAngLimit )
            JointsControl[FCY].value = CarYawAngLimit;
        else if( JointsControl[FCY].value < -CarYawAngLimit )
            JointsControl[FCY].value = -CarYawAngLimit;
    }
    if( JointsControl[RCP].order== 0 ){
        if( JointsControl[RCP].value > CarPitchAngLimit )
            JointsControl[RCP].value = CarPitchAngLimit;
        else if( JointsControl[RCP].value < -CarPitchAngLimit )
            JointsControl[RCP].value = -CarPitchAngLimit;
    }
    if( JointsControl[RCY].order== 0 ){
        if( JointsControl[RCY].value > CarYawAngLimit )
            JointsControl[RCY].value = CarYawAngLimit;
        else if( JointsControl[RCY].value < -CarYawAngLimit )
            JointsControl[RCY].value = -CarYawAngLimit;
    }


    // if an operator tries speed-control then
    // if the pitch angle of a car exceeds CarPitchAngLimit, stop them
    if( FrontCarPitchAng > CarPitchAngLimit && JointsControl[FCP].order==1 ) {
        if( JointsControl[FCP].value<0 )
            setSpinSpeed( FCP, 0 );
    }
    else if( FrontCarPitchAng < -CarPitchAngLimit && JointsControl[FCP].order==1 ) {
        if( JointsControl[FCP].value>0 )
            setSpinSpeed( FCP, 0 );
    }
    if( RearCarPitchAng > CarPitchAngLimit && JointsControl[RCP].order==1 ) {
        if( JointsControl[RCP].value<0 )
            setSpinSpeed( RCP, 0 );
    }
    else if( RearCarPitchAng < -CarPitchAngLimit && JointsControl[RCP].order==1 ) {
        if( JointsControl[RCP].value>0 )
            setSpinSpeed( RCP, 0 );
    }
    
    // if yaw the angle of a car exceeds CarSteerAngLimit, stop them
    if( FrontCarYawAng > CarYawAngLimit ) {
        if( JointsControl[FCY].order==1 && JointsControl[FCY].value<0 ) // if car joint intends to bend more than a limit
            setSpinSpeed( FCY, 0 );
    }
    else if( FrontCarYawAng < -CarYawAngLimit ) {
        if( JointsControl[FCY].order==1 && JointsControl[FCY].value>0 ) // if car joint intends to bend more than a limit
            setSpinSpeed( FCY, 0 );
    }
    if( RearCarYawAng > CarYawAngLimit ) {
        if( JointsControl[RCY].order==1 && JointsControl[RCY].value<0 ) // if car joint intends to bend more than a limit
            setSpinSpeed( RCY, 0 );
    }
    else if( RearCarYawAng < -CarYawAngLimit ) {
        if( JointsControl[RCY].order==1 && JointsControl[RCY].value>0 ) // if car joint intends to bend more than a limit
            setSpinSpeed( RCY, 0 );
    }
}