Chief Delphi - mecanum programming

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)

- Programming (http://www.chiefdelphi.com/forums/forumdisplay.php?f=51)

- - mecanum programming (http://www.chiefdelphi.com/forums/showthread.php?t=51528)

mecanum programming

Hello...

Last year someone posted a link for information on programming for mecanum wheels usage. Does anyone remember what it was. Our team
is concidering this this year. Any help will be greatly appreciated.

Thank You

Re: mecanum programming

We're considering it this year; I'd PM or email the main programmer. His forum name's cprogrammer.

Re: mecanum programming

well....

Start here...

and then use the search tool to look for keywords like omni wheel and holonomic and kiwi There are numerous posts and good info out there ;)

SPAM built an omni wheel 'bot for the 2004 game. It was successful, but you will trade off agility for traction and pushing power. Do you wish to run with the big dogs? Or dance around them!

Eric

Re: mecanum programming

This is what we use for a simple open-loop mecanum drivebase. Default_Routine() computes internal speed, turn rate, and slide (sideways travel) values from the joystick inputs, then computes motor speeds from the internal values.

The p1 joystick is on the left, and p2 is on the right.
pwm01 is the right front motor, pwm02 is the left front, pwm03 is the right rear, and pwm04 is the left rear.
rc_dig_in01 is a "deadman" switch which sets the motors to neutral unless it is closed. This was intended to be used by people "riding" the robot.

Code:

/*******************

function:        deadband()

inputs:        inp, input value

                center, value of input that represents neutral

                band, size of deadband to consider neutral

output:        scaled and deadband-compensated output value

                inputs below center will yield negative output

                inputs above center will yield positive output

                inputs within deadband either side of center will yield 0

usage:        typically call with joystick value (0-254) as input,

                joystick neutral (127) as center, and a small number (5)

                as the size of the deadband.  The white POS joysticks

                should probably have a much larger band (10) instead.

*******************/

int deadband(int inp, int center, int band)

{

int oup;

        if (inp < center-band)

                oup = inp - center + band;

        if (center-band <= inp && inp <= center+band)

                oup = 0;

        if (center+band < inp)

                oup = inp - center - band;

        return oup;

}



/*********************

function:        pwm_limit()

inputs:        inp, input value

output:        scaled and range-limited output value

usage:        call with signed value (+/- 127) as input,

                will return a valid pwm value

*********************/

unsigned char pwm_limit (signed int inp)

{

        if(inp<-127)

                inp = -127;

        if (inp>127)

                inp = 127;

        inp = inp+127;



        return (char)inp;

}



/*******************************************************************************

* FUNCTION NAME: Default_Routine

* PURPOSE:       Performs the default mappings of inputs to outputs for the

*                Robot Controller.

* CALLED FROM:   this file, Process_Data_From_Master_uP routine

* ARGUMENTS:     none

* RETURNS:       void

*******************************************************************************/

void Default_Routine(void)

{

signed int speed, turn, slide;

signed int front_r, front_l, back_r, back_l;

long int l_y, r_y, l_x, r_x;



        l_y = deadband(p1_y,127,10);

        r_y = deadband(p2_y,127,10);

        l_x = deadband(p1_x,127,10);

        r_x = deadband(p2_x,127,10);



        l_y = l_y/2;

        r_y = r_y/2;

        l_x = l_x/2;

        r_x = r_x/2;



        speed = l_y+r_y;

        turn = l_y-r_y;

        slide = -l_x-r_x;

  

          front_r = speed-turn+slide;

        front_l = speed+turn-slide;

        back_r = speed-turn-slide;

        back_l = speed+turn+slide;



if (rc_dig_in01 == 0)

{

        pwm01 = pwm_limit(front_r);

        pwm02 = pwm_limit(front_l);

        pwm03 = pwm_limit(back_r);

        pwm04 = pwm_limit(back_l);

}

else

{

        pwm01 = 127;

        pwm02 = 127;

        pwm03 = 127;

        pwm04 = 127;

}

Re: mecanum programming

Quote:

Originally Posted by Alan Anderson (Post 553913)

This is pretty similar to what Team 95 has used on holonomic/omni test robots in the past (we haven't competed with one since around '97 or so, however).

It's easy for things like variations in the carpet and motor differences to cause your drive mechanism to drive slightly off. You should consider adding a gyroscope and feedback loop so your bot can drive 100% straight (and there's good code examples out there for doing this).

And, with the USB Chiklet available this year, it's really easy to use a three-axis joystick, so it's not hard to control an omni-robots three degrees of freedom direct from one joystick.

Re: mecanum programming

It looks like we aren't the only team who is going to be trying out mecanums this year... just a couple of tips when searching for info, so far I have found results by searching for a couple of common spellings:

meccanum
mechanum and
mecanum

All produce results.

FIRST has info at http://www.usfirst.org/uploadedFiles...heels_Hine.ppt

and the CD white papers have a great power point presentation called "strafing", I believe that runs you through the derivation of the formulas used for a variety of omni-drive systems.

Good luck, and have fun going sideways.

Jason

Re: mecanum programming

Thanks guys...Hmmmm....will send this along to our programmers

Re: mecanum programming

Our team's programmer (<--Yes singular!!) worked out all the math to use the omniwheels, and the mechanum twice, with the wheels oriented differently. Turns out, since we have no money, that we won't be using any holonomic drive this year.

Re: mecanum programming

I am trying to use the code posted above but I am not able to make it strafe using the code. Is there as simple way of doing this. Help is appreciated.

Re: mecanum programming

If by "strafe" you mean travel sideways, the code I posted does that when the joysticks are pushed sideways.

Re: mecanum programming

I am using your code but when I move both joysticks to one side the wheels spin but the robot stays in place. All the wheels either spin inward or all wheels spin outward but never one side spins inward and the other spins outward.

Re: mecanum programming

You're using terms that I don't understand. How does a wheel spin "inward" or "outward"?

This code is for a mecanum drive base, with four mecanum wheels all mounted so they spin either forward or backward. The "handedness" of the wheels is important, and it's possible to set things up in one of two ways. One way has the angled rollers contacting the floor so that the robot can very nearly spin in place without any of the wheels turning. This code assumes the other way, where the angled rollers resist spinning in place when the wheels are stationary.

The "meat" of the code is these four lines:

Code:

          front_r = speed-turn+slide;

        front_l = speed+turn-slide;

        back_r = speed-turn-slide;

        back_l = speed+turn+slide;

All wheels drive at the same rate based on the (forward) speed. The left motors drive more and the right motors drive less based on the (right-hand) turn. The motors on one diagonal drive more in one direction, and the motors on the other diagonal drive more in the other direction, based on the sideways slide.

You might need to change the sign of some of these parts based on how your motors are wired.

Re: mecanum programming

I got your code to work and it works great. One problem: speed controls orange light goes off or blinks about every 5 minutes and we have to recalibrate the speed controls. Is there any way to alleviate or completely fix this. Help is greatly appreciated.

Re: mecanum programming

Quote:

Originally Posted by DustinB_3 (Post 559151)

...speed controls orange light goes off or blinks about every 5 minutes and we have to recalibrate the speed controls...

Are you using the white joysticks from the Kit of Parts? The mechanical trim adjustment on the ones I know from prior years has a habit of moving by itself when you push the joystick far from the center. If that's your problem, try taping it down or using a spot of hot melt glue to keep it in place.

Re: mecanum programming

We'll try that. That's what I think the problem is but our drivers think it's all because of the code. We are using black joysticks from a while ago. They have a square base.

Re: mecanum programming

If you use tank drive the OMNI wheels will deffinately help, but they are a pain to build if you are... 1535 used them for the past 2 years when they were known as 1596, dont know if we're ghoing that way again.

Re: mecanum programming

We're going mecanum for our bot, we're rookies. I'm the programmer, I came up with this code, haven't tried it yet but it's simple and should work perfectly. (cause we don't have a bot yet - only 2 weeks into the six) I also have a program I made that simulates mecanum movement with my drive programming. Here's a section of the user_routines.c with the default drive commented out and my drive put in. It shows which pwm's are which motors, and has a spot for inverting motor outputs if needed.

