USARBot.SkidSteeredRobot

class SkidSteeredRobot extends GroundVehicle config(USARBot);

// Programming Variables
var float cachedLeftValue,cachedRightValue;
var bool gotRobotInfo;
var array<int> LeftPoweredWheels, RightPoweredWheels;

function ProcessCarInput()
{
    local float LeftValue,RightValue;
    local int i;
    local bool noNeedToUpdateLeft, noNeedToUpdateRight;

    // Initialize Local Variables
    noNeedToUpdateLeft = true;
    noNeedToUpdateRight = true;

    Super.ProcessCarInput();

    // Initialize a bunch of variables. Note: this section of code is executed only once.
    if(!gotRobotInfo)
    {
        // Get information about the robot's wheels
        for(i=0; i<Wheels.length; i++)
        {
            if(Wheels[i].PowerType == Left_Powered) // Get all the right powered wheels into a dynamic array
            {
                LeftPoweredWheels.Insert(LeftPoweredWheels.length, 1);            // Make space in the dynamic array to add a left powered wheel
                LeftPoweredWheels[LeftPoweredWheels.length-1] = Wheels[i].Number; // Store the index number of the part in the dynamic array
            }
            else if(Wheels[i].PowerType == Right_Powered) // Get all the left powered wheels into a another dynamic array
            {
                RightPoweredWheels.Insert(RightPoweredWheels.length, 1);            // Make space in the dynamic array to add a right powered wheel
                RightPoweredWheels[RightPoweredWheels.length-1] = Wheels[i].Number; // Store the index number of the part in the dynamic array
            }

            JointParts[Wheels[i].Number].bSteeringLocked = true; // Make sure the wheel is locked (so that it cannot steer)
        }

        // Initialize the controller's properties
        USARRemoteBot(Controller).Normalized = false;
        USARRemoteBot(Controller).LeftThrottle = 0.0;
        USARRemoteBot(Controller).RightThrottle = 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<LeftPoweredWheels.Length; i++) Log("Left Powered Wheel #" $ i+1 $ ": " $ LeftPoweredWheels[i]);
            for(i=0; i<RightPoweredWheels.Length; i++) Log("Right Powered Wheel #" $ i+1 $ ": " $ RightPoweredWheels[i]);
        }
    }

    // If a DRIVE command was issued
    if (USARRemoteBot(Controller).bNewThrottle)
    {
        // Get the left and right throttles from the controller
        LeftValue = USARRemoteBot(Controller).LeftThrottle;
        RightValue = USARRemoteBot(Controller).RightThrottle;

        // If a normalized drive command was received (e.g. the left and right throttle values are between -100 and 100)
        if (USARRemoteBot(Controller).Normalized)
        {
            // Here, we deal with the left throttle
            if(LeftValue < -100)     LeftValue = Converter.SpinSpeedToUU(-maxSpinSpeed);                      // If the normalized value is less than -100, we use the negative of the robot's maximum spin speed
            else if(LeftValue > 100) LeftValue = Converter.SpinSpeedToUU(maxSpinSpeed);                       // If the normalized value is more than 100, we use the robot's maximum spin speed
            else                     LeftValue = (LeftValue/100) * Converter.SpinSpeedToUU(maxSpinSpeed);     // If the normalized value is between -100 and 100, we use a percentage of the maximum spin speed

            // Here, we deal with the right throttle
            if(RightValue < -100)     RightValue = Converter.SpinSpeedToUU(-maxSpinSpeed);                    // If the normalized value is less than -100, we use the negative of the robot's maximum spin speed
            else if(RightValue > 100) RightValue = Converter.SpinSpeedToUU(maxSpinSpeed);                     // If the normalized value is more than 100, we use the robot's maximum spin speed
            else                      RightValue = (RightValue/100) * Converter.SpinSpeedToUU(maxSpinSpeed);  // If the normalized 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 left and right throttle are absolute values, in radians per second)
        else
        {
            // Here, we deal with the left throttle
            if(LeftValue < -maxSpinSpeed)     LeftValue = Converter.SpinSpeedToUU(-maxSpinSpeed);   // If the absolute value is less than the minimum spin speed, we use the negative of the robot's maximum spin speed
            else if(LeftValue > maxSpinSpeed) LeftValue = Converter.SpinSpeedToUU(maxSpinSpeed);    // If the absolute value is more than the maximum spin speed, we use the robot's maximum spin speed
            else                              LeftValue = Converter.SpinSpeedToUU(LeftValue);       // If the absolute value is between the minimum and maximum, we use the spin speed given by the controller

            // Here, we deal with the right throttle
            if(RightValue < -maxSpinSpeed)     RightValue = Converter.SpinSpeedToUU(-maxSpinSpeed); // If the absolute value is less than the minimum spin speed, we use the negative of the robot's maximum spin speed
            else if(RightValue > maxSpinSpeed) RightValue = Converter.SpinSpeedToUU(maxSpinSpeed);  // If the absolute value is more than the maximum spin speed, we use the robot's maximum spin speed
            else                               RightValue = Converter.SpinSpeedToUU(RightValue);    // If the absolute value is between the minimum and maximum, we use the spin speed given by the controller
        }

        // Here, we physically spin all the left-throttled wheels, if needed
        if ((LeftPoweredWheels.Length!=0) && (cachedLeftValue!=LeftValue))
        {
            for(i=0; i<LeftPoweredWheels.Length; i++)
            {
                setSpinSpeed(LeftPoweredWheels[i], LeftValue);
            }

            bNewCommand = true;
        }

        // Here, we physically spin all the right-throttled wheels, if needed
        if ((RightPoweredWheels.Length!=0) && (cachedRightValue!=RightValue))
        {
            for(i=0; i<RightPoweredWheels.Length; i++)
            {
                setSpinSpeed(RightPoweredWheels[i], RightValue);
            }

            bNewCommand = true;
        }


        for(i=1; i<LeftPoweredWheels.Length; i++)  noNeedToUpdateLeft = noNeedToUpdateLeft && (JointsControl[LeftPoweredWheels[0]].value == JointsControl[LeftPoweredWheels[i]].value);

        for(i=1; i<RightPoweredWheels.Length; i++) noNeedToUpdateRight = noNeedToUpdateRight && (JointsControl[RightPoweredWheels[0]].value == JointsControl[RightPoweredWheels[i]].value);


        if(noNeedToUpdateLeft) cachedLeftValue=JointsControl[LeftPoweredWheels[0]].value;
        else                   cachedLeftValue=1000000; // a big value to force updating

        if(noNeedToUpdateRight) cachedRightValue=JointsControl[RightPoweredWheels[0]].value;
        else                    cachedRightValue=1000000; // a big value to force updating

        USARRemoteBot(Controller).bNewThrottle = false;
    }
}


//*********************************************************************************************************************
// DEFAULT PROPERTIES
// DO NOT change these properties since they are used to initialize programming variables
//*********************************************************************************************************************
defaultproperties
{
    gotRobotInfo = false
}