Chief Delphi - Programming Mecanum

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

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

- - Programming Mecanum (http://www.chiefdelphi.com/forums/showthread.php?t=79863)

Programming Mecanum

Hi. Im new to the programming jig, but I'm planning to use Java to program the robot. Our team is going to use mecanum wheels this year, but I don't really know how to program that. Any advice/tips/etc?

Re: Programming Mecanum

I dont know what mecanum wheels are, however you will likely connect the motors to the speed controls, the Jaguars, or to some digital or analog output. If you use Jaguars then there is a class that handles the Jaguars (Victors) as well. There is control for the analog and digital out, although I dont know what they are as I am not as familiar with WPILib.

Re: Programming Mecanum

Check out the holonomicDrive method of edu.wpi.first.wpilibj.RobotDrive. Holonomic Drive is just another name for the class of omnidirectional drives that work by vector addition (mecanum and quad-omnis).

--Ryan

Re: Programming Mecanum

if you are new go with LABview because dragging boxes and conecting wires is easier than typing code and having to worry about syntax

Re: Programming Mecanum

Wait, Ryan, could you give me a sample of what that would look like?

And gaby, I programmed our bot last year, attempting Labview, but ended up going with Windriver.

Re: Programming Mecanum

I actually haven't programmed FRC in Java before, but using the information available at http://first.wpi.edu/FRC/frcjava.html, this is what I'd guess. Follow the instructions in the getting started guide to set up a basic robot project, the code they give you in the guide looks like this:

Code:

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;

import edu.wpi.first.wpilibj.RobotDrive;

import edu.wpi.first.wpilibj.SimpleRobot;

import edu.wpi.first.wpilibj.Timer;

public class RobotTemplate extends SimpleRobot {

    RobotDrive drive = new RobotDrive(1, 2);

    Joystick leftStick = new Joystick(1);

    Joystick rightStick = new Joystick(2);

    public void autonomous() {

        for (int i = 0; i < 4; i++) {

            drive.drive(0.5, 0.0); // drive 50% fwd 0% turn

            Timer.delay(2.0); // wait 2 seconds

            drive.drive(0.0, 0.75); // drive 0% fwd, 75% turn

            }

        drive.drive(0.0, 0.0); // drive 0% forward, 0% turn}    }

        }

    public void operatorControl() {

        while (true && isOperatorControl() && isEnabled()) // loop until change

        {

            drive.tankDrive(leftStick, rightStick); // drive with joysticks

            Timer.delay(0.005);

        }

    }

}

This sets up the robot for a standard differential ("tank") drive. To switch to holonomic drive, refer to the WPIlibJ Javadocs (all of these links are available from that WPI Java resource page [first link]) for the Robot Drive class: Right now, they're constructing the RobotDrive object with two parameters which specify the PWM ports that the left and right motors are connected to. Since you have 4 motors, you should use the

Code:

RobotDrive(int frontLeftMotor, int rearLeftMotor, int frontRightMotor, int rearRightMotor)

constructor instead. Once you have this, you can switch from the drive() and tankDrive() methods to the holonomicDrive() method. You will need to actually use the methods of the Joystick class to get numeric values for the joystick axes, as the holonomicDrive method expects doubles. I would guess that your teleop would look something like this:

Code:

public void operatorControl() {

    while (true && isOperatorControl() && isEnabled()) // loop until change

    {

        drive.holonomicDrive(leftStick.getDirectionDegrees(), leftStick.getMagnitude(), rightStick.getX(GenericHID.Hand.kRight)); // drive with joysticks

        Timer.delay(0.005);

    }

}

Or if your joystick has a twist/Z axis, you can replace rightStick.getX with leftStick.getTwist()

Code is untested, so may require some adjustment.

--Ryan

Re: Programming Mecanum

Alright, got it. Just what I needed. Thanks!

Re: Programming Mecanum

Has this worked out for you?
Our team hasn't tried it yet because the drive base is still being assembled, and we're coding in Labview but I'm pretty sure that the holonomic drive code vi uses the same algorithm as the java RobotDrive holonomicDrive function.

I was curious because I was under the impression that some of the mathematics of mecanum wheels were different from omni wheels, which I believe are what the stock algorithm was designed for.

Re: Programming Mecanum

Quote:

Originally Posted by Todd (Post 900589)

I'm interested in the answer to this question as we have also gotten some conflicting information on the topic.

Re: Programming Mecanum

Not sure I understand where this thread is. I have the coding for mechanums in C and it would have to be translated to Labview or Java or used as C in Labview. But not sure if its needed.

Steve

Re: Programming Mecanum

Here is the code that our team uses for mechanum (in C, but easily portable)-

Code:

velocity = (p1_y - 127); //Joystick 1 y

rotation = (p1_x - 127); //Joystick 1 x

strafe = (p2_x - 127); // Joystick 2 x



wFL = velocity - rotation - strafe; //Front Left wheel, etc...

wFR = velocity + rotation + strafe;

wRL = velocity - rotation + strafe;

wRR = velocity + rotation - strafe;



pwm01 = wFL + 127;

pwm02 = 127 - wFR;

pwm03 = wRL + 127;

pwm04 = 127 - wRR;

Re: Programming Mecanum

Quote:

Originally Posted by Todd (Post 900589)

I was curious because I was under the impression that some of the mathematics of mecanum wheels were different from omni wheels, which I believe are what the stock algorithm was designed for.

The ambiguously named holonomic drive method in C++, LabVIEW, and Java was designed for use with mecanum wheels, not omni wheels.

Re: Programming Mecanum

The holonomic drive function will work with either mecanum or with a 4-wheel omni drive if the wheels are oriented 90 degrees from eachother, arranged like this:
/\
\/
It will not work with kiwi drive.

Re: Programming Mecanum

We tried the above code almost exactly.
It apears to work, but the left-right control of the joystick is inverted. Same with the rotation.

What is the best way to flip to readings from the joystick?

We are currently using the method to get the magitude and direction from the joystick.

Re: Programming Mecanum

I'm not sure in the Java library but in labview direction is departure from 0 degrees forward. Negating your direction should flip it's left/right movement.

Re: Programming Mecanum

Quote:

Originally Posted by Daniel Jones (Post 901675)

The holonomic drive function will work with either mecanum or with a 4-wheel omni drive if the wheels are oriented 90 degrees from eachother, arranged like this:
/\
\/

on your diagram above for holonomic drive, when using mecanum wheels does it matter which way they are oriented?

.

Re: Programming Mecanum

Ah, yes it works pretty well, except there's you gotta use

Code:

drive.holonomicDrive(leftStick.getMagnitude(), leftStick.getDirectionDegrees(), rightStick.getX());

holonomicDrive takes magnitude first, then direction.

One thing though: There's these weird delays, which I'm attempting to fix.

Re: Programming Mecanum

Okay at this moment our programmers are working on trying to figure out the holonomic drive method. The motors are connected to the correct PWMs and we've been using the exact code given here in the forum but through our misfortune we have came up with the notion that the holonomic drive class has errors. Depending on what we do to our code the wheels will run in opposite directions, forward, or rotate; but never reverse.

We think it has something to do with the order of the parameters. Our programming works better if we put rotation where magnitude goes and vice versa. Direction is still much of a mystery.

As you can probably surmise from this entry our troupe of programmers are confused at this point. Right now our mentor is planning to copy the holonomic drive class in order to play with it to see if that yields any positive results.

Re: Programming Mecanum

Do you need to set the motors on one side inverted? When we first just tried we noticed the wheels on one side were reversed and we needed to add:

setInvertedMotor(RobotDrive.MotorType motor, boolean isInverted)

Re: Programming Mecanum

Can someone please post the java scource code for the holonomicDrive Function?

Re: Programming Mecanum

Quote:

Originally Posted by Robototes2412 (Post 920921)

Can someone please post the java scource code for the holonomicDrive Function?

The code is posted below, but anyone with Java can simply look at the source code by opening the project that comes with the distribution:

Code:

    public void holonomicDrive(double magnitude, double direction, double rotation) {

        double frontLeftSpeed, rearLeftSpeed, frontRightSpeed, rearRightSpeed;

        magnitude = limit(magnitude);

        double cosD = Math.cos((direction + 45.0) * 3.14159 / 180.0);

        double sinD = Math.cos((direction - 45.0) * 3.14159 / 180.0);

        frontLeftSpeed = limit((sinD * magnitude + rotation));

        rearLeftSpeed = limit((cosD * magnitude + rotation));

        frontRightSpeed = limit((cosD * magnitude - rotation));

        rearRightSpeed = limit((sinD * magnitude - rotation));



        m_frontLeftMotor.set(frontLeftSpeed * m_invertedMotors[MotorType.kFrontLeft_val]);

        m_frontRightMotor.set(frontRightSpeed * m_invertedMotors[MotorType.kFrontRight_val]);

        m_rearLeftMotor.set(rearLeftSpeed * m_invertedMotors[MotorType.kRearLeft_val]);

        m_rearRightMotor.set(rearRightSpeed * m_invertedMotors[MotorType.kRearRight_val]);

    }

Re: Programming Mecanum

How do we program these wheels when they do not drive straight? In stupid terms cause I do not understand all the programming lingo.

Re: Programming Mecanum

A good idea is to set up each motion (eg front or strafe) separately
then get the axis from your joystick this will get the magnitude
you will have to figure out the math