Mechanum Wheel Robot

Ever wanted to make a robot that was highly dexterous and highly controllable? This is your inscrutable! We will show you how we created a four wheel Mechanum Robot and how you can too!

Electrical Components:

1 x Zybo Development Board (we used:https://store.digilentinc.com/zybo-zynq-7000-arm-f...

4 x DC motors with encoders (we used: https://www.pololu.com/product/2824)

2 x Dual H-Bridge Motor Drivers (we used: https://www.pololu.com/product/1213)

2 x Logic Level Sifters (from 5v to 3.3v) (we used: https://www.sparkfun.com/products/12009)

Note: You need 8 different channels for this project, the Sparkfun board had 4 on each so we only needed two boards, if you get different ones you need to adjust the number of boards needed.

1 x Lipo Battery (we used a 3 cell lipo battery, but you can use any external battery pack that you want, we used:

https://www.amazon.com/gp/product/B071NHPFMG/ref=o...)

1 x Power Switch (does not have to be fancy, the one we used we just had laying around from previous projects)

Mechanical Components:

4 x Mechanum Wheels (we used: https://www.robotshop.com/en/60mm-mecanum-wheel-s...)

Note: These wheels are the second set we used. The first set was of lower quality and could not use, so our advice is not use these wheels: https://www.robotshop.com/en/mecanum-wheel-4-pack-...

Material to build a simple chassis. This is largely up to what you have available, what we used was an assortment of Aluminum Chanel we got from https://www.servocity.com/

Our Robot is using Mechanum wheels due to the increase in dexterity they provide to our robot. They are different then normal wheels due to them having many tiny rollers around the circumference of the wheel. This creates the wheel able to coast at a 45 deg angle. When all the wheels of the robot are Mechanum the robot can translate in any direction without rotating, keeping the front pointed in the same direction. This increased dexterity increases the complexity of the driving program in order to execute the moments.

So we have a zybo board, we have the motor drivers, we have the motor, but how do we get everything connected to the board. First follow the three pictures that have a detailed wiring diagram. There is one picture that shows how you connect the zybo with the motor drivers and motor in an overall diagram. In order to show more detail on how to connect the motor driver chip there is a second photo. Then to get the motor encoders working look at the third included picture.

Once you get all the wiring up how are thing broken down between the FPGA and the ARM processor? The included block diagram shows how everything goes through the FPGA and the processor interacts with he FPGA through the AXI interface.

The FPGA handles the encoder values from the wheels, as well as driving the motors using pulse width modulation (PWM). The encoders use two Hall effect sensors, which sense magnets located on the wheels, sending an electrical pulse 3200 times per rotation. Because each wheel sends two offset signals, comparing each of them on a clock edge can tell you the direction of travel. This method of wheel encoding is called Quadrature Encoding. The Verilog code for reading in the pulses and maintaining an count with direction is shown in the attached file "encoders.sv"

As you can see, to avoid conflicts with increment COUNT in multiple “always” statements, we must delay each incoming signal with a clock, and increment only on the positive edge of the clock.These counts are passed through Xilinx AXI GPIO to the CPU.

To drive the motors, you feed the motors a square signal with duty cycle equal to the percentage of power you wish to run at, as well as a high/low bit for direction. For example, 75% forward means outputting a 75% duty cycle square wave accompanied by a low bit. 75% backwards corresponds to 25% duty cycle (100 - 75) and a high bit. For the square signal, we used a Xilinx AXI Timer set to PWM mode. For the high/low bit, we used a simple Xilinx AXI GPIO block. The C code to interface with the AXI ports, as well as reading in the encoder values, is shown in the attached file "motor_driver.c"

So we have encoder reading from the motor, but how do we get that to speed? The simple approach is to just look a differential between the current sample and last sample. However, this can be noisy, so we applied a Savitzky Golay Filter to the encoder data to get a more accurate speed calculation. A more detailed explanation can be seen in Appendex A. Otherwise the code to calculate the current encoder speed is given in the code "savgo.c"

Now that we have the ability to move the wheels independently and can measure how fast each wheel is traveling we now need to add a control algorithm to it. We developed a control algorithm that would take how fast each motor is going. From that would calculate where we are in 2-d space(assuming we are on a flat surface) and calculates how fast each motor needs to go relative to each other to get the desired trajectory. This involves a lot of transformation matrices and equation solving. If you are curious as to the math behind feel free to look through the code, however if you just want to implement the code just look at the 4 functions you must call.

/*
* Used to initialize the system, needs a parameter of what FREQUENCY your

*control loop is running at ie we are running at 500Hz so we pass the

*parameter of 500 to the function */

void initMech(int deltaT);

/*

* Sets the new goal for our robot to drive to, takes a Vector (this is a float[2][2])

* t[0][0] is the x distance

* t[1][1] is the y distance

* t[0][1]/t[1][1] is not used by this function

* Rotation is a float[2] that is desired rotation in degree wanted set the rotation wanted

* in rottemp[0] */

void setGoal(Vector t,Rotation rottemp);

/*

* Given the current motor speed in a Motor structure (float[4]) curSpeed

* the units expected of curSpeed is RADIANS per second.

* return the RELATIVE motor speed in a Motor structure (float[4]) goalSpeed

* this return value is relative speed. where 1 is max speed and 0 is off. */

void getNewSpeed(volatile Motor curSpeed,volatile Motor goalSpeed);

/*

* Get the current position the algorithm has calculated

* This function is not necessary to be called, on if you are interested */

void getPos(Vector xy, Rotation angle);

Now that we have all the parts working lets combine them together. The attached file "main_mech.c" works with FreeRTOS software that runs on the zybo board. It initializes all components and sets the robot to move to (100,0) centimeters with 0 rotation. This is 1 meter directly left with no translation. Included in the code is the demo function that shows off using the mechanum wheels to move in square pattern.

Enjoy!

This is more detail in to how we calculated and generated our "Magic Numbers" found in our code getting current encoder speeds. We used a Savitzky Golay Filter to perform a polynomial fit to 51 data samples at 500Hz. With a sampling rate of 500Hz the delay introduced by this filter is 50ms. The calculations of this filter is largely due in the set up of the filter and generating constants to multiply to each data sample. We calculated these coefficients in Matlab and exported the coefficients to C.

Matlab Code:

order =4;

taps=51;

dt = 1/500;

[b,g] = sgolay(order,taps);

for p=0:2

g(:,p+1) = g(:,p+1) *factorial(p)/(-dt)^p;

end

The variable g is a 'taps' x 'order' matrix. To get the first derivative (speed) use the coefficients in the second column. (if you wanted to get a different derivative use a different column, in this case we wanted speed) For the C code of this see the attached file "savgo.c"