[apalrd]

C:

//Constants
#define SWAG_BARRIER 0.1
#define SWAG_GAIN 1
#define SWAG_MAX 9000
#define SWAG_PERIOD 500

//Local variables
double old_throttle = 0;
double old_wheel = 0;
double diff_throttle = 0;
double diff_wheel = 0;
double out_throttle;
double out_wheel;
int swag_level = 0;
int swag_period = 0;

//SwagDrive
void SwagDrive(double throttle, double wheel, double* left, double* right)
{
    out_throttle = throttle;
    out_wheel = wheel;

    if(0 == swag_period)
    {
        diff_throttle = abs(throttle) + abs(old_throttle);
        diff_wheel = abs(wheel) + abs(old_wheel);
        if(diff_throttle < SWAG_BARRIER)
        {
            out_throttle = (diff_throttle * SWAG_GAIN) + throttle;
        }
        else
        {
            swag_level++;
        }
        if(diff_wheel < SWAG_BARRIER)
        {
            out_wheel = (diff_wheel * SWAG_GAIN) + wheel;
        }
        else
        {
            swag_level++;
        }

        if(swag_level > SWAG_MAX)
        {
            swag_period = SWAG_PERIOD;
            swag_level = 0;
        }
    }
    else
    {
        out_throttle = 0;
        out_wheel = 0;
    }

    //arcade drive
    left = out_throttle + out_wheel;
    right = out_throttle - out_wheel;

    old_throttle = throttle;
    old_wheel = wheel;
}