USARBot.AckermanSteeredRobot

class AckermanSteeredRobot extends GroundVehicle config(USARBot);

// Structure to hold both the wheel index and the wheel's maximum steer angle
struct SteerData
{
    var int WheelNumber;
    var float maxSteer;
};

// Programming variables
var float FrontSteerSpeed, RearSteerSpeed, FrontSteerTorque, RearSteerTorque;
var float cachedVelocity, cachedFrontSteering, cachedRearSteering;
var bool gotRobotInfo;
var array<int> PoweredWheels;
var array<SteerData> FrontSteerWheels, RearSteerWheels;

function ProcessCarInput()
{
    local float InputVelocity,InputFrontSteering,InputRearSteering;
    local bool isCommandNormalized, noNeedToUpdateSpeed, noNeedToUpdateFrontSteer, noNeedToUpdateRearSteer;
    local int i;

    // Variable Initialization
    noNeedToUpdateSpeed = true;
    noNeedToUpdateFrontSteer = true;
    noNeedToUpdateRearSteer = true;

    Super.ProcessCarInput();

    // This section gets/sets various information about the robot (Note: this code only runs once)
    if(!gotRobotInfo)
    {
        // Get information about the robot's wheels
        for(i=0; i<Wheels.length; i++)
        {
            if(Wheels[i].PowerType == Left_Powered || Wheels[i].PowerType == Right_Powered) // Get all the powered wheels into a dynamic array
            {
                PoweredWheels.Insert(PoweredWheels.length, 1);            // Make space in the dynamic array to add a powered wheel
                PoweredWheels[PoweredWheels.length-1] = Wheels[i].Number; // Store the index number of the part in the dynamic array
            }

            if(Wheels[i].SteerType == Front_Steered) // Get all the front steered wheels into a dynamic array
            {
                FrontSteerWheels.Insert(FrontSteerWheels.length, 1);                            // Make space in the dynamic array to add a front-steered wheel
                FrontSteerWheels[FrontSteerWheels.length-1].WheelNumber = Wheels[i].Number;     // Store the index number of the part in the dynamic array
                FrontSteerWheels[FrontSteerWheels.length-1].maxSteer = Wheels[i].MaxSteerAngle; // Store the maximum steering angle in the dynamic array

                JointParts[Wheels[i].Number].bSteeringLocked = false; //Make sure the wheel is not locked (so that it can be steered)
            }
            else if(Wheels[i].SteerType == Rear_Steered) // Get all the rear steered wheels into a dynamic array
            {
                RearSteerWheels.Insert(RearSteerWheels.length, 1);                            // Make space in the dynamic array to add a rear-steered wheel
                RearSteerWheels[RearSteerWheels.length-1].WheelNumber = Wheels[i].Number;     // Store the index number of the part in the dynamic array
                RearSteerWheels[RearSteerWheels.length-1].maxSteer = Wheels[i].MaxSteerAngle; // Store the maximum steering angle in the dynamic array

                JointParts[Wheels[i].Number].bSteeringLocked = false; //Make sure the wheel is not locked (so that it can be steered)
            }
            else
            {
                JointParts[Wheels[i].Number].bSteeringLocked = true; //Make sure the wheel is locked (so that it cannot be steered)
            }
        }

        // Initialize the controller's properties
        USARRemoteBot(Controller).Normalized = false;
        USARRemoteBot(Controller).Speed = 0.0;
        USARRemoteBot(Controller).FrontSteer = 0.0;
        USARRemoteBot(Controller).RearSteer = 0.0;
        
        gotRobotInfo=true;

        // Section used for debugging purposes to see if the correct joints have been saved in the dynamic arrays
        if(bDebug)
        {
            for(i=0; i<PoweredWheels.Length; i++) Log("Powered Wheel #" $ i+1 $ ": " $ PoweredWheels[i]);

            for(i=0; i<FrontSteerWheels.Length; i++) Log("Front Steered Wheel #" $ i+1 $ ": " $ FrontSteerWheels[i].WheelNumber);

            for(i=0; i<RearSteerWheels.Length; i++) Log("Rear Steered Wheel #" $ i+1 $ ": " $ RearSteerWheels[i].WheelNumber);
        }
    }

    // Here, we deal with the robot's movement
    if (USARRemoteBot(Controller).bNewThrottle)
    {
        isCommandNormalized = USARRemoteBot(Controller).Normalized; // Get the Normalized value from the controller
        InputVelocity = USARRemoteBot(Controller).Speed;            // Get the wheel's spin speed value from the controller
        InputFrontSteering = USARRemoteBot(Controller).FrontSteer;  // Get the front steer value from the controller
        InputRearSteering = USARRemoteBot(Controller).RearSteer;    // Get the rear steer value from the controller

        // If a normalized drive command was received (e.g. the speed value is between -100 and 100)
        if (isCommandNormalized)
        {
            if(InputVelocity < -100)     InputVelocity = Converter.SpinSpeedToUU(-maxSpinSpeed);                       // If the controller's value is less than -100, we use the negative of the robot's maximum spin speed
        else if(InputVelocity > 100) InputVelocity = Converter.SpinSpeedToUU(maxSpinSpeed);                        // If the controller's value is more than 100, we use the robot's maximum spin speed
        else                         InputVelocity = (InputVelocity/100) * Converter.SpinSpeedToUU(maxSpinSpeed);  // If the controller's value is between -100 and 100, we use a percentage of the maximum spin speed
        }
        // If a non-normalized drive command was received (e.g. the speed is an absolute values, in radians per second)
        else
        {
            if(InputVelocity < -maxSpinSpeed)     InputVelocity = Converter.SpinSpeedToUU(-maxSpinSpeed); // If the controller's value is less than -(maxSpinSpeed), we use the negative of the robot's maximum spin speed
        else if(InputVelocity > maxSpinSpeed) InputVelocity = Converter.SpinSpeedToUU(maxSpinSpeed);  // If the controller's value is more than maxSpinSpeed, we use the robot's maximum spin speed
        else                                  InputVelocity = Converter.SpinSpeedToUU(InputVelocity); // Otherwise, we use the controller's value
    }

        // Here, we physically spin the appropriate wheels
        if((PoweredWheels.length!=0) && (cachedVelocity != InputVelocity))
        {
            // Spin the appropriate wheels
            for(i=0; i<PoweredWheels.length; i++)
            {
                setSpinSpeed(PoweredWheels[i], InputVelocity);
            }

            bNewCommand = true;
        }

        // Here, we turn the appropiate front-steered wheels, taking into account whether or not the drive command issued is normalized
        if((FrontSteerWheels.length!=0) && (cachedFrontSteering != InputFrontSteering))
        {
            for(i=0; i<FrontSteerWheels.length; i++)
            {
                // If a normalized drive command was received (e.g. the front-steer value is between -100 and 100)
                if (isCommandNormalized)
                {
                    if(InputFrontSteering < -100)     setAngle(FrontSteerWheels[i].WheelNumber,Converter.AngleToUU(-FrontSteerWheels[i].maxSteer));
                    else if(InputFrontSteering > 100) setAngle(FrontSteerWheels[i].WheelNumber,Converter.AngleToUU(FrontSteerWheels[i].maxSteer));
                    else                              setAngle(FrontSteerWheels[i].WheelNumber,((InputFrontSteering/100) * Converter.AngleToUU(FrontSteerWheels[i].maxSteer)));
                }
                // If a non-normalized drive command was received (e.g. the front-steer is an absolute values, in radians)
                else
                {
                    if(InputFrontSteering < -FrontSteerWheels[i].maxSteer)     setAngle(FrontSteerWheels[i].WheelNumber,Converter.AngleToUU(-FrontSteerWheels[i].maxSteer));
                    else if(InputFrontSteering > FrontSteerWheels[i].maxSteer) setAngle(FrontSteerWheels[i].WheelNumber,Converter.AngleToUU(FrontSteerWheels[i].maxSteer));
                    else                                                       setAngle(FrontSteerWheels[i].WheelNumber,Converter.AngleToUU(InputFrontSteering));
                }
            }

            bNewCommand = true;
        }

        // Here, we turn the appropiate rear-steered wheels, taking into account whether or not the drive command issued is normalized
        if((RearSteerWheels.length!=0) && (cachedRearSteering!=InputRearSteering))
        {
            for(i=0; i<RearSteerWheels.length; i++)
            {
                // If a normalized drive command was received (e.g. the rear-steer value is between -100 and 100)
                if (isCommandNormalized)
                {
                    if(InputRearSteering < -100)     setAngle(RearSteerWheels[i].WheelNumber,Converter.AngleToUU(-RearSteerWheels[i].maxSteer));
                    else if(InputRearSteering > 100) setAngle(RearSteerWheels[i].WheelNumber,Converter.AngleToUU(RearSteerWheels[i].maxSteer));
                    else                             setAngle(RearSteerWheels[i].WheelNumber,(InputRearSteering/100) * Converter.AngleToUU(RearSteerWheels[i].maxSteer));
                }
                // If a non-normalized drive command was received (e.g. the rear-steer is an absolute values, in radians)
                else
                {
                    if(InputRearSteering < -RearSteerWheels[i].maxSteer)     setAngle(RearSteerWheels[i].WheelNumber,Converter.AngleToUU(-RearSteerWheels[i].maxSteer));
                    else if(InputRearSteering > RearSteerWheels[i].maxSteer) setAngle(RearSteerWheels[i].WheelNumber,Converter.AngleToUU(RearSteerWheels[i].maxSteer));
                    else                                                     setAngle(RearSteerWheels[i].WheelNumber,Converter.AngleToUU(InputRearSteering));
                }
            }

            bNewCommand = true;
        }

        for(i=1; i<PoweredWheels.length; i++)    noNeedToUpdateSpeed = noNeedToUpdateSpeed && (JointsControl[PoweredWheels[0]].value == JointsControl[PoweredWheels[i]].value);

        for(i=1; i<FrontSteerWheels.length; i++) noNeedToUpdateFrontSteer = noNeedToUpdateFrontSteer && (JointsControl[FrontSteerWheels[0].WheelNumber].steer == JointsControl[FrontSteerWheels[i].WheelNumber].steer);

        for(i=1; i<RearSteerWheels.length; i++)  noNeedToUpdateRearSteer = noNeedToUpdateRearSteer && (JointsControl[RearSteerWheels[0].WheelNumber].steer == JointsControl[FrontSteerWheels[i].WheelNumber].steer);


        if((PoweredWheels.length!=0) && noNeedToUpdateSpeed) cachedVelocity=JointsControl[PoweredWheels[0]].value;
        else                                                 cachedVelocity=1000000; // a big value to force updating

        if((FrontSteerWheels.length!=0) && noNeedToUpdateFrontSteer) cachedFrontSteering=JointsControl[FrontSteerWheels[0].WheelNumber].steer;
        else                                                         cachedFrontSteering=1000000; // a big value to force updating

        if((RearSteerWheels.length!=0) && noNeedToUpdateRearSteer) cachedRearSteering=JointsControl[RearSteerWheels[0].WheelNumber].steer;
        else                                                       cachedRearSteering=1000000; // a big value to force updating

    }
}

