Mecanum Drive Not Working

FIRST General Forum
#1

So for some reason we can’t get our mecanum drive working. Our code (in C++) looks like this:


[This just calculates the motor speeds which will be set later.
-myAbs(float) is just a function which finds the absolute value of a float.
-largest(float ...) is a function which finds the largest any number of inputted float values.
-these two functions have been tested and work correctly.]

   //Square Joystick inputs for better precision control
    x = mainStick.GetX(); x = x*x;
    y = mainStick.GetY(); y = y*y;
    twist = secondaryStick.GetX();
    //[0, 1] Throttle value which may be used to artificially decrease speed
    throttleFactor = -(mainStick.GetThrottle()-1)/2;
    //Set minimum threshold on Joystick inputs
    if(myAbs(x) + myAbs(y) + myAbs(twist) > JOY_THRESHOLD)
    {
    	//Calculate raw motor speeds for vector drive
        Speed[NW] = y + x + twist;    Speed[NE] = y - x - twist;
        Speed[SW] = y - x + twist;    Speed[SE] = y + x - twist;
        
        //Calculate magnitude of largest motor speed
        topSpeed = largest(myAbs(Speed[NW]), myAbs(Speed[NE]), myAbs(Speed[SW]), myAbs(Speed[SE]));
        
        //If the calculated largest speed is greater than 1, scale everything down
        if(topSpeed > 1)
        {
            Speed[NW] /= topSpeed;    Speed[NE] /= topSpeed;
            Speed[SW] /= topSpeed;    Speed[SE] /= topSpeed;
        }

        //Give option artificially slow down with throttle while holding down a button
        if (mainStick.GetRawButton(THROTTLE_BUTTON))
        {
            Speed[NW] *= throttleFactor;    Speed[NE] *= throttleFactor;
            Speed[SW] *= throttleFactor;    Speed[SE] *= throttleFactor;
        }
    }
    else  //If Joystick inputs not within threshold
    {
        Speed[NW] = 0;    Speed[NE] = 0;
        Speed[SW] = 0;    Speed[SE] = 0;
    }

We later set the motors in a separate function:


    //Invert motors in code instead of electronically
    NWMotor.Set(Speed[NW]*NW_MOTOR_INVERT);
    NEMotor.Set(Speed[NE]*NE_MOTOR_INVERT);
    SWMotor.Set(Speed[SW]*SW_MOTOR_INVERT);
    SEMotor.Set(Speed[SE]*SE_MOTOR_INVERT);

The motors do move, but in weird ways (for example if we push the main joystick full forward, I believe only two motors on opposite corners moved).
Also, if we put default code on the robot it does seem to make all of the motors move, but we don’t want to use default code because we feel like it’s cheating and it would just be nice to get our own code working so we can mess or with it or change it if we want.
So… anybody got any ideas?

#2

When you square x (and y) you change their range from -1 to +1] to [0 to +1] .

Perhaps you accounted for this in your code but I didn’t see it with a cursory review.

Try xxx instead of x*x. That preserves the sign.

See the following posts:

TUNABLE JOYSTICK SENSITIVITY
http://www.chiefdelphi.com/forums/showthread.php?p=921992

MECANUM CODE
http://www.chiefdelphi.com/forums/showthread.php?p=916383#post916383

~

#3

Hmm… that’s true. Thanks. It still does not explain, however, why drive does not work correctly even if I just move the joystick forward, making the only input a positive y value…

#4

Check it again, with the bot on blocks so you can see which wheels are moving, and observe the jag LEDs.

~

#5

You can use the absolute value function to square inputs.


#include <cmath>

x *= fabs(x);
y *= fabs(y);
#6

or you could have an if statement, so there’s less multiplying…

if (x<0.0) x = -(xx); else x = xx;

#7

Squaring is a funny thing. All of your inputs are between -1 and 1.

If: -1 < x < 1
then: x > x^2

If you do away with squaring, the following calculations will be easier to debug:

Speed[NW] = y + x + twist;
Speed[NE] = y - x - twist;
Speed[SW] = y - x + twist;
Speed[SE] = y + x - twist;

#8

Well I guess we’ll just cube inputs later instead of squaring to take care of the sign change problem; the point of doing that is to have better precision control (it’s easier to make small movements). For testing we can just take that out; this has no impact, however, on how the motors are moving right now (i.e. in weird directions), which means something else is wrong…

#9

If you are seeking help, it would help if you would put the bot up on blocks and carefully note which way each wheel spins (forward or backward) for each joystick input (forward, backward, right, left, twist CW, twist CCW)*. Also note what the LED on each jag is doing for each of these cases.

Post your results here and I am quite sure you will get the help you need.

*remove the squaring of the inputs first

~

#10

Aside from ‘if we push the main joystick full forward, I believe only two motors on opposite corners moved’, what are some of the other symptoms? I’m assuming the others motors move in other joystick positions? Is it the other two motors when full right?

#11

Okay so we figured out why it wasn’t working. A silly problem. Basically we just didn’t include the header file for the cpp which included our custom functions [namely float largest() in this case]. Thanks though everyone :smiley:

#12