Code:

 /*---------- 1 Joystick Drive ----------------------------------------------

  *--------------------------------------------------------------------------

  *  This code mixes the Y and X axis on Port 1 to allow one joystick drive. 

  *  Joystick forward  = Robot forward

  *  Joystick backward = Robot backward

  *  Joystick right    = Robot rotates right

  *  Joystick left     = Robot rotates left

  *  Connect the right drive motors to PWM13 and/or PWM14 on the RC.

  *  Connect the left  drive motors to PWM15 and/or PWM16 on the RC.

  */  

  

  /*

  p1_x = 255 - p1_y;

  p1_y = 255 - pwm05;



  pwm13 = pwm14 = Limit_Mix(2000 + p1_y + p1_x - 127);

  pwm15 = pwm16 = Limit_Mix(2000 + p1_y - p1_x + 127);

  */

  

  

  

  /* here's my mechanum drive, ignore all of the above default code

  here's which pwms are which motors

  14   13

  

  15   16

  */

  

  double motor[4] = {127.0,127.0,127.0,127.0};

  

  // actual axis mixing

  motor[0] = p1_y - p1_x - p1_wheel + 255.0;

  motor[1] = p1_y + p1_x + p1_wheel - 255.0;

  motor[2] = p1_y - p1_x + p1_wheel;

  motor[3] = p1_y + p1_x - p1_wheel;

  

  // limit motor outputs to 0 - 254

  for (int n=0; n<4; n++)

  {

      if (motor[n] > 254.0)

          motor[n] = 254.0;

      if (motor[n] < 0.0)

          motor[n] = 0.0;

  }

  

  // invert motor outputs here. Ex:

  // motor[0] = -1.0 * motor[0] + 254;

  

  

  // add .5 so type cast to int (pwm_13 = motor[0];) is rounded,

  //   not always rounded down

  for (int n=0; n<4; n++)

      motor[n] += .5;

  

  pwm_13 = motor[0];

  pwm_14 = motor[1];

  pwm_15 = motor[2];

  pwm_16 = motor[3];

