[Bryan Herbst]

Hey guys, here's the deal. Our mechanum code has decided (between yesterday and a week ago) to start trying to send -16256 to the front left wheel, and 16608 to the rear left wheel, with both right wheels getting 0 (the slight differences in left values are due to joystick neutralizing issues). We really don't remember changing anything that would cause this, and we're totally stumped. the PWMs are still getting a reasonable output, but not the right output.

Theres a pic from the COM window attached


And here's the code-

void Mechanum(void)
{

      signed double wFL, wFR, wRL, wRR, velocity, rotation, strafe, wMax;
    signed int difference, difference2, difference3, difference4;
    signed int prepwm01, prepwm02, prepwm03, prepwm04;
    static char pwm01_1, pwm02_1, pwm03_1, pwm04_1;

    /*velocity = (p1_y - 127);
    rotation = (p1_x - 127);
    strafe = (p2_x - 127);*/

    /*velocity = deadband(p1_y,127,15);
    rotation = deadband(p1_x,127,15);
    strafe = deadband(p2_x,127,15);*/
   
    velocity = ctbl[joysticklimit(p1_y)];
    rotation = (ctbl[joysticklimit(p1_x)] / 2);
    strafe = ctbl[joysticklimit(p2_x)];
   
    wFL = velocity - rotation - strafe;
    wFR = velocity + rotation + strafe;
    wRL = velocity - rotation + strafe;
    wRR = velocity + rotation - strafe;

   wMax = intMax(127, intABS(wFL));
   wMax = intMax(wMax, intABS(wFR));
   wMax = intMax(wMax, intABS(wRL));
   wMax = intMax(wMax, intABS(wRR));


    wFL = (wFL * 127 / wMax);
    wFR = (wFR * 127 / wMax);
    wRL = (wRL * 127 / wMax);
    wRR = (wRR * 127 / wMax);
    if (-127 > wFL || wFL > 127 || -127 > wFR || wFR > 127 || -127 > wRL || wRL > 127 || -127 > wRR || wRR > 127)
    {     
        printf("Received an unexpected number FL: %d FR: %d RL: %d RR: %d /n", wFL, wFR, wRL, wRR);
    }
else
    {
   

    prepwm01 = wFL + 127;
    prepwm02 = 127 - wFR;
    prepwm03 = wRL + 127;
    prepwm04 = 127 - wRR;   


//Trying to fix issues with sensitive joysticks (below)


/* if (prepwm01 < 132 || prepwm01 > 122)
{
prepwm01 = 127;
}
if (prepwm02 < 132) || prepwm02 > 122)
{
prepwm02 = 127;
}
if (prepwm03 < 132 || prepwm03 > 122)
{
prepwm03 = 127;
}
if (prepwm04 < 132 || prepwm04 > 122)
{
prepwm04 = 127;
} */


    pwm01 = prepwm01;
    pwm02 = prepwm02;
    pwm03 = prepwm03;
    pwm04 = prepwm04;

printf("PWMS- pwm01: %d , pwm02: %d , pwm03: %d , pwm04: %d \r \n", pwm01, pwm02, pwm03, pwm04);
printf("Wheels- FL: %d FR: %d RL: %d RR: %d \r \n", wFL, wFR, wRL, wRR);
printf("wMax: %d Port 1 Y: %d , Port 1 X: %d , Port 2 X: %d \r", wMax, p1_y, p1_x, p2_x);

    /*if (((intABS((char)pwm01 - pwm01_1)) > 0 ) || ((intABS((char)pwm02 - pwm02_1)) > 0 ) || ((intABS((char)pwm03 - pwm03_1)) > 0 ) || ((intABS((char)pwm04 - pwm04_1)) > 0 )) {
        printf(" pwm01: %d , pwm02: %d , pwm03: %d , pwm04: %d \r", pwm01, pwm02, pwm03, pwm04);
        printf(" Port 1 Y: %d , Port 1 X: %d , Port 2 X: %d \r", p1_y, p1_x, p2_x);
    }

    pwm01_1 = intABS(pwm01 - 127);
    pwm02_1 = intABS(pwm02 - 127);
    pwm03_1 = intABS(pwm03 - 127);
    pwm04_1 = intABS(pwm04 - 127);*/
}
}

The ctbl just adds a curve to the control, and switching to straight-out joystick driving (the first set of commented strafe = and rotation =) gives us pretty much the same results.

The wMax and intABS functions are-

int intMax(int x, int y)
{
    int max = x;
    if(y > max)
    {
           max = y;
    }
   return max;
}

int intABS(int i)
{
    if (i < 0)
        return -i;
    else
        return i;
}
int joysticklimit(int inp)
{
    if(inp < 127)
        {
            inp += 1;
            return inp;
        }
    else if (inp > 127)
        {
            inp -= 1;
            return inp;
        }
    else
        {
            return inp;
        }

Sorry if any of that was confusing, ask if you need clarification.
Thanks!