Introduction: 5 DOF Robotic Arm Kit With Code
In this tutorial, we will create a robotic arm that will be controlled by a keyboard. This can be used to demonstrate a cool project to friends or something that can be used as part of a bigger project. The system was created using a Xilinx Zybo development board and the Vivado Design Suite.
Hardware List:
- One Zybo Zynq-7000 Development Board
- Five standard servos*
- Two External 5V Power Sources
- 6 DOF Robotic Arm Kit, which can be found on Amazon for about $60
Software List:
- FreeRTOS
- Vivado Design Suite v2016.2
- Digilent Adept 2
- ROS (Robot Operating System)
*Note: Metal-geared servos work best. The servo for the claw actually needs to be quite powerful as the tension increases once the claw gets to its completely open position
Step 1: Getting Started on the Vivado Software
Let's go ahead and get started using Vivado. It's worth noting that this software can be rather frustrating to deal with, so bear with us.
- First, open Vivado.
- Once the project windows comes up, hit "Next>" and type your project name.
- Choose the RTL project and hit "Next."
- Once the window pops up to look for a part, search "xc7z010clg400-1." Then select the appropriate part. Click "Next" and then "Finish."
- Next, on the left-hand panel, find "Create Block Design." Here we will begin adding the block diagram of the design. Give the design a name and click "OK."
- While in the "Diagram" tab, right-click and choose "Add IP." Then, in the search box, type "ZYNQ7 Processing System."
- Next, repeat the step above, but make sure toe add an "AXI GPIO" block
- Then, on the top of the "Diagram" tab, click "Run Block Automation."
- Then, in the same location, click "Run Connection Automation."
- Now, double-click the GPIO module and set the channel width to 4 and make sure it is set to be an output.
- Finally, rename the GPIO output to "R_CONTROL."
Step 2: Configuring the Zynq IP Block
- Double-click the Zynq block.
- In the toolbar of the newly opened window, click "Import XPS Settings." Import the provided "Zybo_zyqn_def.xml" file provided.
- Navigate to "MIO Configuration." Then, enable Timer 0 and Watchdog.
- Also enable UART 1 under I/O preferences.
We decided to use UART 1, which is pins 49 and 48 that are contained internally in the micro USB port because it makes communication over USB much easier.
- Then, under "Clock Configuration," make sure to select FCLK_CLK1 under PL Fabric Clocks.
- Last, click OK
Attachments
Step 3: Add PWM Module to Design
Next, we will go ahead and add the module that will control the servo motors.
- First, under sources, right click the file ending in ".bd" and click "Generate HDL Wrapper."
- Then, right-click the new wrapper (it should end in ".v") and click "Add Design Source."
- Next, you can either import the attached file. In doing so, make sure that you let the program copy the file into the project directory.
- Alternatively, you can create a new design source and copy and past the code below:
`timescale 1ns / 1ps module pwm( input [3:0] duty, input clock, output sclk ); parameter RIGHT = 'b0011; parameter LEFT = 'b0010; logic [29:0] clk_cnt, limit = 'd150_000; logic [29:0] count = 'd2_000_000; logic [29:0] timer_cnt, ms = 'd100_000; logic newclock, s_newclock; assign sclk = newclock; always @(posedge clock && timer_cnt == ms) begin case ( duty ) RIGHT : limit = limit + 'd500; LEFT : limit = limit - 'd500; default : limit = limit; endcase if ( limit > 'd220_000 ) begin limit = 'd220_000; end if ( limit < 'd80_000) begin limit = 'd80_000; end end // Clock Divider always @(posedge clock) begin if ( clk_cnt > count ) begin clk_cnt = 0; end else clk_cnt = clk_cnt + 1; end // Timer always @(posedge clock) begin if (timer_cnt > ms ) begin timer_cnt = 0; end else timer_cnt = timer_cnt + 1; end // PWM 'Maker' always_comb begin if ( clk_cnt > limit ) begin s_newclock = 1'b0; end else s_newclock = 1'b1; end assign newclock = s_newclock; endmodule
Step 4: Configure Wrapper and Instantiate Modules
Next, we will take the newly imported PWM module and instantiate it within the wrapper.
- Firstly, go ahead and comment out every instance involving "R_CLOCK" and "r_control_tri_o" in the wrapper except the wires.
- Make sure to add "output [4:0]pwm;" below the input and output section to declare pwm as an output.
- Next, below the instantiation of the design, we will instantiate 5 instances of the pwm module.
- Finally, we will tweak the parameters in each module instance so that each servo will respond to a different GPIO value.*
- Attached is what the final wrapper should look like. You can go ahead and open the file in any text editor and copy and paste it into the wrapper currently in Vivado.
*Basically, it is currently implemented by having each PWM module being associated with the appropriate 3-bit number for each servo, and the 4th bit telling the servo which direction to move. For example, 0011 would move servo 1 in the right direction while 0010 would move servo 1 in the left direction. Servo 2 would be controller by 0100 and 0101, etc...
Attachments
Step 5: Map PWM Modules to Pmod Pins
The last thing to do for the hardware design portion is to determine where we will map the PWM outputs to. Check the attached image for how the Pmod pins are laid out.*
- Next, choose a Pmod to output to. or the sake of this tutorial, we will use JE.
- Then, under sources, right-click and select "Import Constraints File."
- Then select the attached ".xdc" file.
- Next, open the file in Vivado and find the entries pertaining to the JE header.
- Finally, uncomment the "set_property" lines for five of the ports and instead of it being set to "je[0]", change each one to "pwm[0]" while increasing the number to correspond with the pin.
*Note: Do not use the Vcc pins. They only provide 3.3V and have a limited current output. Most servos require 5V and a relatively high amount of current. Connecting them directly to the board instead of an external power source could ruin the Zybo board.
Attachments
Step 6: Implement Design and Generate Bitstream
Lastly, before we get into the SDK, we need to generate the bitstream to output to the SDK.
- First, under "Flow Navigator" click "Run Implementation." This should take several minutes depending on your CPU power. Make sure to correct any errors. Generally, the warnings can be ignored.
- Next, click "Generate Bitstream" under "Program and Debug." Once that is done, go ahead and click "Export Hardware" under the "Export" menu under "File." Make sure to click "Include Bitstream"
- Then navigate to File->Launch SDK and click OK. Now we will get to Vivado's SDK.
Step 7: Moving on to Vivado's SDK
Now that we are done with the hardware design in Vivado, we will now start implementing the software system on the board.
- First, lets start by downloading the latest version of FreeRTOS here.
- Go ahead and open the .exe and extract everything.
- Next, we will go ahead and import FreeRTOS into the SDK.
- Click File->Import, then under "General" click "Existing Projects Into Workspace" then click "Next"
- Navigate to "FreeRTOS/Demo/CORTEX_A9_Zynq_ZC702" in the FreeRTOS folder. After doing this, make sure to only import the "RTOSDemo"
- Next, let's go ahead and generate a Board Support Package. This allows RTOS to communicate with the hardware modules you have implemented.
- Let's do this by clicking File->New Board Support Package
- Choose "ps7_cortexa9_0". You can leave the name as is. Make sure to check "lwip141" and click "OK".
- Lastly, right-click on the blue RTOSDemo folder and choose "Project References."
- Uncheck "RTOSDemo_bsp" and check the new bsp you just created.
Step 8: Modifying FreeRTOS Code
- First, go ahead and open up "main.c" under RTOSDemo
- Then, go ahead and find "#define mainSELECTED_APPLICATION"
- Change the number after it from 0 to 1. This will change the starting program to be main_blinky.c
Next, open up "ParTest.c" under RTOSDemo. This is where we will initialize the UART and GPIOs on the software side.
This code effectively takes in an ASCII character from the GPIO and converts it into a 4 bit code to be sent to the PWM module in hardware. It also clears the buffer to the GPIO every so often so we are not constantly sending out commands to move the arm if the user is not actually providing an input.
The characters WASD, IJKL and TY are the ASCII letters we chose to control the arm because they are common controls for video games, and we were using an Xbox controller to control the arm.
At the top of the file add: #include "xuartps.h", to add all the necessary UART functions.
The rest of the code for the ParTest.c file can be found below.
<p>*-----------------------------------------------------------</p><p> * Simple IO routines to control the LEDs. * This file is called ParTest.c for historic reasons. Originally it stood for * PARallel port TEST. *-----------------------------------------------------------*</p><p>* Scheduler includes. */ #include "FreeRTOS.h" #include "task.h"</p><p>/* Demo includes. */ #include "partest.h"</p><p>/* Xilinx includes. */ #include "xgpiops.h" #include "xgpio.h" #include "xuartps.h"</p><p>#include </p><p>#define partstNUM_LEDS ( 1 ) #define partstDIRECTION_INPUT ( 1 ) #define partstDIRECTION_OUTPUT ( 0 ) #define partstOUTPUT_ENABLED ( 1 ) #define partstLED_OUTPUT ( 1 ) #define partst_RX ( 48 ) #define partst_TX ( 49 ) #define portTick_Period_MS ( 500 )</p><p>/*-----------------------------------------------------------*/</p><p>static XUartPs xUart; static XGpio xGpio; static BaseType_t xServoControl = 2;</p><p>unsigned int addr; unsigned int num = 1; double count; u8 buff;</p><p>/*-----------------------------------------------------------*/</p><p>void vParTestInitialise( void ) { XUartPs_Config *uart; XGpio_Config *pxConfigPtr; BaseType_t xStatus; BaseType_t xxStatus; BaseType_t xxxStatus;</p><p> /* Initialize the Uart */ uart = XUartPs_LookupConfig(XPAR_PS7_UART_1_DEVICE_ID); xxStatus = XUartPs_CfgInitialize(&xUart, uart, uart->BaseAddress); configASSERT( xxStatus == XST_SUCCESS); (void) xxStatus;</p><p> XUartPs_SetBaudRate(&xUart, 57600); XUartPs_Recv(&xUart, &buff, sizeof(double)); count = 1.5;</p><p> /* Initialise the GPIO driver. */ pxConfigPtr = XGpio_LookupConfig( XPAR_AXI_GPIO_0_DEVICE_ID ); xStatus = XGpio_CfgInitialize( &xGpio, pxConfigPtr, pxConfigPtr->BaseAddress ); configASSERT( xStatus == XST_SUCCESS ); ( void ) xStatus; /* Remove compiler warning if configASSERT() is not defined. */</p><p> /* Enable outputs and set low. */ XGpio_SetDataDirection( &xGpio, partstLED_OUTPUT, partstDIRECTION_OUTPUT ); //XGpio_DiscreteWrite( &xGpio, partstLED_OUTPUT, partstOUTPUT_ENABLED ); XGpio_DiscreteClear( &xGpio, partstLED_OUTPUT, 0xFFFF );</p><p>} /*-----------------------------------------------------------*/</p><p>void vParTestSetLED( UBaseType_t uxLED, BaseType_t xValue ) { ( void ) uxLED; XGpio_DiscreteWrite( &xGpio, partstLED_OUTPUT, xValue ); } /*-----------------------------------------------------------*/</p><p>void vParTestToggleLED( unsigned portBASE_TYPE uxLED ) {</p><p> (void) uxLED; XUartPs_Recv(&xUart, &buff, sizeof(double));</p><p> if(buff == 0x61){ // 0x61 = a base left xServoControl = 0x3; // 0011 }else if(buff == 0x64){ // 0x64 = d base right xServoControl = 0x2; // 0010 }else if(buff == 0x77){ // 0x77 = w shoulder up xServoControl = 0x5; // 0101 }else if(buff == 0x73){ // 0x73 = s shoulder down xServoControl = 0x4; // 0100 }else if(buff == 0x69){ // 0x69 = i elbow up xServoControl = 0x6; // 0110 }else if(buff == 0x6B){ // 0x6B = k elbow down xServoControl = 0x7; // 0111 }else if(buff == 0x6A){ // 0x6A = j wrist left xServoControl = 0x8; // 1000 }else if(buff == 0x6C){ // 0x6C = l wrist right xServoControl = 0x9; // 1001 }else if(buff == 0x74){ // 0x74 = t claw open xServoControl = 0xA; // 1010 }else if(buff == 0x79){ // 0x79 = y claw close xServoControl = 0xB; // 1011 }else { xServoControl = 0x0; }</p><p> XGpio_DiscreteWrite( &xGpio, partstLED_OUTPUT, xServoControl); buff = 0x0;</p><p>}</p><p>void vParTestClearGPIO( void ){ XGpio_DiscreteWrite( &xGpio, partstLED_OUTPUT, 0x00);</p><p>}</p>
Step 9: Assemble Arm Hardware
The hardware is composed of several brackets, servos and servo horns. We ordered our brackets from the following link. The servo horns are pivotal for this project and must be ordered separately, so make sure your order matches your servos!
Step 10: Base and Shoulder
The base is assembled with 3 of the large brackets. To build the shoulder, bolt the servo horn to the the shoulder servo and place it in a small bracket and bolt down. The U-shaped bracket is then placed around the servo and it's bracket. You can stack the next servo and it's bracket to complete the shoulder.
Step 11: Elbow, Wrist and Claw
The servo brackets and the U brackets are stacked one after the other until the final servo is parallel to the length of the arm. The claw can now be screwed on along with the claw servo. The brackets are designed to be modular so you can customize the length of the arm and its degrees of motions.
Step 12: Take Control!
The software we've provided allows for interface with a serial monitor such as 'putty' for control through a keyboard. The controls are mapped to a, s, d, w, j, k, l, y and t. The project can be expanded to include an interface with any joy stick with the correct drivers and scripts, but the robot arm software and hardware will work as is. Make sure to provide the required voltage to the servos separate from power to the board to avoid drawing too much current through the Zybo.
We used ROS (Robot Operating System) to control our arm through a game pad. ROS is collection of open source software packages that allows for simple connection across multiple devices. To learn about ROS and how to install it you can read more here. If you'd like to control from a game pad using our 'arm' project we have provided you can learn about the ROS package 'joy' here.