EDIT:
And of course, since this is c and not c++, you have to move the variable declarations to the beginning of the subroutine.
In other words, put

Code:

double motor[4] = {127.0,127.0,127.0,127.0};

int n=0;

right after

Code:

void Default_Routine(void)

{

and change the for loops from

Code:

for (int n=0; n<4; n++)

Code:

for (n=0; n<4; n++)

there's my edit

Use the following program, copy and paste to a compiler, to see what my mecanum drive does. It gives you the total diagonal vectors (as that's how the mecanum wheels push, and they roll freely in the other diagonal, each pair of wheels pushes in a different diagonal), then converts those into the horizontal/vertical force vectors, then converts that into the total movement vector and the angle of movement. It also gives the rotational vector force. Vector outputs are such that "where 100 = the result of one motor's full, direct power" as is commented in the program. It also gives each motor's output (keep in mind 127 is stopped, 254 is forward, 0 is backward) You give in input for each joystick axis where 100 = full forward or right and -100 = full backward or left. Y axis is joystick up/down, makes the bot move forward/backward. x axis is joystick left/right, makes bot strafe left/right. z axis is joystick twist or throttle wheel or a second joystick's left/right, and makes the bot spin to the left/right. It converts the -100 - 100 input to 0-255 input. It's all in double for accuracy, so feel free to input stuff like 75.678. Notice how when going full forward you have a certain forward force, but when you add to that a full spin right the forward speed halves. Then when you stop moving, the full spin to the right doubles in speed. That's cause some of the motors' force have to be redistributed from movement to turning, slowing the movement. That's how my nifty mecanum drive code works :) . Normal 4-wheel, 4-wheel-drive does the same thing. When you're going full forward and decide to turn at the same time, you will slow down a bit.

Code:

#include <iostream>

#include <cmath>

using namespace std;



#define PI 3.141592654



// converts degrees to radians

double rad (double deg)

{

    return deg * ( (2.0*PI)/360.0 );

}



// converts radians to degrees

double deg (double rad)

{

    return rad * ( 360.0/(2.0*PI) );

}



// actual program

main()

{

    // declare variables

    double x=127.5, y=127.5, z=127.5,

           motor[4] = {127.0,127.0,127.0,127.0},

           fwdright = 0.0, fwdleft = 0.0, turnright = 0.0,

           fwd = 0.0, right = 0.0,

           angle = 90.0, total = 0.0;

    

    while(true)

    {

        // get joystick axis inputs

        cout << "Y Axis: ";

        cin >> y;

        cout << "X Axis: ";

        cin >> x;

        cout << "Z Axis: ";

        cin >> z;

        

        // change axis inputs (-100 - 100) to 4-bit inputs (0 - 255)

        x = (x+100.0) * (255.0/200.0);

        y = (y+100.0) * (255.0/200.0);

        z = (z+100.0) * (255.0/200.0);

        

        

        // actual axis mixing

        motor[0] = y - x - z + 255.0;

        motor[1] = y + x + z - 255.0;

        motor[2] = y - x + z;

        motor[3] = y + x - z;

        

        // limit motor outputs to 0 - 254

        for (int n=0; n<4; n++)

        {

            if (motor[n] > 254.0)

                motor[n] = 254.0;

            if (motor[n] < 0.0)

                motor[n] = 0.0;

        }

        

        // round really small motor numbers to 0

        for (int n=0; n<4; n++)

            if (motor[n] < .001)

                motor[n] = 0;

        

        // show motor outputs

        for (int n=0; n<4; n++)

            cout << "\nMotor " << n << ": " << motor[n];

        

        

        // change motor 4-bit outputs (0-255) to display (-100 - 100)

        for (int n=0; n<4; n++)

            motor[n] = (motor[n]-127.0) * (200.0/254.0);

        

        

        // calculating movement vectors and rotational vector

        //   where 100 = the result of one motor's full, direct power

        fwdleft   = motor[0] + motor[2];

        fwdright  = motor[1] + motor[3];

        turnright = motor[1] + motor[2] - motor[0] - motor[3];

        

        // calculating forward and right movement vectors

        fwd   = sin( rad(135.0) ) * fwdleft +

                sin( rad(45.0 ) ) * fwdright;

        right = cos( rad(135.0) ) * fwdleft +

                cos( rad(45.0 ) ) * fwdright;

        

        // calculating total movement vector and angle

        total = sqrt( pow(fwd, 2) + pow(right,2) );

        angle = deg ( atan2(fwd, right) );

        if (angle < 0)

            angle += 360;

        

        // round really small vector numbers to 0

        if ( fabs(fwdleft) < .001)

            fwdleft = 0;

        if (fabs(fwdright) < .001)

            fwdright = 0;

        if (fabs(turnright) < .001)

            turnright = 0;

        if (fabs(fwd) < .001)

            fwd = 0;

        if (fabs(right) < .001)

            right = 0;

        if (fabs(total) < .001)

            total = 0;

        if (fabs(angle) < .001)

            angle = 0;

        

        

        // show all vectors

        cout << "\n\nTurn Right vector: " << turnright

             << "\n\nForward Left movement vector:   " << fwdleft

             << "\nForward Right movement vector:  " << fwdright

             << "\n\nVertical movement vector:   " << fwd

             << "\nHorizontal movement vector: " << right

             << "\n\nTotal movement vector:       " << total

             << "\nTotal movement vector angle: " << angle << " degrees";

        

        getchar();getchar();

        

        cout << "\n-----------------------------------------"

             << "---------------------------------------\n";

    }

}

hope this helps

Re: mecanum programming

oh yeah and if u want a gyro for true holonomic movement (as im planning to do) just turn the xy joystick inputs to magnitude and direction, then offset the angle by the gyro's input, then convert back into the joystick xy input and run through the drive like nothing happened. simple trig. This makes it so that no matter what direction the bot is facing, forward always makes the bot move 'up' and backward always makes if move 'down' etc. even if 'down' is actually the bot's forward-left.

Re: mecanum programming

The valid range of pwm control outputs is 0-254, with 127 being neutral. You should change all your 127.5 values to 127, and all your 255 values to 254.

Re: mecanum programming

thanks very much, didn't know that - thought it was normal 4 bit. I haven't tried it yet, guess it wouldn't have worked. The joystick serial inputs are still 0-255 right?

Re: mecanum programming

Quote:

Originally Posted by LieAfterLie (Post 562866)

thanks very much, didn't know that - thought it was normal 4 bit. I haven't tried it yet, guess it wouldn't have worked. The joystick serial inputs are still 0-255 right?

Do you mean "8 bit"? Yes, the pwms are 8-bit values, but they're defined to have 127 as neutral and go +/- 127 from there.

There aren't any joystick serial inputs. Did you mean "game port"? The joystick values are typically set up with 127 being neutral as well. I can't remember whether 255 is a possible value, but it works to assume the valid joystick range matches the valid pwm range.

Re: mecanum programming

Just as an general guideline for programming with the RC: Avoid the use of floating-point numbers whenever possible. It seems as if you have calculated everything as doubles for the sake of rounding accuracy later on. It is not very helpful to use doubles in your actual robot code, because a difference in the PWM outputs by 1, will be extremely negligible.

As for the whole max joystick value issue, you can always just quickly check it by printing out the value of the joystick inputs in your code. Either way, you can simply just check to see if your variables have overflowed at the end of your code.

Another suggestion: If you are going to use a temporary variable to hold your PWM outputs while they are being calculated, it may make more sense to store them as signed chars instead of unsigned chars, since they intuitively make more sense. Then at the end just add 127 to your values.

Re: mecanum programming

they're serial ports. ill use the operator interface leds to test for 255 later, and use signed chars. Thanks. And yes, I meant 8 bit. I should've taken another second to think about it, I was off by a power.

Re: mecanum programming

Quote:

Originally Posted by LieAfterLie (Post 563316)

they're serial ports.

Joysticks plug into the 15-pin game ports. The only serial port on the OI is the dashboard connector. (Well, there's the radio modem connector, but that's special. And there's serial stuff on the competition port when connected to the field electronics, but that's even more special.)

Quote:

ill use the operator interface leds to test for 255 later, and use signed chars.

Don't use signed chars to store joystick values. They only go from -128 to +127, and would be essentially inside out for motor control. The negative value of greatest magnitude would equate to the slowest forward speed, and the smallest positive value would equate to the fastest negative speed. For reading the joystick values and setting the pwm values, you want unsigned chars. (What you do with those values is your own business; you can obviously subtract 127 from them and get your signed value for internal program use if you like.)

Re: mecanum programming

Thanks again, you've been very helpful. Unsigned it is. Personally I do understand the 0-254 more than -128 - +127, so I guess I'll use unsigned, although it doesn't really matter. By serial I simply meant not parallel, by that definition a game port is a kind of serial port (I do believe...). The default code calls it serial too, that's where i got it from. But it's not the standard computer connector for serial. I can't update the code, I guess it's too long ago. If anyone really wants the working one say so.

Re: mecanum programming

Quote:

Originally Posted by LieAfterLie (Post 563392)

By serial I simply meant not parallel, by that definition a game port is a kind of serial port (I do believe...). The default code calls it serial too, that's where i got it from. But it's not the standard computer connector for serial.

The joystick ports each have four analog inputs and four switch inputs simultaneously, plus either another four switches or four LED drive outputs. I'd say that's at least as parallel as a standard parallel printer port. Can you point me to the spot in the default code that mentions the joystick ports as "serial"? I can only find references to the actual RC serial ports.

Re: mecanum programming

"printer ports are serial", lieafterlie
:):D

Re: mecanum programming

Quote:

Originally Posted by Alan Anderson (Post 562952)

ours is set up with 255 being full power forward, 127 being neutral and 0 being full reverse

the program also sets it up for the wheels on either side to be linked with each other, so it can be either side (one wheel from each axel) being controlled