View Single Post
  #17   Spotlight this post!  
Unread 22-01-2007, 13:41
LieAfterLie's Avatar
LieAfterLie LieAfterLie is offline
Registered User
FRC #2028
Team Role: Programmer
 
Join Date: Jan 2007
Rookie Year: 2007
Location: Hampton, VA
Posts: 35
LieAfterLie is an unknown quantity at this point
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++)
to
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

Last edited by LieAfterLie : 22-01-2007 at 21:32. Reason: changed motor ouput 127.5's to 127.0's and 255's to 254's