/*
   Code which might be used to regulate the speed and torque of the steering
*/
/*
simulated function Tick(float Delta)
{
    local KCarWheelJoint Joint;
    local int i;

    Super.Tick(Delta);

    // Here, we set the steering speed and steering torque of the front wheels
    for(i=0; i<FrontSteerWheels.Length; i++)
    {
        Joint=KCarWheelJoint(Joints[FrontSteerWheels[i].WheelNumber]);
        Joint.KMaxSteerSpeed = FrontSteerSpeed;
        Joint.KMaxSteerTorque = FrontSteerTorque;
        Joint.KUpdateConstraintParams();
    }

    // Here, we set the steering speed and steering torque of the rear wheels
    for(i=0; i<RearSteerWheels.Length; i++)
    {
        Joint=KCarWheelJoint(Joints[FrontSteerWheels[i].WheelNumber]);
        Joint.KMaxSteerSpeed = RearSteerSpeed;
        Joint.KMaxSteerTorque = RearSteerTorque;
        Joint.KUpdateConstraintParams();
    }
}*/


//*********************************************************************************************************************
// DEFAULT PROPERTIES
// DO NOT change these properties since they are used to initialize programming variables
//*********************************************************************************************************************
defaultproperties
{
    gotRobotInfo = false
    FrontSteerSpeed = SteerSpeed
    RearSteerSpeed = SteerSpeed
    FrontSteerTorque = SteerTorque
    RearSteerTorque = SteerTorque
}