Controlling a robotic arm using linux
Magnus Hägg
ÅF consult, School of Innovation, Design and Engineering,
Mälardalens Högskola,
2009
mhg05004@student.mdh.se
Supervisor: Tommy Klevin
Infrastruktur, ÅF Consult
Examiner: Mats Björkman
School of Innovation, Design and Engineering
November 17, 2009
Abstract
This thesis deals with the construction of a control system for a robotic arm, that is researched and constructed from a prede ned robotic arm and a FPGA-based development board, including a benchmark on the hardware used by the control system of the robotic arm between uClinux 2.6.20 and Linux 2.6.18, through several runs on the dhrystone benchmark.
The project was conducted during the summer and halfway into the autumn semester and is the basis of a thesis in Computer Science at School of Innovation, Design and Engineering. Mälardalen University.
The project was commissioned by AF consult infrastruktur västerås who wanted a moving robotic arm to demonstrate at exhibitions and other public relation events. They previously aquired the robotic arm and the development board from Xilinx.
From those preconditions vhdl components were created for the joystick and controlling of the robotic arm. The embedded system contained several Xilinx components, including the Microblaze CPU which ran the dierent kernels.
The results show that uClinux is slower but not enough for it to be rejected on the grounds that the other properties weigh up the drawback.
Sammanfattning
Denna rapport behandlar konstruktionen av ett styrsystem för en robotarm som utreds och kon-strueras utifrån en förutbestämd robotarm och FPGA-kort, samt en prestandajämförelse på hård-varan som användes för själva styrsystemet mellan uClinux 2.6.20 och Linux 2.6.18, detta genom ertalet körningar utav dhrystone testet.
Projektet genomfördes under sommaren och första perioden 2009 och är grunden på ett examen-sarbete inom Datavetenskap vid Akademin för Innovation, design och teknik. Mälardalens Högskola. Uppdragsgivare är ÅF consult infrastruktur västerås som ville ha en rörlig robotarm att visa upp vid mässor och liknande PR-tillställningar. De hade redan innan projektets start införskaat den tilltänkta robotarmen och ett FPGA-kort från Xilinx.
Därefter så skapades vhdl-komponenter för joysticken samt styrningen för robotarmen. Det in-byggda systemet inkluderar ett ertal av Xilinx komponenter, bland annat Microblaze CPU som de olika kärnorna kördes på.
Resultaten visar att uCLinux är långsammare men inte tillräckligt för att det bör väljas bort då dess övriga egenskaper väger upp prestandaförlusten.
Contents
1 Introduction 2 2 Electronics 2 2.1 Signals . . . 2 2.2 Servos . . . 2 2.3 Robotic arm . . . 2 2.4 Opto couplers . . . 2 2.5 Joystick . . . 3 2.6 PCB . . . 3 2.7 The Hardware . . . 43 Field-Programmable Gate Array 4 3.1 Component . . . 4 3.1.1 Joystick . . . 5 3.1.2 Pwm . . . 5 3.2 Embedded systems . . . 5 3.3 Xilinx Spartan-3AN . . . 5 3.4 Soft-microprocessor . . . 6 3.5 Microblaze . . . 6 4 Software 6 4.1 Development platform . . . 6 4.2 Operating systems . . . 7 4.2.1 Real-time, non-real-time . . . 7 4.3 Linux . . . 7
5 Linux and Embedded systems 7 5.1 The platform . . . 7
5.2 PetaLinux . . . 8
5.3 Controlling the arm . . . 9
6 Testing 9 7 Results 10 8 Summary 10 8.1 Conclusions . . . 10
8.2 Thesis work evaluation . . . 10
9 References 11 A Source code: C 12 A.1 joystick.c . . . 12 A.2 pwm.c . . . 13 A.3 move.c . . . 14 B Source code: VHDL 19 B.1 joystick.vhdl . . . 19 B.2 pwm.vhdl . . . 20 C Measurements 21
1 Introduction
This thesis was thought of by AF consult who needed a demonstration platform to promote their company on exhibitions, for this project they had obtained a robotic arm and a Xilinx FPGA board. My part in this was to complete their vision and do a benchmark on the system for comparison of embedded operating systems performance.
2 Electronics
2.1 Signals
Signals are used to share information between devices. Among the dierent kinds of signals this thesis only involves digital input and output IO-signals and pulse width modulated signals (PWM).
Digital IO is an agreement that dierent voltage levels indicates dierent logic levels, for instance 0 to 2 volts for a 0 or false and 3 to 5 volts for a 1 or true.
PWM-Signals are electric signals that are periodic counting from one rising edge to the next and has a duty-cycle that is a percental representation of the time of the period is it high. For the servos the period in this case is 50 Hz or 20 ms, and the duty-cycle are 5 to 10 percent or 1 to 2 ms.
Figure 1: The rst one is a digital signal over time, while the second illustrates the PWM signal.
2.2 Servos
Servos are electric motors that orient themselves according to a PWM signal that operates at 50 Hz. Depending on the duty-cycle of the signal the servo positions itself between 0 and 90 degrees. Standard servos operate at 4.8 to 6 volts [8]
2.3 Robotic arm
Figure 2: The robotic arm. The robotic arm is from the Lynxmotion corporation that specializes
in the do it yourself enthusiasts of robotics. The model, Lynx 6 Pos-sesses ve degrees of freedom with 90 degrees of motions in the axis plus a gripper to manipulate objects.[11] The movement is achieved through seven servos where two of them are coupled in parallel to increase the torque in the crucial joint.
2.4 Opto couplers
When dealing with sensitive or expensive equipment opto couplers can aid the design by shielding the components from harsh environments. They work by isolating two sides from each other and transfer infor-mation between the two sides by lighting a infrared diode on one side that activates a photo transistor on the other side of the circuit. By doing this the two sides have no electronic connectivity between them. On the output from the FPGA there is 3.3 volts. To light the diode in the opto coupler a resistor is added at 27 Ω to produce the right current. The collector side of the opto coupler has 5 volts coupled to
to the servo is connected. The resistor is there to ensure that the voltage goes down when the input signal is low. [9]
Figure 3: The schematics for one of the optocouplers.
2.5 Joystick
The plan was initially to use a standard joystick with serial or USB interface, but since the board was missing a USB port and the seriell joysticks are unavailable on the market a joystick was constructed from scratch using a four way digital joystick and 3 buttons that are coupled from logic one through the switch to a pull-down resistor. The main joystick has four directions (up,down,left,right) and three buttons (axis1,axis2 and slow). There is also a separate box with 3 buttons to control the demo mode of the arm.
Figure 4: The left shows the control joystick and the right one is the macro.
2.6 PCB
Printed Circuit Board, a way to package electronic components and connect them to each other. It often starts as a wiring diagram that evolves into a PCB design, these range from 1 layer of conducting pathways and up, depending on the complexity of the design.
After the designs of the opto couplers had been tested on a breadboard, a PCB which housed both the opto couplers and the pull-down resistors was designed in CadSoft EAGLE. When the schematics are done and tested on a breadboard the design has to be moved to a board design. The designer is then faced with the task to place the components to eliminate that lines cross and that the design is as compact as possibly, if lines must cross they can use a jumper or if there are many lines that cross an extra layer to route across.
Since costs of manufacturing depends on both layers and board area companies have developed algo-rithms to minimize both for maximal cost e ciency.
When the design is completed the PCB can be etched from a copperlaminate and chemicals, milled, or sent to a company that specializes in the manufacturing of other companies boards.
Figure 5: The left one is the nal design and the right is the manufactured part.
2.7 The Hardware
To be able to move the demonstration platform in an e cient way the robotic arm is stored in a box so that the lid detaches and becomes the base of the arm. This way, the arm has a stable platform so it doesn't tip over when extended and is protected in transport. The electronics has a powersupply that delivers 5 volt and 3.5 Amperes. The joysticks are connected to the PCB via 2 serial connectors (dsub-9) and wired in a way that they dont work if they are mixed up.
Figure 6: The left is the transportbox and the right is with the lid removed.
3 Field-Programmable Gate Array
FPGA for short, a chip that is currently used to construct agile hardware. Since the chip rapidly can be reprogrammed with new logic and functions it is ideal for hardware development, especially in embedded systems. In the beginning FPGAs were very expensive and housed only few logic elements, but nowadays a chip can contain millions of gates that enable them to house very diverse hardware internally such as processors and game systems.
The chip is "based around a matrix of con gurable logic blocks (CLBs) connected via programmable interconnects." [1] "The switch matrix is highly exible and can be con gured to handle combinatorial logic, shift registers or RAM." [1] This enables it to route signals, calculate values and generate output. To program these kinds of chips there are two main Hardware Description Languages, VHDL and verilog. They describe how the signals in and out of the FPGA and its components connect and behave. Another advantage of the FPGA technology is that time critical functions can be moved from software to hardware to ensure their requirements and don't need a faster, more expensive cpu solution.
3.1 Component
A type of building block that is used in larger FPGA designs. This helps in top down losophy in that several components can be speci ed and then developed in parallel. Nowadays there are companies that specialize in development of Intellectual Property or IP components for others to use in their products. This reduces development time which is crucial with today's time to market demands.
3.1.1 Joystick
The joystick component connects to the PLB bus to transfer the input signals from the external pins of the FPGA to the CPU. It also contains its own button debouncer to ensure a clear signal from the button.
The debounce works that when the signal changes from 1 to 0 or from 0 to 1 it uses a counter that operates at 62.5 MHz that counts to 324988 which is equivalent to 0.00052 seconds. This eliminates the unsteady signal that often is assosiated when a button is pressed. It can be solved via analogue lters but this solution is simpler and just as e cient since it only delays the signal 0.52 milliseconds, for reference see,B.1.
3.1.2 Pwm
The PWM component is also connected to the PLB bus. Like the joystick bus it doesn't write to the bus, it reads from it to get the value of the register to actuate the PWM pulsewidth between ve and ten percent that corresponds to 0 to 90 degrees for the servos.
The pulse has a xed frequency of 50 Hz, this is achieved by dividing the system clock at 62.5 MHz by 125 into 2 microseconds per tick, which equals 500 pulses per millisecond The inner counter counts to 10000. The rst 500 are 1, the next 500 are regulated by the value in the registry whilst the last 9000 pulses are 0. This ensures a 50 Hz PWM with 5 to 10 percent dutycycle for reference see, B.2
3.2 Embedded systems
These are computers that lack the ordinary computer IO such as mouse, keyboard and a monitor, these systems can be found in smart devices in almost every modern household; everything from dishwashers to mobile phones are labeled embedded systems.
When developing these systems companies often use FPGA-circuits to house an entire system part of the system within one board. This helps developer when testing new hardware with fast implementation and debugging capabilities.
3.3 Xilinx Spartan-3AN
The board contains the following devices and connections.
Xilinx Devices
• Spartan-3AN (XC3S700AN-FG484) • Platform Flash (XCF04S-VOG20C) Clocks
• 50 MHz crystal oscillator on-board • Open slot for optional user-installed clock Memory
• 4 Mbit Platform Flash PROM • 32M x 16 DDR2 SDRAM • 32 Mbit parallel Flash • 2-16 Mbit SPI Flash devices Analog Interface Devices
• 4-channel D/A converter • 2-channel A/D converter • Signal ampli er
Connectors and Interfaces • Ethernet 10/100 PHY • JTAG USB download port • Two 9-pin RS-232 serial port • PS/2-style mouse/keyboard port
• 15-pin VGA connector capable of 4,096 colors • One FX2 100-pin and two 6-pin expansion connectors
• 20 user I/O available on standard header pins • Stereo mini-jack for PWM audio
• Rotary/push button function switch • Eight individual LED outputs
• Four slider switches, four push-button switches. Display
• 16 character, 2-Line LCD
[6] Of these the only ones used are the spartan-3AN, the extarnal clock, the DDR2 SDRAM, one RS-232 and 20 user I/O header pins.
Figure 7: Here one can see the spartan-3AN board with the connectors on the J2 an J15 ports connected.
3.4 Soft-microprocessor
The development of FPGA-technology has come so far that an entire central processing unit (CPU) can be housed inside the FPGA, this has led to the development of so called soft microprocessors smaller CPUs that operate at megahertz compared to modern day systems that often operate at gigahertz clock frequencies. There are a couple of them that are more frequently used, nios II supported by altera and pico and microblaze supported by Xilinx. Both vendors ship their respective soft microprocessor with the development platform for their FPGAs.
3.5 Microblaze
Microblaze is a 32-bit soft microprocessor RISC design and since we are using an FPGA from Xilinxs the Microblaze soft microprocessor was the obvious choice. Since version 7 of Microblaze there has been an optional memory management unit which can be selected. This enables Microblaze to run more complex operating systems such as Linux. This system is con gured to run at 133 MHz CPU clock and a bus clock frequency is 62.5 MHz.
4 Software
4.1 Development platform
Xilinx ships a platform for the hardware construction, con guration of Microblaze and a compiler for simpler programs. It includes guides such as the Base System Builder (BSB) that aids the developer in con guration of a soc with the development boards drivers and how they interconnect with each other.
4.2 Operating systems
An operating system makes it possibly to run multiple programs on the same cpu giving the impression that they are running simultaneously. The operating system gives the programs dierent priorities de-pending on importance and then runs them dede-pending on the current scheduling strategy. Among them are rst in rst out or round robin. This also enables multiple users to use the same computer to run programs at the same time.
4.2.1 Real-time, non-real-time
When the requirements of the computation demands more deterministic behavior, the operating systems often have to be replaced by a more specialized one, a real-time operating system (RT-OS). These are very deterministic compared to ordinary OS's. RT-OS are used in applications that can't aord that a program is not executed before its deadline, for example an engine management system in a car.
The RT-OS's are divided into two subgroups, hard and soft. The hard ones are for applications where a deadline miss can result in injuries or damages as in a airplane and soft are more relaxed and a deadline miss might result in a glitch in the audio of a media player.
4.3 Linux
A kernel originally written by Linus Torwalds in the early 90s used in several distributions commonly known as Linux based operating systems. The kernel is very agile and portable to almost any hardware-platform and since version 7.1 of the Microblaze architecture (when it was enriched with a mmu) the full Linux is able to run on the soft microprocessor.
5 Linux and Embedded systems
Many developers are beginning to show interest in using the Linux kernel in their products. This opens ways for end users to modify their devices to suit their needs, such as Linksys in their routers or porting linux to hardware not delivered with linux like porting Google Android to smartphones.
5.1 The platform
The system was composed via the base system builder where a Microblaze 7.10 CPU was equipped with 2 KB instruction and data cache, which was deactivated during the benchmark tests. The bus was connected to several slave devices: DDR ram, RS232, Ethernet, Bram, Interrupt controller and a timer. All of them interupt driven.
The FPGA is externally coupled so that the joystick is connected to the external J2 port and the PWM channels are coupled to the J15 port.
The CPU was operating at 133MHz and the bus at 62.5 MHz, both the joystick and the PWM gen-erators use the same clock as the Bus.
J2
Internal External Function joystick_0_Joystick_in_pin<1> A4 reset macro joystick_0_Joystick_in_pin<2> A5 store macro joystick_0_Joystick_in_pin<3> B6 macro joystick_0_Joystick_in_pin<4> A6 slow joystick_0_Joystick_in_pin<5> A7 axis2 joystick_0_Joystick_in_pin<6> A8 axis1 joystick_0_Joystick_in_pin<7> A9 right joystick_0_Joystick_in_pin<8> C10 left joystick_0_Joystick_in_pin<9> A10 up joystick_0_Joystick_in_pin<10> A12 down J15
Internal External Function pwm_0_Pwm_out_pin AA3 base pwm_1_Pwm_out_pin AB2 turb
pwm_2_Pwm_out_pin AA4 lower middle pwm_3_Pwm_out_pin AB3 wrist
pwm_4_Pwm_out_pin AB6 tilt pwm_5_Pwm_out_pin AA6 gripper
Figure 8: The board with the connectors to the j2 and j15. they are connected to the PCB via two 2x17 oppy header connectors.
5.2 PetaLinux
A software development kit that is composed by PetaLogix, a company specializing on Xilinx based Linux systems. The software development kit (SDK) is composed of [3]:
Software
• Complete Linux kernel source
• Complete userland source code including libraries and standard utilities • Root lesystem image builder
Hardware
• Reference designs for popular FPGA development boards Tools
• PetaLogix automated BSP generator to automatically target Linux systems to a particular custom hardware platform
• Self-contained GCC cross-compiler toolchains including Linux standard C libraries • GDB cross-debugger and gdbserver target debug stub
• Application and module generators
Unfortunately the reference designs don't include the Spartan-3AN that is used in this project, so it had to be inserted manually into the development platform.
Since the computer used for the Xilinx software ran Windows Vista, the PetaLinux had to be run on Sun VirtualBox [7] and Petalogix are kind enough to provide documentation how to [4].
To make a userprogram for petalinux there is a bash-script that is called petalinux-new-app, it is invoked by petalinux-new-app name. This automatically creates a C program structure to easily import to the enviroment, since it generates the entire structure of folder, .c le and a make le the compilation is of standard gcc style by just typing
t make. When done the application is inserted into the image via the petalinux command t make romfs .
When the image is compiled with all user applications it has to be loaded onto the fpga. This is done via XMD that is provided through the xilinx development suite. it transfers the image from the pc to the embedded system via the programcable as such.
xmd
connect mb mdm
dow -data image.bin 0x8c000000 con 0x8c000000
exit
this is in the folder where the image is contained, as described in [12]
5.3 Controlling the arm
Before the entire control system was implemented the individual functions of the components were tested. This meant that a program, joystick was executed and it read the register 60 times with a delay between as seen in the source code A.1. The input is then bitshifted 5 bits to the right to minimize the data volume.
The pwm registers was tested in similar fashion, the user executed pwm followed by which channel (0-5) and how many bits to write to it (0-500) see source code, A.2 .
The controlling program, move A.3 then works by reading the values from the joystick component and editing the corresponding registers by pressing the store macro button the program stores the values from the arm and by collecting several of these points the arm can move between these states. The program stores a maximum of 255 points but it can be scaled up for future use.
"The macros are not stored, so so if the program is closed or the system rebooted the macros are gone. This because the OS is stored in RAM on the development-board. The program is exited by pressing play and remove macro simultanious.
Once the system is nished booting, log in to the system. (standard uclinux: ) To start moving the arm type move and then switch on the power to the servos. The movement is divided into 3 parts. If none of the axis selection buttons are used, the forward - backwards on the joystick toggles the gripper, left - right the rotation of the gripper. Axis 1 enables the movement of the middle part of the arm and Axis 2 moves the part connected to the base and rotates the base.
To store the movement for a macro the current position when the button is pressed is stored in a array that holds all positions, then the arm is moved to another position and once the button is pressed the new position is added to the array.
When the macro is replayed the arm moves from point a to point b in all axis at once one position at a time until it reaches the designated value for each.
6 Testing
For the testing two dierent versions of the Linux kernel was used; 2.6.18 and uclinux 2.6.20. The dhrystone benchmark was executed for a series of ten thousand to ten million times. For greater accuracy the benchmark was run 10 times for every measuring point. Because the Dhrystone benchmark is cache sensitive the cache was disabled during the tests. The benchmark which was originally published in 1984 by Reinhold P. Weicker.
As [2] states, "Dhrystone is a short synthetic benchmark program intended to be representative for system (integer) programming."
7 Results
The results show that uClinux is marginally slower than the standard kernel. For a complete series of measurements see C.
Microseconds per dhrystone run
os 10000 100000 1000000 10000000 uc 2.6 247.4 243.43 243.14 243.12 lk 2.6 247.7 242.99 243.74 242.73
Dhrystone runs per second
os 10000 100000 1000000 10000000 uc 2.6 4042,41 4107,97 4112,64 4113,48 lk 2.6 4037,68 4115,39 4120,16 4119,80
8 Summary
8.1 Conclusions
The performance loss in using uClinux 2.6.20 is so small that the gain in determinism weighs up the dierence. By enabling the cache the performance is signi cantly increased.
The performance is further enhanced when activating the cache so the performance dierence should not be noticable.
8.2 Thesis work evaluation
During this project I have learned to manage my own workload, and to plan a whole project by myself. I have worked with the Xilinx development suite, and succeeded in installing Linux on an embedded system. I would have liked to put more time on the development and features of controlling the robotic arm, rather than on the benchmarking of the system.
9 References
References
[1] http://www.xilinx.com/company/gettingstarted/index.htm, Xilinx - Getting Started with FP-GAs - 2009-10-27
[2] http://performance.netlib.org/performance/html/dhrystone.intro.html, dhrystone - 2009-10-15.
[3] http://www.petalogix.com/products/petalinux/petalinux, The PetaLinux SDK - 2009-10-15. [4] http://www.petalogix.com/resources/documentation/petalinux/userguide/
AdvancedTopics/NonstandardInstall, Petalinux Non-standard Installations 2009-09-05. [5] http://209.85.129.132/search?q=cache:_HYp9YJdY6IJ:developer.petalogix.com/
wiki/MicroBlaze-2.6-Kernel%3Fformat%3Dtxt+http://developer.petalogix.com/wiki/ MicroBlaze-2.6-Kernel&cd=3&hl=sv&ct=clnk&gl=se&client=firefox-a, Linux 2.6 on MicroB-laze - 2009-09-5
[6] http://www.xilinx.com/products/devkits/HW-SPAR3AN-SK-UNI-G.htm, Spartan-3AN starter kit - 2009-10-15
[7] http://www.virtualbox.org/wiki/VirtualBox,VirtualBox - 2009-10-15
[8] http://www.hooked-on-rc-airplanes.com/servo-tutorial.html, Servo Tutorial - How a Servo Works - 2009-10-16
[9] http://www1.jaycar.com.au/images_uploaded/optocoup.pdf,Optocouplers: When and How to use them - 2009-9-02
[10] L. Lindh and T. Klevin, (2008) HW/SW Embedded System Design with FPGA Technology Pages 26-35
[11] http://www.lynxmotion.com/Category.aspx?CategoryID=25#specs, Lynx 6 2009-9-18 [12] http://forums.xilinx.com/xlnx/board/message?board.id=ELINUX&message.id=208#M208,
A Source code: C
A.1 joystick.c
#include <stdio.h> #include <stdlib.h> #include <unistd.h> #include "../../../hardware/user-platforms/xilinx-spartan3an-edk10-1/ microblaze_0/include/xbasic_types.h" #include "../../../hardware/user-platforms/xilinx-spartan3an-edk10-1/ microblaze_0/include/xio.h" #include "../../../hardware/user-platforms/xilinx-spartan3an-edk10-1/ microblaze_0/include/xparameters.h"int main(int argc, char *argv[]) {
int count; Xuint32 temp;
for(count=0;count<60;count++) {
temp = XIo_In32(XPAR_JOYSTICK_0_BASEADDR); // read from the bus temp = temp >> 5; // bitshift 5 steps
printf("%d",temp); // print to stdout
for(temp = 1000;temp>0;temp--){} // retardation loop }
return 0; }
A.2 pwm.c
#include <stdio.h> #include <stdlib.h> #include "../../../hardware/user-platforms/xilinx-spartan3an-edk10-1/ microblaze_0/include/xbasic_types.h" #include "../../../hardware/user-platforms/xilinx-spartan3an-edk10-1/ microblaze_0/include/xio.h" #include "../../../hardware/user-platforms/xilinx-spartan3an-edk10-1/ microblaze_0/include/xparameters.h"int main(int argc, char *argv[]) { int pwm=-1, value=-1; if ((argc < 3)||(argc > 5)) printf("Systemcall: pwm 0-5 0-500"); else // { pwm = atoi (argv[1]); value = atoi (argv[2]); if(value < 0 || value > 500) { printf("Systemcall: pwm 0-5 0-500"); } else switch (pwm) { case 0: XIo_Out32(XPAR_PWM_0_BASEADDR,value); break; case 1: XIo_Out32(XPAR_PWM_1_BASEADDR,value); break; case 2: XIo_Out32(XPAR_PWM_2_BASEADDR,value); break; case 3: XIo_Out32(XPAR_PWM_3_BASEADDR,value); break; case 4: XIo_Out32(XPAR_PWM_4_BASEADDR,value); break; case 5: XIo_Out32(XPAR_PWM_5_BASEADDR,value); break; default: break; } } return 0; }
A.3 move.c
#include <stdio.h> #include <stdlib.h> #include <math.h> #include <unistd.h> #include "../../../hardware/user-platforms/xilinx-spartan3an-edk10-1/ microblaze_0/include/xbasic_types.h" #include "../../../hardware/user-platforms/xilinx-spartan3an-edk10-1/ microblaze_0/include/xio.h" #include "../../../hardware/user-platforms/xilinx-spartan3an-edk10-1/ microblaze_0/include/xparameters.h"/* the bitmasking values for the input signals */ #define up 0x0001
#define down 0x0002 #define left 0x0004 #define right 0x0008 #define axis1 0x000a #define axis2
#define slow #define macro #define macro1 #define macro2
/* home positions for the arm */ #define home0 0 #define home1 250 #define home2 0 #define home3 500 #define home4 350 #define home5 500 /* global variables */ Xuint32 servo[6]; /* struct declaration */ struct point { Xuint32 pos[6]; }; /********************************************************************* * sets the corresponding chanel from 0 to 6 with a value
* between 0 and 500
*
*********************************************************************/ void pwm(int pwm, Xuint32 value)
// same as the main function in pwm.c
/************************************************************** * translates the input from the joystick to movement of
* the robotic arm
void move(Xuint32 temp) {
if (temp & axis1) // bitmasking to check if axis1 button was pressed {
if(temp & up) // joystick up { if(servo[2] < 500) pwm(2,servo[2]+=1); else pwm(2,servo[2]); }
if(temp & down) // joystick down { if(servo[2] > 0) pwm(2,servo[2]-=1); else pwm(2,servo[2]); }
if(temp & left) // joystick left { if(servo[3] < 500) pwm(3,servo[3]+=1); else pwm(3,servo[3]); }
if(temp & right) // joystick right { if(servo[3] > 0) pwm(3,servo[3]-=1); else pwm(3,servo[3]); } }
else if (temp & axis2) // check for axis 2 {
if(temp & up) // joystick up { if(servo[4] < 500) pwm(4,servo[4]+=1); else pwm(4,servo[4]); }
if(temp & down) // joystick down { if(servo[4] > 0) pwm(4,servo[4]-=1); else pwm(4,servo[4]); }
if(temp & left) // joystick left { if(servo[5] < 500) pwm(5,servo[5]+=1); else pwm(5,servo[5]); }
{ if(servo[5] > 0) pwm(5,servo[5]-=1); else pwm(5,servo[5]); } }
else // if not axis1 or axis2 {
if(temp & up) // joystick up { if(servo[0] < 500) pwm(0,servo[0]+=1); else pwm(0,servo[0]); }
if(temp & down) // joystick down { if(servo[0] > 0) pwm(0,servo[0]-=1); else pwm(0,servo[0]); }
if(temp & left) // joystick left { if(servo[1] < 500) pwm(1,servo[1]+=1); else pwm(1,servo[1]); }
if(temp & right) // joystick right { if(servo[1] > 0) pwm(1,servo[1]-=1); else pwm(1,servo[1]); } }} /******************************************************************* * macromove repeats movement between up to 500 recorded positions *
*******************************************************************/ void macromove(Xuint32 start[], Xuint32 stop[])
{ int i,j,m; for(m=0;m<501;m++) { for(i=0;i<6;i++) { if(start[i]!=stop[i]) { if(start[i] < stop[i]) { if(start[i]<500) pwm(i,start[i]+=1); else
pwm(i,start[i]); }
else if (start[i] > stop[i]) { if(start[i]>0) pwm(i,start[i]-=1); else pwm(i,start[i]); } }
//delay for repeating the movement for(j=0;j<250000;j++){;}
} } }
int main(int argc, char *argv[]) {
int i,j,time,mlength=0,learn=0; Xuint32 temp;
struct point mac[255]; //mac[0].pos[0] = -1;
temp = XIo_In32(XPAR_JOYSTICK_0_BASEADDR); temp = temp >> 5;
/* setting the values for home possition */ servo[0] = home0; servo[1] = home1; servo[2] = home2; servo[3] = home3; servo[4] = home4; servo[5] = home5; for(i=0;i<6;i++) pwm(i,servo[i]);
while(temp != (macro + macro2) )
/* to quit press play macro and reset macro at the same time */ {
/* read data from the joystick registry */ temp = XIo_In32(XPAR_JOYSTICK_0_BASEADDR);
/* shift the bits for easier handling of input */ temp = temp >> 5;
/* move the arm according to the input data */ move(temp);
/* macro handler */ if(temp & macro) { /* fix fix fix*/
mlength = 0;
printf("\n rensar macro \n"); for(i=0;i<133333333;i++){} }
if(temp & macro1)
{ /* recording macro point by point */ if(mlength == 0) // if macro is empty
{
for(i=0;i<6;i++) {
}
mlength=1; }
else // if not empty { j=0; for(i=0;i<6;i++) { if(mac[mlength-1 ].pos[i]==servo[i]) { j++; } } if(j < 6) {
printf("\n Point added to macro \n"); for(i=0;i<6;i++) { mac[mlength].pos[i]=servo[i]; } mlength++; } } for(i=0;i<5000000;i++){} }
if(temp & macro2) // playback macro {
if(mlength != 0) // macro greater then zero points { do { for(i=0;i<mlength;i++) { /* debug code for(j=0;j<6;j++) { printf("%d",mac[i].pos[j]); } printf("\n"); */ macromove(servo,mac[i].pos); } temp = XIo_In32(XPAR_JOYSTICK_0_BASEADDR); } while(temp==0); } }
/* retardation loop instead of sleep function, coupled with the slow button */ if (temp & slow)
time = 100000; else time = 50000;
for(i=0;i<time;i++){} }
pwm(0,home0); pwm(1,home1); pwm(2,home2); pwm(3,home3); pwm(4,home4); pwm(5,home5);
printf("arm in home position, quitting"); return 0;
}
B Source code: VHDL
B.1 joystick.vhdl
Joystick_proc_0 : process ( Clk_in ) is variable cnt: integer range 0 to 524287; variable current: std_logic;
begin
if Clk_in'event and Clk_in = '1' then if Bus2IP_Reset = '1' then
-- zero all variables cnt:= 0;
current := '0';
else -- debounce
if current /= joystick_in(1) then cnt := 0; current := joystick_in(1); else cnt := cnt + 1; end if; if cnt > 324988 then joystick(1) <= current; cnt := 330000; end if; end if; end if; end process;
B.2 pwm.vhdl
begin
-- Pwm generation with a resolution of 500 bits for standard analouge servos. -- Clk_in has to be 62.5 Mhz
PWM_proc : process ( Clk_in ) is
variable cnt: integer range 0 to 127; -- variable to downclock the clkin
variable pwm_stage: integer range 0 to 10240; -- variable to count the pulses for 50 Hz begin
if Clk_in'event and Clk_in = '1' then if Bus2IP_Reset = '1' then
-- zero variables cnt :=0;
pwm_stage :=0; else
if cnt > 124 then -- count from 0 to 124 cnt:= 0; -- reseta
if pwm_stage < 10000 then -- count from 0 to 10000 pwm_stage:=pwm_stage+1;
else
pwm_stage:=0; end if;
if pwm_stage < 499 then -- the servo requires 50 Hz and a dutycycle between 5 and 10 % -- 500 pulses is 5%
Pwm_out <= '1';
elsif pwm_stage < (slv_reg0+499) then -- the variable part of the pulse between 5 and 10% -- between pulse 500 and 1000.
Pwm_out <= '1'; else Pwm_out <= '0'; end if; end if; cnt := cnt + 1; end if; end if; end process;
