DSP IMPLEMENTATION OF A-CONTROL ALGORITHM FOR A FORWARDER CRANE

Transcription

1 1 P a g e DSP IMPLEMENTATION OF A-CONTROL ALGORITHM FOR A FORWARDER CRANE Naga Praveen Parchuru Jagadeesh Thati This thesis is presented as part of Degree of Master of Science in Electrical Engineering Blekinge Institute of Technology December 2009 Blekinge Institute of Technology School of Engineering Department of Electrical Engineering Supervisor: Anders Hultgren Examiner: Anders Hultgren

2 2 P a g e Symbols θ 1 θ 2 θ 3 d 4 The swivel arm angle The lift arm angle The elbow arm angle Length of prismatic joint θ 1,θ 2 Angular velocities of both the links d 4 Joint velocity of the extension link J(q) v O 4 Jacobian function Velocity function The crane tip position r, z Cartesian coordinates in r and z coordinates d d 4 The desired prismatic joint velocity θ 2m, θ 3m, d 4m Maximum joint velocities of the joints k d 4c d 4e T Gain parameter which is adjustable The Center position of the prismatic joint The End position of the prismatic joint The Transformation matrix

3 3 P a g e Foreword/Acknowledgement First, we thank our respected Supervisors, Mr. JONAS LINDHOLM, Mr. ANDERS HULTGREN and Mr. BJORN NILSSON who guided us in right way to accomplish this project in time. Whole hearted thanks to Dasa Control Systems AB, Vaxjo University, Blekinge Institute of Technology Sweden as well as Rottne Industry AB, for providing us such a good platform for carrying out the course and project in a satisfactory manner. Finally, our gratitude to our all friends and teachers, those who gave help and assistance needed. God bless all of them and we express our deep gratitude. Naga Praveen Parchuru Jagadeesh Thati December 2009.

4 4 P a g e Table of Contents 1 INTRODUCTION Forwarder Introduction Area of research Review of State of Art Problem definition Goals Tools Required Contents of Thesis BACKGROUND Code Composer Studio (CCS) D5 Developer Software Automatic Conversion Tools Crane Geometry Crane Coordinates Forwarder Kinematics Today s Forwarder Operators AUTOMATIC PRISMATIC JOINT ALGORITHM IMPLEMENTATION The Jacobian Matrix The Weighting Function and Gain parameter Parameters Chosen for Algorithm Implementation RK-62 PARAMETERS DASA LAB TESTING ENVIRONMENT d5dcl... 25

5 5 P a g e 5.2 d5designer d5bundler dasa5 system units d5CI12 Base Unit TFT Monitor d5mc (Main Controller) Unit d5io12 Unit d5io48 Unit Sensors THE CRANE LAB SET UP ENVIRONMENT Lab Crane set up with dasa Equipment Physical connections at the Crane Lab with d5 Equipment Using the dspace system in the Crane Lab Set Up Replacement of dspace system with dasa5 Equipment AGILITY MCS SYNTHESIZER TOOL Creation of C code automatically Agility MCS Characteristics Agility MCS Extensive Sustainability to Matlab Agility MCS Synthesizer Block Diagram CONCLUSIONS REFRENCES RESULTS UML DIAGRAMS CODE DOCUMENTATION.60 Appendix

6 6 P a g e

7 7 P a g e 1. INTRODUCTION 1.1 Forwarder Introduction: In the design of automated heavy vehicles, especially in the areas of control systems and robotics, the engineers and researchers meet exiting challenges. The field of automation has gained its importance making operation of heavy vehicles much easier e.g., in the design of forwarders. Forwarders are used for handling timber in forest. Still there is much to do in order to improve the forwarder operators working condition. The main task for a forwarder operator is to control the crane collecting logs from the ground and loading the forwarder wagon, or unloading the forwarder and putting the logs in piles. While controlling the crane the operator has to handle several joy stick functions. The main objective for this work is to improve the working condition for the forwarder operator, by means of partly automate the crane. This thesis project is about implementation of an algorithm for automation of one crane link i.e., the prismatic link. 1.2 Thesis Project: This thesis mainly concentrates on implementation of the algorithm of the automatic control of the prismatic link of a crane forwarder. The implementation is made in a Texas DSP as a part of the control system for the forwarder. The algorithm is called Software 1A. It should be implemented using C/C++ programming language. The algorithm is developed in a research project* within forwarder crane control at Linneus University and Blekinge Institute of Technology. The algorithm is tested by use of a dspace system controlling a laboratory crane. The code is available as Matlab and Simulink code.this code needs to be made compatible to DSP code that is needed to be implemented in the Code Composer studio (CCS). With the help of dasa5 system equipment which is provided by Dasa Company, we build the user interface for the desired algorithm developed in Code Composer Studio (CCS) for the operation of the crane links. The developed algorithm should first be tested in the Dasa lab and then it should be tested in the crane laboratory at the Linneus University in Vaxjo. 1.3 Current Ongoing Projects on Cranes: Various current projects are mainly concentrated on improved control algorithms for the cranes, which make use of automation as the means of driving force. This helps to reduce the time consumption and speeds up the operation and makes itself operator friendly. Research projects within control of forwarder cranes are taking place at several places, e.g. Umea University[1] and Swedish Royal Technical University[2]. At Vaxjo University a set of thesis works and within the forwarder crane project has addressed different aspects of modeling issues, see[3]. [4], [5], [6], [7], [8].

8 8 P a g e 1.4 Problem Definition: The thesis task is to implement the algorithm for the automatic extension link into a proper Dasa computer system in order to control a forwarder crane. The implemented code should be tested at the laboratory crane at Linneus University, Vaxjo. 1.5 Goals: The main objective of this thesis is to develop Software 1A in C/C++ programming language, which is obtained from the Matlab code. This developed Software 1A should be able to be interfaced with the Code Composer Studio of the Texas Instruments along with the dasa5 system which able to run the algorithm. The goals of this thesis can be divided into mainly three parts. Firstly, we should develop the lab testing environments at dasa and verify the results using the Visual studio C++.Secondly, we need to verify the developed algorithm in the crane lab in the practical environment. Lastly, we need to design the parameters which could suite the real crane environment with a suitable forwarder. The forwarder will then be used for the tests of the implemented algorithm. 1.6 Tools Required: Implementation of our project urges to work on specific software which is best suited for the target system for its efficient functioning. We used the C-code in Visual Studio C++ software programming language that could be made compatible for the Code Composer Studio (CCS) of Texas Instruments to execute the result. Digital Signal Processing applications can also be performed with the use of software that is provided by Texas Instruments. User friendly environment is provided by the Code Composer Studio where in which the C programming is used. The DSP processors of family TMS320C6712 is of in specific. The dasa5 software which is the software provided by dasa company on its own, is also made used in implementing the crane tip control. The graphical user interface for the crane control is implemented in the Dasa5 software typically. The GUI environment with the proper assignment of parameters those which can be also configured using the dasa software. The parameters of choice are used in the designing of the code in the Code Composer Studio (CCS) and are interfaced with the dasa5 software for the functioning of the crane from the operators point of view. The parameters can be such as buttons, voltmeters, ammeters, power meters, and indicators etc which are available from the dasa developer software. These can be interfaced with the Code Composer Studio end with developing the code for automatic crane tip control.

9 9 P a g e 1.7 Contents of Thesis: In the following chapters of the thesis we have discussed various topics and finally reached the goals. In chapter 2, the main concentration is about the background of software that was used, in order to implement the algorithm. The kinematics of the crane is also discussed in this chapter which may include the working of the crane, forwarder kinematics and about the present forwarders that were in use in forestry purposes. Thus this chapter briefs the core idea to proceed further in accomplishing the goals. In chapter 3, is mainly concerned on the prismatic link algorithm development. This chapter also includes the parameters that were chosen to be configurable. In chapter 4 the explanation of RK-62 crane parameters is carried out. Specifically the various parameters of the crane links are discussed. In chapter 5 a more detail discussion is given about dasa equipment and all their functionality. In chapter 6 the clear idea about the crane lab environment that could give us the clear picture of the physical set up of a real crane which was provided by the Rottne Industry AB at the Vaxjo University. Detail scenario of the physical connections of the real crane is studied. Replacing the old dspace system with the new dasa equipment is also studied. In chapter 7 the Agility MCS tool is discussed in detail. Thus the knowledge about the efficiency, the method of conversion and the utilities is discussed in detail. In chapter 8 the conclusion of the project of the automatic extension link of the crane is given,chapter 9 we list out the references and in chapter 10 discuss the graphical representations UML representations of the software implemented in this thesis. In chapter 11 the results and in chapter 12 code is documented lastly. Finally bring out the relationship between the algorithms of the automatic extension link and relate with the code implemented through the UML diagram representation is of higher priority. Then the code I commented is in the readable format so that we have the better understanding of the code that we have implemented.

10 10 P a g e

11 11 P a g e 2. BACKGROUND 2.1 Code Composer Studio (CCS): Code Composer Studio (CCS), provides the necessary software support tools. It gives an integrated development environment, bringing together the C compiler, assembler, linker, and debugger. It has graphical capabilities and support real-time debugging capabilities. Thus the Code Composer Studio is an easy to use software tool to build and debug programs. The code composer studio includes the tools for code generation which is its explicit characteristics. Thus we develop the C code in this environment and are interfaced to other software simultaneously. 2.2 d5 Developer Software: The d5 developer is a combination of various developing tools for developing, configuring, testing, and troubleshooting purposes. It comprises of five sub functions software, which are d5dcl, d5 Designer, d5access, d5 Analyzer and d5 Bundler respectively. In our thesis we mainly concentrate on d5dcl, d5designer and d5bundler software. This software is used to create the graphical user interface for the program logic that has been developed in the Code Composer Studio. Therefore it is necessary for the crane logic to have this interface, as the main goal of our thesis is to replace the dspace system with the dasa control systems. We describe about each developing tools clearly in the chapter Automatic Conversion Tools: The tools needed for the automatic conversion of Matlab code to the C code are to be investigated. These tools must also be investigated in terms of efficiency and less time consuming procedures in the process of conversion. Agility MCS synthesizer is one among those tools that is investigated in specified terms. This automatically converts the Matlab files to the Desired C code. Agility MCS synthesizer can be known in detail in the later sections of the report. The Matlab itself can also be a tool itself that can be used in the conversion procedure but is proved to be less efficient with student version software. But, we have not done enough work in working with these tools in our thesis. We now continue our discussion with the crane kinematics and the geometry that would be the basic building blocks of the project and implementation of the algorithm. 2.4 Kinematics of the Crane:

12 12 P a g e Crane Geometry: Lift arm Elbow arm θ 3 θ 2 d 4 z Swivel arm O 4 θ 1 r Figure 1: Forward crane geometry in two dimensions. In Figure 1 the forwarder crane is represented in two dimensions. The swivel joint θ 1 is left out in our case in order to be able continue our presentation into the two dimensional case. The crane tip position, O 4, can be represented in Cartesian coordinates, r and z or in terms of crane coordinates θ 2, θ 3 and d 4 from the Figure 1. From the crane geometry we have θ 1, θ 2 and θ 3 are the angles of rotation at the links. The value θ 2, θ 3 reprensent various links such as swivel arm,lift arm and elbow arm respectively. Thus the value of angles of rotation can be the parameters of choice. Where, θ 1 is the swivel arm angle in radians, θ 2 is the lift arm angle in radians, θ 3 is the elbow arm angle in radians, d 4 is the length of the extension boom in length (meters), and, O 4 is the crane tip position.

13 13 P a g e Crane Coordinates: y 2 x 3 x 2 y 1 θ 3 z 3 x 1 d 4 θ 2 θ 1 z 1 θ 2 z O 4 θ 1 z 0 x 0 r Figure 2: Crane coordinate according to Denavite-Hartenberg Table Forwarder Kinematics: The forwarder kinematics can be derived using the methods presented in [9]. According to Denavite- Hartenberg the transformation matrix T, transforming the coordinates between two subsequent coordinate system, from system i to system i-1, is given by the following relation as, T i 1 = T i i 1 = cosθ i sinθ i cosα i sinθ i sinα i a i cosθ i sinθ i cosθ i cosα i cosθ i sinα i a i sinθ i 0 sinα i cosα i d i (1) Denavite Hartenberg uses four parameters in the transformation matrix. The coordinate systems for the crane can then convenient be presented in a table with four parameters per the crane link, see Table 1.

14 14 P a g e The corresponding D-H table is given in Table 1. DH-table, Crane θ d a Swivel arm, link1 θ 1 = 0 d 1 = d 1 a 1 = 0 1 = π 2 Lift arm, link2 θ 2 = 0 d 2 = 0 a 2 = a 2 2 = 0 Elbow arm, link3 θ 3 = 0 d 3 = 0 a 3 = 0 3 = π 2 Extension link, link4 θ 4 = 0 d 4 = d 4 a 4 = 0 4 = 0 Table 1: Denavite-Hartenberg table for the crane considered. Each of the four parameters is used in a simple transformation matrix in order to generate the eq(1). The simple transformation matrices are given by relation as, T 2 = T 1 = T 0 = cosθ sinθ sinθ 0 cosθ cos (alpha) 0 sin (alpha) a cos (alpha) 0 cos (alpha) and, T 4 = d 0 1 are the transformation matrices.

15 15 P a g e The transformation from the O 4 coordinate system to the O 0 coordinate system is given by the relation as, T 4 0 = T 0 T 1 T 2 T 3... (2) T 4 0 = cos( θ 2 + θ 3 ) 0 sin ( θ 2 + θ 3) 1 sin ( θ 2 + θ 3) sin ( θ 2 + θ 3) a 2 cosθ 2 + d 4 sin (θ 2 + θ 3 ) cos( θ 2 + θ 3 ) a 2 sinθ 2 d 4 cos (θ 2 + θ 3 ) cos( θ 2 + θ 3 ) d 1 + a 2 sinθ 2 d 4 cos (θ 2 + θ 3 ) 0 1 The O 4 i.e. the origin in the fourth coordinate system, is given in the (O 0, x 0. y 0. z 0 ) system by the relation as, p 0 = T 0 T 1 T 2 T 3 O 4 The forwarder kinematics for the crane when θ 1 = 0 is given by the relation as, p 0 = x 0 z 0 = a 2 cosθ 2 + d 4 sin (θ 2 + θ 3 ) d 1 + a 2 sinθ 2 d 4 cos (θ 2 + θ 3 )...(3) This forwarder kinematics, when the swivel angle theta1=0, is a mapping from the three crane coordinates, theta2, theta3, and d4 onto the Cartesian coordinates x0 and z0. The mapping clearly shows the non-linear function from the crane coordinates to Cartesian coordinates. The mapping will be used in a later section, but first the today s way of crane control is introduced Today s forwarder operators: The operator controls the crane through joysticks. The operator s joysticks directly control the oil flow to the hydraulic valves in the crane to which the powerful hydraulic system is attached. Each joystick controls the oil flow in each hydraulic cylinder attached to each link. The control actuator signal, used by the operator, is then approximately the crane joints velocities. The operator need to be skilled in order to control the crane tip, O4, in an accurate and fast way.

16 16 P a g e

17 17 P a g e 3. PRISMATIC JOINT ALGORITHM IMPLEMENTATION In the field of study Laute and In den Kleef has recorded the operators way of controlling the crane while loading and unloading logs, see [8]. In figure 3 and 4 the correlation between the extension boom, expressed by the crane coordinate d4, and the first boom and the outer boom, expressed by the coordinates theta2 and theta3, are shown. The correlation achieved for one operator is shown in Figure 3 and for another operator in figure. Figure 3: Measurement of links velocities for a working forwarder for operator 1, from report by Laute and In den Kleef. Figure 4: Measurement : Measurement of links velocities for a working forwarder for operator 2, from report by Laute and In den Kleef..

18 18 P a g e In the measurement of the angular velocities it can be seen that the velocities of the extension boom are highly correlated with the first boom and the outer boom angular velocities, e.g., positive velocity of the extension boom is correlated with negative velocity of the first boom. The suggested automatic system uses the found correlation in order to partly automate the crane control. In order to develop the prismatic joint algorithm we need to consider some intermediate functions in achieving the goal. Those intermediate functions include Jacobian Matrix, The gain parameter, the weighting function and the angular velocities of the various links which are discussed in the following sections 3.1 The Jacobian Matrix: The Jacobian for the crane operating in two dimensional space spanned by (r, z ), can be obtained by taking the derivative of the corresponding forwarder kinematics of the crane, eq(3). Cartesian velocity equals the Jacobian times joint velocity. The derivation is performed when swivel joint is considered to θ 1 = 0 and the coordinates x 0 coincides with the coordinate r. The Cartesian velocity relation can be explained in the following relation as, v = J q q (4) Where v is the Cartesian velocity and q matrix representing the angular velocity of the each links. Where q is q = θ 2 θ 3 d 4. By taking the derivative of the left hand side and the right hand side of (3) the Jacobian cab be derived. The Jacobian Matrix is given by the relation as, J(q)= a 2sinθ 2 + d 4 cos (θ 2 + θ 3 ) d 4 cos (θ 2 + θ 3 ) sin (θ 2 + θ 3 ) a 2 cosθ 2 + d 4 sin (θ 2 + θ 3 ) d 4 sin (θ 2 + θ 3 ) cos (θ 2 + θ 3 )...(5) We now proceed in discussing to the other functions that are involved in building the algorithm of the crane. The Cartesian velocities in two dimensions case is given by the relation as follows, v = v r v z = d dt r z = d dt x 0 z 0 (6)

19 19 P a g e This can be calculated as follows, v = J q q Where q = θ 2 θ 3 d (7) The values of the angular velocities can be obtained from the sensors. In the development of the algorithm the maximum joint velocities are also consideredθ 2m, θ 3m, d 2m. 3.2 The Weighting Function and Gain parameter: A weighting function is introduced in order to take into account the prismatic link position and velocity. The weighting function is given by, d 4, d 4 = d 4e d 4 d 4e d 4c, d 4 > d 4c & d 4 > 0 1, d 4 > d 4c & d 4 < 0 1, d 4 < d 4c & d 4 > 0 1 d 4e d 4 d 4e d 40, d 4 > d 4c & d 4 > 0 (8) Where d 4c the centre is position of the prismatic joint and d 4e is the end position of the prismatic joint Thus these equations are a part of the algorithm for the desired prismatic link that should be implemented in the dspace system previously. The algorithm should to be implemented in the Code Composer Studio environment. This software here is known as the software 1A. 3.3 Parameters Chosen for the Algorithm Implementation: The operator selects the parameters of his choice in order to operate the crane. The operator is provided with a display screen where the values can be configurable. Firstly, we need to deduce which parameters we need to consider that are needed to be modified. Care should be taken in choosing the parameters and understand the significance In the D-H table we have the angles of rotation and the prismatic joint length, the link twist angle α and the link length a 2 are explanied..the link a 2 has one of the parameters of choice.

20 20 P a g e Then the gain parameter k for the parameter of choice is also considered. The value of k is deduced from the desired prismatic joint velocity. The parameters must be made configurable and the programming is developed according to the parameters of our choice to reach the desired goals of the automatic extension link. Algorithm associated parameters Description of Parameter d 4 Joint Velocity of the Extension link. d d The Desired Prismatic Joint Velocity. 4 d 4c The Centre Position of the Prismatic Joint. dd 4 desire k The desired values of the prismatic link velocities. The scalar gain parameter constant a 2 The length of the lift arm link of the crane in meters d 4 Length of the Prismatic Joint Table 2: The algorithm parameters that were illustrated in tabulate form.

21 21 P a g e

22 22 P a g e 4. RK-62 PARAMETERS Figure 5: The RK-62 parameters which we need to be implemented in the Crane lab set up. The algorithm implemented is now tested in the lab environment. The RK-62 parameters are the real crane parameters where in which we set the algorithm implemented for the actual physical crane. The elbow arm, lift arm and the extension link are chosen from the given data sheet in order to operate the crane. The gain parameter and the lift arm are the two parameters of choice in our algorithm. These parameters could be explicitly varied on the operator s choice. When we deal with rk-62 parameters, we may have more numbers of parameters in picture. Thus the choosing the parameters could be a crucial thing in the implementation in the real crane scenario. The values in the dasa lab testing carried out which will be in the following sections must be modified with the RK-62 parameters so that we need to make our algorithm run with the rottne crane available at the crane lab. From the Figure 5 we can deduce the RK-62 parameters those to be implemented in the control algorithm. The parameter specifications from the RK-62 crane can be stated as the gain parameter, the length of the lift arm a 2, the length of the extension link d 4 and the maximum joint velocities of the crane.

23 23 P a g e

24 24 P a g e 5. DASA LAB TESTING ENVIRONMENT The dasa lab testing of the algorithm is carried out using the Visual Studio C++ software initially. We need to verify the simulations of the algorithm of the automatic extension of the prismatic link in the dasa environment. With these valid simulations we need to proceed to the crane lab for the practical crane issues of operating the crane links. The algorithm is written in this environment using all the functions and calculations available in the algorithm. The algorithm written needs to be verified for the errors and the errors needed to be resolved. When the algorithm is made to compile and the errors are eliminated, and then the process of debugging is initialized. The whole code is debugged and the results at each output are verified. The debugging process also gives us some errors and they need to be resolved and we should continue this process till we obtain desired results. Once sure of the results obtained after the debugging process are valid, the development of desired algorithm for running the crane of operator s choice is ready for the simulation. This code is now sent into the Code Composer Studio (CCS) for making it DSP processor compatible which is the main focus of our project. Then needs to check the algorithm for errors and make it errors free even in Code Composer Studio environment. Then once sure of the algorithm is error free we need to get it with the dasa5 software, which builds a user interface for the code to get the crane links to operate and obtain the desired results to reach our first goal. In the following sections the dasa5 software sub functions such as the d5dcl, d5designer and d5bundler are briefly discussed which are the part of achieving our goal in the crating of user interface for the logic we develop.

25 25 P a g e 5.1 d5dcl: It is a programming tool for establishing application linked DCL code. We configure descriptions of component parts, I/O specifications, variable and control logic. Figure 6: The d5dcl window of the logic development for the crane link parameters. 5.2 d5designer: It is the tool for composing customized user interface. It communicates with the operator, display of operational knowledge and images for modifying and this study.

26 26 P a g e Figure 7: The d5designer window which provides the graphical user interface. 5.3 d5bundler: It is the software that collects MMI, DCL packages and an access file and sender to dasa5 main controller. Those files which are collected are sent into a distribution file which serves as the gateway communication between d5developer and the dasa5 microcontroller. Figure 8: The d5bundler which bundles the various d5 software and links to the dasa equipment. 5.4 dasa5 System Units: The physical units of the dasa equipment comprises of different I/O units, Main Controller and base units those can be seen in detail in the following chapters d5ci12 Unit: This is capable of doing time critical mathematics in real time scenario. These may include as bucking calculations in forestry machinery. It also has 12 programmable inputs which are digitized in general. Some of the inputs such as voltage inputs, frequency inputs, 2 channel pulse inputs or counters can be programmed.

27 27 P a g e Figure 9: The Physical structure of d5ci12 unit TFT Monitor: It is a mobile windows-based display monitor developed for the suitable environment of heavy vehicle. These units are now used for communicating with each other in the physical environment in the real crane scenario. Figure 10: The monitor screen display at the dasa lab for verifying the simulations d5mc (Main Controller) Unit: It comprises of a DSP processor for controlling the remaining units through a CAN bus to the machine, Together with an X-scale processor for controlling the communications and the operator interface. Depending upon the operator choice there can be two or three CAN buses, which are able to connect to number of machine function receivers or senders.

28 28 P a g e Figure 11: The main controller unit d5mc for controlling the signal flow d5io12 Unit: I/O units which contain 12 independent programmable inputs or outputs, of the inputs are current, voltage, frequency, counters and outputs as PWM for servo values, digital & analog and with Max rating 3A. Figure 12: The d5io12 input-output unit in the dasa equipment d5io48 Unit: It is also an I/O unit for an extent of 48 digital outputs and 48 digital as well as analog inputs for low power applications.

29 29 P a g e Figure 13: The input-output unit d5io48 of the dasa equipment Sensors: Figure 14: Sensors for the dasa equipment.

30 30 P a g e

31 31 P a g e 6. THE CRANE LAB SETUP ENVIRONMENT 6.1 Lab Crane Set Up With the Dasa equipment: The physical crane setup is kept at the Vaxjo University which is provided by the ROTTNE INDUSTRII AB. We need to replace the existing dspace system with the dasa5 equipment for running the crane. The modules such as d5mc, d5io48, d5io12 and d5ci12 units are needed along with CAN buses do the Dasa equipment. The software which we develop is called to be Crane Box that is needed to be en suited in the d5ci12 base. We need to control the crane by using joysticks according to the operator s choice. The different parameters needed to be configurable according to the operator choice, are needed to be configured by the d5 equipment. There are also some important units such as mounting of sensors at the setup and assignment of proper ID tags for the proper signaling of the systems is done with utmost care. Therefore we shall see in detail are the interconnections in the crane lab setup in the following explanation. The Crane Box unit is interfaced with the three sensors Theta2, Theta3 and d4 with the sref, S90, S00 connections of the Port A. The Port B gets the signals from the d5 MC unit and an ID Tag. The d5 MC unit is interfaced with the Crane Box with the CAN buses of 120 Ohm resistance. The d5 MC unit is also interconnected to the d5io48 unit with the PWR/IO. The d5io48 unit is also connected to an ID Tag as well as to the Crane Box unit with the 120 Ohm resistance. The d5io48 unit is interfaced with the left joystick of the left Dsub female pin through the Port A. The port B is interconnected with the right joystick through the right Dsub female pin. Thus by using these joysticks the operator controls the crane of his choice. The d5io12 unit is interfaced with the main controller unit through the CAN bus which is specifically CAN1 Bus. This in turn connected to the crane valves which comprises of 8 current outputs to the crane set up. The sensors mounted on the crane were connected at various links and we get the desired outputs feedback from them. The interconnections can be made with at most importance because of the previous dspace system connections. We also should take at most care about the hydraulic pressure pump while we are setting it open and closing the valves while operating the crane. Key Aspects, Mounting the sensors. Switching the dspace to Dasa equipment. Handling the pressure pump. Right transfer of code to the source system at the lab, and Joysticks.

32 32 P a g e 6.1.1Physical connections at the crane lab: Theta2: sensors Theta3: sensors d4: sensors Crane Setup +yellow +Red +yellow +Red +Blue +brown +Green +Brown +Green Valves (8pcs) 8Current Out sref 8 sref PWR 24v S00 s90 GND 2mcables (connector) d5io12 GND PWR 5V tags Idtag=1 Port A : Id CAN1 bus Tag +8 Crane Box Port B CAN2L 0,5 m CAN2H 0, 5 m ohm +5 CAN d5mc PWR20,5 m CAN2L 0,5m CAN2H0,5m GND0,5 m ohm PWR20,5 m +12 PWR/IO +3 GND0,5 m GND CAN/PWR Port B Same pinning as for port A left: Dsub female Left: Joystick IO48 +2 X +1 +cyan +black +12 Y +6 3: IdTag Port A +3 Z V +3 Right: Dsub female Right: Joystick Figure 15: Overview of the lab crane system with dasa5 system

33 33 P a g e SL NO Notation in Physical Connections Description of each Notation 1 Sref The zero position signal line for the sensor of 24 Volts. 2 S90 The signal but is delayed by 90 degrees for the sensor of 24 Volts. 3 S00 The signal line for the sensor of 24 Volts. 4 GND The ground for the power supply. 5 PWR The power rating for the sensor. 6 Id Tag Provide a unique ID for the d5io12 UNIT. 7 2:Id Tag Provide a unique ID for the CRANE BOX. 8 3:Id Tag Provide a unique ID for the IO48 UNIT. 9 Right Dsub The Joystick controller signal at Port A of d5io Left Dsub The Joystick controller signal at Port B of d5io CAN 1 The bus structure for data transfer at d5io12 and d5mc. 12 CAN 2L The CAN 2 LOW. 13 CAN 2H The CAN 2 HIGH. Table 3: The connections in the crane laboratory that were illustrated in tabulate form.

34 34 P a g e 6.2 Using the dspace System in Crane Lab Set up: CRANE SETUP Crane SENSORS Values Theta2, theta3, d4 8 current outputs 8 Joystick s DSPACE system Crane Control Software d5i012 CAN bus d5mc Figure 16: Overview of the lab crane system with dspace system

35 35 P a g e In the lab crane system, various units are interfaced with the actual crane. Mainly a dspace system where in which the crane controls software is implemented previously. The setup include a d5mc (main controller), d5i012 base, crane values, sensors, joysticks for the operator. The setup is the physical implementation where the dspace systems interface with firstly the control signal from the joysticks of the user end and secondly the sensors through the parameter of the choice are operated automatically and thirdly with the dasa software s. At the lab provided with a crane which is of the ROTTNE INDUSTRII AB Company. In the Lab environment the crane, is mounted on a well settled base which is in equilibrium position. The crane set up is provided with the hydraulic pressure pump generator through which the necessary oil pressure is sent to the crane. The crane is provided with the necessary electricity and is plugged in to the power supply. The crane set up is provided with the dasa5 system units, d5 MC which is the main controller which is equipped with the d5 software which provides the user interface with the crane setup. This has the micro controllers and the micro processors for the communication with the other devices. The d5mc unit is connected to the d5ci12 base through the CAN buses where in which the input and output operations are carried out. The signaling and controlling operations are generated to the dspace system, the joysticks and to the sensors. The dspace system is connected to the d5ci12 base in which the actual software is executed for the crane control. This dspace system is made to function with the SIMULINK and MATLAB programming formerly. This is evident that now we need to replace the dspace system with the new d5 system equipment in which the Crane Control SW is implemented through the C programming for the crane operation. The dspace system is further connected to the sensors and also to the joysticks for the operator. Firstly the joysticks can be controlled by the operators which are assigned with the parameters of our choice that are needed to be controlled on the crane. Secondly the sensors are connected at various links through which we obtain the feedback signals for each movement in the respective links of the crane. In specific we are having three links and we can have only 2 angles and a length, such as theta 2, theta3 and d4 respectively. Finally the crane valves which are connected to the d5ci12 base, through which provided with 8 current output vales of the crane setup in order to control the crane. The method of switching the dspace system to the Dasa system is very much of the focus also. The means of switching the dspace equipment to the Dasa equipment can know in detail from the following chapter. There are some steps involved in it to switch the equipment and they should be followed.

36 36 P a g e 6.3 Replacement of dspace System with dasa5 Equipment:. Crane Setup Sensors Valves 8pcs Theta2, Theta3, d4 8currentout 8 Crane Control SW Interface for d5 Crane Box 112 CAN bus Device drivers Interface Cane Box d5mc d5io12 CAN bus IO48 Joysticks Figure 17: Overview of the lab crane system with dasa5 system

37 37 P a g e The Dasa5 implementation is carried out with enough modifications and replacements of the various units which are present already in the crane lab setup with the dspace system quipment.as we are already sure that we need to replace the dspace system with the dasa5 equipment in operating the crane is the foremost task. This d5 implementation environment comprises of various units which include as, d5io48 unit,d5mc unit,crane Box in which the SOFTWARE 1A is implemented,d5ci12 unit,joysticks, the sensors,the CAN buses and the crane valves.these units are interfaced with slight modifications in the interconnections and are implemented at the lab crane environment. The joysticks are connected to the d5io48 unit which is an I/O unit generally use dfor low power applications.the unit is in turn connected to the d5 MC the main controller unit which comprises ofo the DSP processor for controlling and signalling various other units in the environment and is in turn interfaced with the crane control sofware, device drivers and the interface of the dasa5 equipment.the d5mc unit is further connected to the d5ci12 base which is also an I/O unit to control varoius parameters restricted within the crane set up. The sensors are connected similarly as in the crane lab set up, where they are connected at various links.the fuction of the sensors is also similar, as they send the feedback signals with some specifuc valuesto the operator.the crane valves are connected to the d5io12 base where we obtain 8 current outputs. The sensors specifications in the lab crane set up are different to the D5 implementation environment.in the case of the lab crane system we are using 5 Volts sensors whereas in the case of the dasa5 implementation we have the 24 Volts specfication sensors.this helps us to know that the dasa equipment are spacifically designed for that particular voltages only.but when we compare the dspace system in the lab crane set up we are using quite low specification sensors I the operating the crane. The dasa5 implementation is carried out along with the SOFTWARE 1A for the controlling of the crane accordiing to the operators choice. These are all the necessary modifications done to the crane set up in order to test the crane functioning.there should be also precaution measures to be taken while operating the crane such as the situations of hitting the ground and the the side way movements. 6.4 Typical crane functionality: The typical functioning of the crane is made with finding the zero positions of the both lift arm and the elbow arm of the crane and it is said to in normal mode of operation. The angles should be set to zero at the zero position of each angle.then once the zero positions are found now the crane can be made use of in automatically extending the extension link of the crane is seen. If at all the zero positions are not found then the process of stepping into the normal operation must be done again and again. While operating the crane the voltages of the sensors must be also taken into account and we need to set the maximum and minimum limits of the voltages. The utmost care should be taken that the scaling of the max and min values so that the values we obtain must not exceed.

38 38 P a g e Figure 18: Overview of the sensor voltage at dd4 display on dasa5 system software In the Figure 18 the sensor voltage at the dd4 displayed for the each change in the angles as well as the change in the extension link when they are switched back and forth. The minimum voltage and the maximum voltages are set at to according to the figure above. This voltage could be changed using the DCL configuration settings by assigning the required voltage settings. We also need to know the sensors specifications such that they may be supporting to their maximum capacity only. When we go on to operate the crane we can obtain the voltage values in mille volts on the screen and accordingly we can perform our tests on the algorithm we excecute. In the Figure 19 crane system sensor voltage as well as the current at d4 display in dasa5 system software is shown. Here the sensor voltage as well as the current at the d4 is taken into consideration with the help of some filtering as well. The dasa5 software help us building this graphical user interfaces displaying the voltages. Here also using the DCL we configure the minimum and maximum limits of the voltage as well as the current of the d4. Typically the movement of the d4 extension link is back and forth. The voltages tend to increase and decrease when the link is moving back and forth and this can be noticed in the screen displayed voltages and the respective currents which have negative and positive values. The usage of filters can give us the smoothing output response where in which we are able to track the values without any sudden fluctuations in the values in turn leads to error prone responses otherwise. Thus it is only be possible through the dasa5 DCL configurable tool where in which we can dynamically alter the values and execute the code for obtaining the desirable output responses.

39 39 P a g e Figure 19: Overview of the lab crane system sensor voltage at d4 display in dasa5 system software In the Figure 20 the DCL software is discussed in configuring the voltages and currents of the dd4 and d4 displays. The window shown above allows configuring the parameters and the respective values to them. We know can have the brief overview of configuring the parameters. In the window shown we have the main menu which includes the file menu, edit menu, view menu, project menu, tools menu and help. We create the new project where in which we configure the system variables and the respective assignments of the addresses. Once when we are sure of all the system variables are correctly assigned we then scale them as the next step. We scale the respective parameters click on each system variable and check out for each of the cell information and then we click on configure scale. In the configure scale we configure the values. If at all we need to edit the scale we then change the scale using the edit scale. We know build the program when the assignment is done to the respective parameter. Then we transfer the code to the user file in the actual program for displaying the values.

40 40 P a g e Figure 20: Configuring the sensor voltages in dasa5 software at the crane lab environment. The functionality of the crane can be viewed both in the theoretical basis and the practical basis. The sensor voltage also plays a major role in filtering out the values and displays the stable values. The 10 ms sampling time is another major concern in taking the value for that amount of time. Thus the normal operation detection, mounting the sensors, the voltage supply for the sensors at each link of the crane, and then configuring the dasa software are the major issues in the project. These all serve as the sequence of steps that should be practiced in order to operate the crane and obtain the desired automatic extension of the prismatic boom.

41 41 P a g e

42 42 P a g e 7. AGILITY MCS SYNTHESIZER TOOL Agility MCS is an automatic generator of ANSI C code from MATLAB source. This tool was designed by Agility Design Solutions which are formerly known as CATALYTIC INC. The CATALYTIC Inc. combined with CELOXICA ESL and formed a new organization named to Agility Design Solutions. It also develops C-code and FPGA-code synthesis. Its mission is to lessen the time to implement, develop and validate signal processing algorithms. Therefore agility MCS became user friendly due to its time saving approach to complex signaling in signal processing applications. It supports different toolboxes such as MATLAB communications toolbox, MATLAB signal processing toolbox and MATLAB image processing tool box. It runs in Microsoft Windows (2000 or XP) and 32-bit or 64-bit Linux (Red Hat Enterprises version 3 and 4).it requires the product such as MATLAB(R 12.1 or later). Thus Agility MCS runs successfully once we create this entire environment and gives us the optimized results. We deal in detail the procedure of conversion along with some peculiar features and characteristics of Agility MCS software tool in the further sections. 7.1 Creation of C code automatically: Agility MCS helps MATLAB users to get required C code templates of the M-code models respectively. There is some help to the user of MATLAB in the implementation of the algorithm. The Agility MCS takes off once site from manually writing C-code which proves to be tedious, time consuming job and vulnerable to errors. It creates a platform named as MATLAB test bench in common for repeated algorithm validation. It develops algorithm models that flexibly cope up with already present flows and performs on any ANSI C-code platform. Beside this the agility MCS defines a specific white-box algorithm hand-off for signal processing applications. Agility MCS family is an automated process which helps the programmers to start even through the algorithm is modifying current time. Agility MCS also prepares prototypes and development in the algorithm within minutes rather than in days or week. Thus, these exciting and unique features are attracting many programmers in MATLAB to use agility MCS synthesizer for many signal processing and communication applications. 7.2 Agility MCS Characteristics: Generally at the time of conversion of the desired M-code to the C-code using the Agility MCS, the obtained C-code exhibits some unique features in the code hierarchy. Thus proven to be user friendly and has become an easier choice for the programmers to work with. Therefore we shall look into those features in detail in the following. The key characteristics of MATLAB to C synthesis tool i.e. agility MCS family are as following, It generates the C-code without discoloration or with blemish and also readable one.

43 43 P a g e It does change the function names and also the variable names present in the M-code that is subjected to conversion. When the function hierarchy and file stricter are compared from the M-code it remains similar and helps the programmers to validate the code. Besides conversion it preserves the M-code as comments in the obtained C-code. It includes the pre-implemented C-code. Agility MCS through graphical user interface its cross validate MATLAB code and the C-code obtained. 7.4 Agility MCS Synthesizer Block Diagram: M-code Agility MCS Target Custom Processor Algorithm prototype ESL System Simulation Verification C-code Software Application Figure 21: Agility MCS block representation of the synthesizer.

44 44 P a g e

45 45 P a g e 8. CONCLUSION The aim with this thesis project was to implement an automatic algorithm for a forwarder crane. The algorithm is implemented into a Dasa control hardware called Crane box including interface routines for communication with the D5 box. The implementation was performed manually. The functionality is partly tested by use of three methods. First is the software tested in the crane laboratory at Linneus University in Vaxjo. The implementation has been tested by the skilled crane operators from the Rottne Industry AB. Secondly the software is tested by interaction with the simulated crane, also implemented in the Crane Box system. Thirdly the software output for some examples has been compared to the dspace implementation of the algorithm. The result of these tests was in favor for the implementation. We guess that no errors exist.

46 46 P a g e 9. REFRENCES [1] Westerberg S., Manchester I.R., Hera P.L.,and Shiriaev A. Virtual environment teleportation of a hydraulic forestry crane. In International conference on Robotics and Automation, Pasadora, C.A, USA, May 19-23, IEEE, [2] Lofgren B., Kinematic Control of Redundent Kunckle Booms eith Automatic Path-following Functions, Doctrol Thesis, KTH, [3] Sigvardsson M. and Olsson T., Modelling and Simulation of a Hydraulic Crane, Master thesis, University of Kalmar, February [4] Ekevid T. On optimal control of hydraulic cranes, Proc. 19th Nordic Seminar on Computational Mechanics, NSCM-19, Lund, Sweden, October 2006, pp [5] Fazululla M. and Srikanth K., Mathematical Modeling and Simulation in Dymola of a laboratory crane, Master thesis, University of Kalmar, June [6] Heinze A., Modelling and Simulating of Laboration Crane, friction and dynamics, Master Thesis Report, Växjö University, [7] Zhamykhanova A. B. Modelling of a laboratory crane, Master thesis, University of Kalmar, [8] Laute M, and In den Kleef P,(2010), Measurement of the link operation of a working forwarder, Project report, Vaxjo University,to appear [9] Spong M.W., Hutchinson S., and Vidhyasagar M., Robot Modelling and Control, John Wiley and Sons,2006. [11] Available:

47 47 P a g e

48 48 P a g e GRAPHICAL RELATION Graphical diagram: Instant Crane Tip velocity, d 4 d =f θ 2, θ 3, θ 2, θ 3d 4 Calculation of v = J(q)q K, Gain Parameter Maximum joint velocities, θ 2m, θ 3m,d 4m Jacobian Function J(q) Algorithm.c dd4desiredfun Jocobian function qdot2v q = θ 2 θ 3 d 4 q = θ 2 θ 3 d 4 Derivative of corresponding forwarder kinematics User.c dasa application interface Crane Coordinates x 0, z 0 DH-Table Represents relation between the C-Code and Algorithm. CraneExperiments.c Matrix.c Figure 22: Overview of the relationship between the algorithm and the code.

49 49 P a g e The desired prismatic link velocity can be obtained from the functions of the crane geometry.then we formulate the Jacobian matrix by taking the derivative of the forwarder kinematics obtained from the transformation matrices. This could be related to the code is qdot2v function. In this function not only evaluates the Jacobian matrix but in tern the Cartesian velocities of the links of the crane. The angular velocities can be obtained from the sensors of the crane. Thus we need to consider the maximum angular velocities of the links of the crane. These also could be included in the qdot2v function. These are all the functions that were included in the algorithm. The gain parameter k is also included in the qdot2v function. The gain parameter is a scalar quantity that could be multiplied to the Jacobian matrix in this function. Then the crane tip in order not to hit the end positions we develop a weighting function that could be included in the algorithm. The Weighting Function is related to the code as the Lambdafun function for the algorithm implementation. These were used to compute the dd4desire values of each crane experiments that were performed at the dasa laboratory. These could be related to Crane Experiments.c in the code. The dd4deridedfunc is rela.ted to the dd4desire values evaluation of the algorithm. Then using the dasa software we need to develop the user interface for the links of the crane in functioning of the crane. These were all used in order to compute the d 4 d velocities of the crane. The User.c can be used to configure the variables for the crane laboratory and also provides the graphical user interface for the display screen. This User.c of the code is related to the user interface of the algorithm. Thus the extension boom velocities are calculated in the algorithm with all these supporting files in the code development. Thus we have brought the relation between the algorithm and the C code that would help in understanding the UML diagram which is discussed in the Figure 19 above. We also use the matrix library using the Matrix.c file where we have large number of functions available such as the matrix multiplications, scalar multiplications, divisions etc. We shall also have detail description of the code in the UML representation diagrams and we could get familiar with the coding part of this thesis. Practically the Lambda function is implementation is not considered in our algorithm by taking the value as equal to 1.Thus we take the whole lambda as scalar quantity.

50 50 P a g e

51 51 P a g e 10. RESULTS Figure 23: Dasa lab testing environment simulations with crane lab experiments. This was the simple crane experiment simulation of the crane lab environment conducted in visual C ++ environment. The simple experiment of moving the crane links forward, in order to pick up a log on the ground. The values of the simulations are a smooth curve that we could see in the matlab simulations. The values vary from to m/sec. The values of the dtheta 2, dtheta 3 and dd4 values are subjected to 0.01,0.02 and 0.01 radians respectively. In every updating we have the increment in theta 2, theta3 and d4 similarly we will have the updating of the dtheta2, dtheta3 and dd4 values once we initiate the loop. Thus we could see a smooth curve of values in the graph of dd4desire velocities. These could be compared to the matlab simulations with some particularly values if d theta 2, dtheta3 and dd4 values.

52 52 P a g e Figure 24: Crane lab testing environment output curve from the d5 Analyzer. This was the output curve that was taken from the d5 Analyzer and we can notice a smooth curve when the same testing of picking of a log from the ground is performed. The curve is a smooth decreasing one that could be compared against the matlab or the lab simulations as we said before. We have to perform different steps while testing the crane functionality. We have taken out the manual steps in the figuring out of the outputs from the crane functionality. We have taken up the process of identifying the zero positions of the links and then we started to read the inputs and respectively we have drawn the outputs. We have figured out various values of the thea2, theta 3 and the respective dd4desire values at that point of time. This all has bee possible only through the usage of d5 Analyzer. We need to connect the d5 Analyzer to the computer and we need to connect this to the IP address of the remote desktop connection. Then we need to make it active whenever we need to figure out the output curve. We also can get the various values the output graph by positioning the line wherever the value is to be retrieved. The procedure for knowing the theta2, theta3 as well as the derivatives is carried out manually except the output curve is plotted for the dd4 desire from the d5 Analyzer. In the next section we could tabulate the results in the particular manner so that we could get the values of all the parameters as expected. Thus we can continue to the next section of data retrieval process of the parameters.

53 53 P a g e Theta 2 angle(radians) Theta 3 angle(radians) dd4 desire velocity(m/sec) Table 4: The algorithm dd4dersire values that were taken from the crane are illustrated. From the appendices Figure 35 and Figure 36 we compare the Matlab simulations with the real crane results and we have obtained approximately the same results except the scaling is different. The Craneexp 1 is related to the picking the log from the ground and the same is done in the case of real crane environment.

54 54 P a g e

55 55 P a g e 11. UML DIAGRAMS ANALYSIS DAIAGRAM: act algorithm EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr ActivityInitial theta2 theta3 Calculate derivatives EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 V4m Unregistered matrix Trial Version EA 7.5 Unregistered Tr EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version div ide both EA 7.5 Unregistered Tr V23m matrix EA 7.5 Unregistered Trial Version Dependencies() EA 7.5 Unregistered Trial Version multiplication EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr UpdateInputValues AndDependencies () UpdateInputValuesAnd d4 deriv ativ es(dtheta2,dtheta3,dd4) qdot4m matrix Jacobian Matrix multiplication multiplication EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr qdotm matrix EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered desired Trial velocity Version d4 dotd EA 7.5 Unregistered Tr EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr dd4desirefun() d4hat matrix Gain K Lambda EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Tr Figure 25: Analysis diagram gives the relation between the physical and coding parts. qdot2v() Transpose Matrix dd4desirefun() V23 matrix Normalization of V4m matrix normalization of V23m matrix Calculated4Dotd() matrices

56 56 P a g e COMPONENT DIAGRAM registered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA cmp Components registered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA registered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA registered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA registered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA Algorithm registered Trial Version EA 7.5 Unregistered + Calculated4Dotd() Trial : voidversion EA 7.5 Unregistered Trial Version EA registered Trial Version EA 7.5 Unregistered + Update3x1Matrix() Trial : voidversion EA 7.5 Unregistered Trial Version EA registered Trial Version EA 7.5 Unregistered + UpdateInputValuesAndDependencies() Trial Version EA : void7.5 Unregistered Trial Version EA registered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA CI12Base + Initialized4DotdCalculation() : void + dd4desirefun() : void + qdot2v() : void + Updated4DotdData() : void + UpdatedPreviousTimeMs() : void registered + ReadInputValue() Trial Version : void EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA + WriteSysVar() : void Matrix registered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA registered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA Figure 26: Component diagram gives the relation between the physical and coding parts. User Crane experiments

57 57 P a g e ACTIVITY DIAGRAM: EA act 7.5 RUNNING Unregistered THE CRANE LABTrial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio ActivityInitial EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA 7.5 Unregistered Trial Version EA INITIALIZATION 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio [False] EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA 7.5 Unregistered Trial Version EA 7.5 [True] Unregistered Trial Version EA 7.5 Unregistered Trial Versio need to find the Zero EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA 7.5 Unregistered READ Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio INPUTS(THETA2,THETA3,D4) Found both zero positions FIND ZERO POSITIONS GREEN LIGHT NORMAL OPERATION RED LIGHT The Loop goes to the point where we still positions WRITE OUTPUTS EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio CONVERSION OF INPUTS UPDATE PARAMETERS Calculated4Dotd VELOCITY EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Versio EA Figure 7.5 Unregistered 27: Activity diagram Trial gives Version the relation EA 7.5 between Unregistered the physical Trial and Version coding parts. EA 7.5 Unregistered Trial Versio

58 58 P a g e STATE DIAGRAM: Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version stm CRANE LAB Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version Conditions Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version Unregistered Trial Version EA UNDEFINED_HAS_NO_ZERO_POS 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version UNDEFINED_HAS_THETA2_ONLY Theta 2 Angle Initial Zero positions of the UNDEFINED_HAS_THETA3_ONLY Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version Theta 3 Angle Theta2 and Theta 3 Angles Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version Normal Operation Theta 3 Angle Theta 2 Angle Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version Figure 28: State diagram gives the relation between the physical and coding parts.

59 59 P a g e

60 60 P a g e 12. CODE DOCUMENTATION Algorithm.h #include "matrix.h" // // Defines // #define PI f #define MIN(a, b) (((a) < (b))? (a) : (b)) #define MAX(a, b) (((a) > (b))? (a) : (b)) // // Data structure definitions // struct Calculated4DotData //Link lengths of the Crane double swivellength; double liftlength; double elbowlength; //Initial conditions of the parameters double theta1; // swivel arm angular velocity in radians/miliseconds double d1; double a1; double alpha1; // swivel arm double theta2; double d2; double a2; double alpha2; // lift arm double theta3; double d3; double a3; double alpha3; // elbow arm double theta4; double d4; double a4; double alpha4; // extension link // desired4 Calculation double gain; // Configurable value,... double dd4m; // double dtheta2m; // double dtheta3m; // double d4c; // double d4e; // double lambda; // double x,z; // TBD?? double dx,dz; // TBD?? double dd4desire; //... // Movement, incremental Values double dtheta1; // Delta deviation of theta1 angle, is the current angle minus the previous angle double dtheta2; // Delta deviation of theta2 angle, is the current angle minus the previous angle double dtheta3; // Delta deviation of theta3 angle, is the current angle minus the previous angle

61 61 P a g e double dd4; // Delta deviation of d4 length, is the current length minus the previous length unsigned int previoustimems; double xold; // TBD?? Not used. double zold; // TBD?? Not used. // matrix variables declaration, initialize to zeros double T104x4Block[4][4]; matrix T104x4Matrix; // temporary matrix in Calculated4Dotd double T214x4Block[4][4]; matrix T214x4Matrix; // temporary matrix in Calculated4Dotd double T324x4Block[4][4]; matrix T324x4Matrix; // temporary matrix in Calculated4Dotd double T434x4Block[4][4]; matrix T434x4Matrix; // temporary matrix in Calculated4Dotd double S4x2Block[4][2]; matrix S4x2Matrix; // TBD! Not used! double Sa4x2Block[4][2]; matrix Sa4x2Matrix; // swivelarm (homogeneus coordinates). TBD! Only used for coordinate system double L4x2Block[4][2]; matrix L4x2Matrix; // temporary matrix in Calculated4Dotd double E4x2Block[4][2]; matrix E4x2Matrix; // temporary matrix in Calculated4Dotd double P4x2Block[4][2]; matrix P4x2Matrix; // temporary matrix in Calculated4Dotd double Li4x2Block[4][2]; matrix Li4x2Matrix; // temporary matrix in Calculated4Dotd double Lif4x2Block[4][2]; matrix Lif4x2Matrix; // liftarm (homogeneus coordinates). TBD! Only used for coordinate system double T14x4Block[4][4]; matrix T14x4Matrix; // temporary matrix in Calculated4Dotd double T24x4Block[4][4]; matrix T24x4Matrix; // temporary matrix in Calculated4Dotd double El4x2Block[4][2]; matrix El4x2Matrix; // elbowarm (homogeneus coordinates). TBD! Only used for coordinate system double T34x4Block[4][4]; matrix T34x4Matrix; // temporary matrix in Calculated4Dotd double Pr4x2Block[4][2]; matrix Pr4x2Matrix; // prismaticarm (homogeneus coordinates). TBD! Only used for coordinate system double qdotm3x1block[3][1]; matrix qdotm3x1matrix; // temporary matrix in Calculated4Dotd double q3x1block[3][1]; matrix q3x1matrix; // temporary matrix in Calculated4Dotd double qdot3x1block[3][1]; matrix qdot3x1matrix; // temporary matrix in Calculated4Dotd double V2x1Block[2][1]; matrix V2x1Matrix; // temporary matrix in Calculated4Dotd ; // // Public function definitions

62 62 P a g e // void Initialized4DotdCalculation(struct Calculated4DotData* dp); void Updated4DotdData(double gain, double a2, struct Calculated4DotData* dp); /* Fucntion : Calculated4Dotd Parameters : theta theta d4 -.. Affected value: dp->dd4desire is updated with the suggested delta d4 velocity Return values : 0 - indicates failure to calculate (typically deltatime is zero) 1 - indicates success Description : */ int Calculated4Dotd(double theta2, double theta3, double d4, unsigned int currenttimems, struct Calculated4DotData* dp); void Update3x1Matrix(double a, double b, double c, matrixptr matrixptr); void UpdatedPreviousTimeMs(unsigned int previoustimems, struct Calculated4DotData* dp); craneexperiments.h #ifdef CRANELAB void cranelabsimulation1(int loop); #else // // Public function definitions // void CraneExp1b(int loop); void CraneExp1a(int loop); void CraneExp1c(int loop); void CraneExp1d(int loop); void CraneExp1e(int loop); void cranelabsimulation1(int loop); #endif

63 63 P a g e Matrix.h #ifndef defsmat #define defsmat 1 /* Matrix structure */ typedef struct int rows; int cols; double *block; matrix,*matrixptr; /* E.G. double b4x4a[4][4]= 6,1,6,6, 1,6,6,0, 0,3,2,1, 8,6,1,9 ; matrix m4x4a=4,4,&b4x4a[0][0]; */ /* Function prototypes */ void mcreate(int rows, int cols, double* block, matrix* m); void mprint(matrix* m); void smmult(matrixptr,double); int madd(matrixptr m1,matrixptr m2,matrixptr dm); int mmult(matrixptr m1,matrixptr m2,matrixptr dm); int mcopy(matrixptr sm,matrixptr dm); int mtrans(matrixptr sm,matrixptr dm); double det(matrixptr m); //int minv(matrixptr sm,matrixptr dm); int nsolve(int rows,double *data); int mid(matrixptr); void mzero(matrixptr); double mnorm(matrixptr); #endif

64 64 P a g e Craneboxpc.c #include "stdio.h" #include "Algorithm.h" #include "CraneExperiments.h" //struct Values // double theta2; double theta3; double gain; double a2; //; // //struct Values inputarray[10] = // // theta2, theta3, gain, a2 // 0.00f, -0.01f, 5, 60, // 0.10f, -0.11f, 5, 60, // 0.20f, -0.21f, 5, 60, // 0.30f, -0.31f, 5, 60, // 0.40f, -0.41f, 5, 60, // 0.50f, -0.51f, 5, 60, // 0.60f, -0.61f, 5, 60, // 0.70f, -0.71f, 5, 60, // 0.80f, -0.81f, 5, 60, // 0.90f, -0.91f, 5, 60, //; struct Calculated4DotData d4dotddata; int main() // Dafault values double theta2=30*pi/180.0, theta3=-29*pi/180.0, gain=5, a2=60; double d4dotd=0; char choice=0; int k; while (1) printf("\n \n"); printf("experiments\na,b,c,d,e,l\n>"); scanf_s("%c", &choice, 1); if (choice == '0') //// Phase 1: Raed inputs //// Using the defult values of inputs //// Phase 2: Calculate //d4dotd = Calculated4Dotd(theta2,theta3,gain, a2); // d4dot //// Phase 3: Write outputs //printf("output : d4dotd=%f\n", d4dotd); else if (choice == '1') else if (choice == '2') // Automatic, scenario 1(craneexp1b)

65 65 P a g e #ifdef CRANELAB else if (choice == 'L') for (k = 0; k <= 27; k++) cranelabsimulation1(k); #else else if (choice == 'a') for (k = 0; k <= 27; k++) CraneExp1a(k); else if (choice == 'b') for (k = 0; k <= 27; k++) CraneExp1b(k); else if (choice == 'c') for (k = 0; k <= 27; k++) CraneExp1c(k); else if (choice == 'd') for (k = 0; k <= 27; k++) CraneExp1d(k); else if (choice == 'e') for (k = 0; k <= 27; k++) CraneExp1e(k); #endif else if (choice == 'q') break; // Exit while-loop printf("ready\n");

66 66 P a g e CraneExperiments.c /* File name : <CraneExperiments.c> Description : The functions a crane for the dasa lab testing */ #include "math.h" #include "matrix.h" #include "Algorithm.h" #include "CraneExperiments.h" #include "stdio.h" /*Pre declarations*/ // // External Defintitions // extern struct Calculated4DotData d4dotddata; #ifndef CRANELAB // // Local Function Definitions // static void UpdateParametersFromExperimentalData(struct Calculated4DotData* dp); /* File name : <UpdateParametersFromExoerimentData.c> Description : The dd4 calcluation and also updating the angles and lengths each and eveyrtime when there is the change in the link movements */ // // Function Declarations // static void UpdateParametersFromExperimentalData(struct Calculated4DotData* dp) dp->dd4=min(max(-dp->dd4m,dp->dd4desire),dp->dd4m); //Angle and Length Updating dp->theta1 += dp->dtheta1; dp->theta2 += dp->dtheta2; dp->theta3 += dp->dtheta3; dp->d4 += dp->dd4; //O4 Position Update // TBD?? dp->xold=dp->x; // TBD?? dp->zold=dp->z; // TBD??

67 67 P a g e /* Fucntion : <craneexp1b.c> Parameters : dtheta2,dtheta3 and dd4 values are updated with the gain,a2 and dd4desire values given. Return values : dd4desire value where we obtain the desired velocities. Description : We take the values of the d4e,d4c,theta2,theta3 and d4 along with the delta values and we obtain the dd4desire value */ // //The craneexp1 animates "goto a logg on the ground 4.5 meter away // void CraneExp1b(int loop) // Animation values that are Constants double gain=1; double a2=2.3; double dd4desire; static unsigned int currenttimems= 0; int bok; if (loop == 0) Initialized4DotdCalculation(&d4DotdData); //Intializes the d4dotcalculation Updated4DotdData(gain,a2,&d4DotdData); //Updates the parametes // Override data d4dotddata with experimental specific values d4dotddata.d4c=2; // no side effects d4dotddata.d4e=4; // no side effects d4dotddata.theta2=30*(pi/180.0); // no side effects d4dotddata.theta3=-30*(pi/180.0); // no side effects d4dotddata.d4=d4dotddata.elbowlength;// no side effects d4dotddata.dtheta1=0; // must update Updateqdot3x1Matrix d4dotddata.dtheta2=-2*(pi/180.0); // must update Updateqdot3x1Matrix d4dotddata.dtheta3=4*(pi/180.0); // must update Updateqdot3x1Matrix d4dotddata.dd4=0; // must update Updateqdot3x1Matrix Update3x1Matrix(d4DotdData.dtheta2, d4dotddata.dtheta3, d4dotddata.dd4, &d4dotddata.qdot3x1matrix); UpdatedPreviousTimeMs(currentTimeMs, &d4dotddata); // Update prevtime just before caluclation starts else // Animation loop currenttimems += 500; // every loop, increase time bok =Calculated4Dotd(d4DotdData.theta2,d4DotdData.theta3,d4DotdData.d4,currentTimeMs, &d4dotddata); dd4desire=d4dotddata.dd4desire; printf("n(%d) dd4desire=%f\n", loop, dd4desire);

68 68 P a g e UpdateParametersFromExperimentalData(&d4DotdData); /* Fucntion : <craneexp1a.c> Parameters : dtheta2,dtheta3 and dd4 values are updated with the gain,a2 and dd4desire values given. Return values : dd4desire value where we obtain the desired velocities. Description : We take the values of the d4e,d4c,theta2,theta3 and d4 along with the delta values and we obtain the dd4desire value */ // //The craneexp1a animates "goto a logg on the ground 4.5 meter away // void CraneExp1a(int loop) // Animation values double gain=1; double a2=2.3; double dd4desire; static unsigned int currenttimems = 0; int bok; if (loop == 0) Initialized4DotdCalculation(&d4DotdData); Updated4DotdData(gain,a2,&d4DotdData); // Override data d4dotddata with experimental specific values d4dotddata.d4c=4; // no side effects d4dotddata.d4e=8; // no side effects d4dotddata.theta2=30*(pi/180.0); // no side effects d4dotddata.theta3=-30*(pi/180.0); // no side effects d4dotddata.d4=d4dotddata.elbowlength;// no side effects d4dotddata.dtheta1=0; // must update Updateqdot3x1Matrix d4dotddata.dtheta2=-2*(pi/180.0); // must update Updateqdot3x1Matrix d4dotddata.dtheta3=4*(pi/180.0); // must update Updateqdot3x1Matrix d4dotddata.dd4=0; // must update Updateqdot3x1Matrix Update3x1Matrix(d4DotdData.dtheta2, d4dotddata.dtheta3, d4dotddata.dd4, &d4dotddata.qdot3x1matrix); UpdatedPreviousTimeMs(currentTimeMs, &d4dotddata); // Update prevtime just before caluclation starts else // Animation loop currenttimems ++; bok = Calculated4Dotd(d4DotdData.theta2,d4DotdData.theta3,d4DotdData.d4,currentTimeMs,&d 4DotdData);

69 69 P a g e dd4desire=d4dotddata.dd4desire; printf("n(%d) dd4desire=%f\n", loop, dd4desire); UpdateParametersFromExperimentalData(&d4DotdData); /* Fucntion : <craneexp1c.c> Parameters : dtheta2,dtheta3 and dd4 values are updated with the gain,a2 and dd4desire values given. Return values : dd4desire value where we obtain the desired velocities. Description : We take the values of the d4e,d4c,theta2,theta3 and d4 along with the delta values and we obtain the dd4desire value */ // //The craneexp1c animates "fetch a logg from the ground 4.5 meter away // void CraneExp1c(int loop) // Animation values double gain=1; double a2=2.3; double dd4desire; static unsigned int currenttimems = 0; int bok; if (loop == 0) Initialized4DotdCalculation(&d4DotdData); Updated4DotdData(gain,a2,&d4DotdData); // Override data d4dotddata with experimental specific values d4dotddata.d4c=1; // no side effects d4dotddata.d4e=2; // no side effects d4dotddata.theta2=(30-27*2)*(pi/180.0); // no side effects d4dotddata.theta3=(-30+27*4)*(pi/180.0); // no side effects d4dotddata.d4=d4dotddata.elbowlength+1.2; // no side effects d4dotddata.dtheta1=0; // must update Updateqdot3x1Matrix d4dotddata.dtheta2=2*(pi/180.0); // must update Updateqdot3x1Matrix d4dotddata.dtheta3=-4*(pi/180.0); // must update Updateqdot3x1Matrix d4dotddata.dd4=0; // must update Updateqdot3x1Matrix Update3x1Matrix(d4DotdData.dtheta2, d4dotddata.dtheta3, d4dotddata.dd4, &d4dotddata.qdot3x1matrix); UpdatedPreviousTimeMs(currentTimeMs, &d4dotddata); // Update prevtime just before caluclation starts else // Animation loop currenttimems += 500;

70 70 P a g e bok = Calculated4Dotd(d4DotdData.theta2,d4DotdData.theta3,d4DotdData.d4,currentTimeMs,&d 4DotdData); dd4desire=d4dotddata.dd4desire; printf("n(%d) dd4desire=%f\n", loop, dd4desire); UpdateParametersFromExperimentalData(&d4DotdData); /* Fucntion : <craneexp1d.c> Parameters : dtheta2,dtheta3 and dd4 values are updated with the gain,a2 and dd4desire values given. Return values : dd4desire value where we obtain the desired velocities. Description : We take the values of the d4e,d4c,theta2,theta3 and d4 along with the delta values and we obtain the dd4desire value */ // //The craneexp3 animates "goto a logg on the ground close // void CraneExp1d(int loop) // Animation values double gain=1; double a2=2.3; double dd4desire; static unsigned int currenttimems = 0; int bok; if (loop == 0) Initialized4DotdCalculation(&d4DotdData); Updated4DotdData(gain,a2,&d4DotdData); // Override data d4dotddata with experimental specific values d4dotddata.d4c=2; d4dotddata.d4e=4; // no side effects d4dotddata.d4c=2; // no side effects d4dotddata.theta2=(45)*(pi/180.0); // no side effects d4dotddata.theta3=(-45)*(pi/180.0); // no side effects d4dotddata.d4=d4dotddata.elbowlength;// no side effects d4dotddata.dtheta2=-(2)*(pi/180.0); // must update Updateqdotm3x1Matrix d4dotddata.dtheta3=(2)*(pi/180.0); // must update Updateqdotm3x1Matrix d4dotddata.dd4=0; // must update Updateqdotm3x1Matrix Update3x1Matrix(d4DotdData.dtheta2, d4dotddata.dtheta3, d4dotddata.dd4, &d4dotddata.qdot3x1matrix); UpdatedPreviousTimeMs(currentTimeMs, &d4dotddata); // Update prevtime just before caluclation starts else

71 71 P a g e // Animation loop currenttimems += 500; bok = Calculated4Dotd(d4DotdData.theta2,d4DotdData.theta3,d4DotdData.d4,currentTimeMs,&d 4DotdData); dd4desire=d4dotddata.dd4desire; printf("n(%d) dd4desire=%f\n", loop, dd4desire); UpdateParametersFromExperimentalData(&d4DotdData); /* Fucntion : <craneexp1e.c> Parameters : dtheta2,dtheta3 and dd4 values are updated with the gain,a2 and dd4desire values given. Return values : dd4desire value where we obtain the desired velocities. Description : We take the values of the d4e,d4c,theta2,theta3 and d4 along with the delta values and we obtain the dd4desire value */ // //The craneexp4 animates "movement of only the elbow // void CraneExp1e(int loop) // Animation values double gain=1; double a2=2.3; double dd4desire; unsigned int currenttimems = 0; int bok; if (loop == 0) Initialized4DotdCalculation(&d4DotdData); Updated4DotdData(gain,a2,&d4DotdData); // Override data d4dotddata with experimental specific values d4dotddata.d4c=1; d4dotddata.d4e=4; // no side effects d4dotddata.d4c=2; // no side effects d4dotddata.theta2=(45)*(pi/180.0); // no side effects d4dotddata.theta3=(-30)*(pi/180.0); // no side effects d4dotddata.d4=d4dotddata.elbowlength; // no side effects d4dotddata.dtheta3=(2)*(pi/180.0); // must update Updateqdotm3x1Matrix d4dotddata.dtheta2=(-2)*(pi/180.0); // must update Updateqdotm3x1Matrix d4dotddata.dd4=0; // must update Updateqdotm3x1Matrix Update3x1Matrix(d4DotdData.dtheta2, d4dotddata.dtheta3, d4dotddata.dd4, &d4dotddata.qdot3x1matrix); UpdatedPreviousTimeMs(currentTimeMs, &d4dotddata); // Update prevtime just before caluclation starts else

72 72 P a g e // Animation loop currenttimems += 500; bok = Calculated4Dotd(d4DotdData.theta2,d4DotdData.theta3,d4DotdData.d4,currentTimeMs,&d 4DotdData); dd4desire=d4dotddata.dd4desire; printf("n(%d) dd4desire=%f\n", loop, dd4desire); UpdateParametersFromExperimentalData(&d4DotdData); #else // ifdef CRANELAB void cranelabsimulation1(int loop) //animation values double gain=1; double a2=2.3; double dd4desire; unsigned int currenttimems = 0; int bok; static double theta2 = 0; static double theta3 = 0; static double d4 = 0.980; double dtheta2 = 0.01; double dtheta3 = 0.02; double dd4 = 0.01; if (loop == 0) Initialized4DotdCalculation(&d4DotdData); Updated4DotdData(gain,a2,&d4DotdData); UpdatedPreviousTimeMs(currentTimeMs, &d4dotddata); // Update prevtime just before caluclation starts else //first loop // Update I/O theta2 += dtheta2; theta3 += dtheta3; d4 += dd4; //Use same time as previous time bok = Calculated4Dotd(theta2,theta3,d4,currentTimeMs,&d4DotdData); dd4desire=d4dotddata.dd4desire; //second loop //update I/O theta2 += dtheta2; theta3 += dtheta3; d4 += dd4; currenttimems++; // Increase time bok = Calculated4Dotd(theta2,theta3,d4,currentTimeMs,&d4DotdData); dd4desire=d4dotddata.dd4desire; printf("n(%d) dd4desire=%f\n", loop, dd4desire);

73 73 P a g e #endif MATRIX.C /* Matrix operation functions and typedefs */ /* Written by Nigel Salt */ #include <stdio.h> //#include <malloc.h> #include <math.h> #include "matrix.h" void mcreate(int rows, int cols, double* block, matrix* m) m->rows=rows; m->cols=cols; m->block=block; /* function definitions */ void mprint(matrix* m) int i,j; int cols; cols=m->cols; for (i=0;i<m->rows;i++) printf("\n"); for (j=0;j<cols;j++) printf("%4.8lf ",*(m->block+i*cols+j)); printf("\n"); /* add two matrices giving a third matrix */ int madd(matrixptr m1,matrixptr m2,matrixptr dm) int i,j; if (m1->rows!=m2->rows m1->cols!=m2->cols) fprintf(stderr,"\nmadd error - matrices different sizes"); return(1); if (m1->rows!=dm->rows m1->cols!=dm->cols) fprintf(stderr,"\nmadd error - matrices different sizes"); return(1); for (i=0;i<m1->rows;i++) for (j=0;j<m1->cols;j++) *(dm->block+i*m1->cols+j)=*(m1->block+i*m1->cols+j)+\

74 74 P a g e *(m2->block+i*m1->cols+j); return(0); int mcopy(sm,dm) matrixptr sm,dm; int i,j; if (dm->rows!=sm->rows dm->cols!=sm->cols) fprintf(stderr,"\nmcopy error - matrices different sizes"); return(1); for (i=0;i<dm->rows;i++) for (j=0;j<dm->cols;j++) *(dm->block+i*dm->cols+j)=*(sm->block+i*dm->cols+j); return(0); /* multiply matrix by scalar double */ void smmult(matrixptr m, double s) int i,j; for (i=0;i<m->rows;i++) for (j=0;j<m->cols;j++) *(m->block+i*m->cols+j)=*(m->block+i*m->cols+j)*s; int mmult(m1,m2,dm) matrixptr m1,m2,dm; int i,j,k; double cellval; if (m1->cols!=m2->rows) fprintf(stderr,"\nmmult error - matrix 1 cols must = matrix 2 rows"); return(1); if (m2->cols!=dm->cols) fprintf(stderr,"\nmmult error - dest matrix cols must = matrix 2 cols"); return(1); if (m1->rows!=dm->rows) fprintf(stderr,"\nmmult error - dest matrix rows must = matrix 1 rows"); return(1); for (i=0;i<m1->rows;i++) for (j=0;j<m2->cols;j++) cellval=0.0; for (k=0;k<m1->cols;k++) cellval+=*(m1->block+i*(m1->cols)+k) * (*(m2->block+k*(m2->cols)+j));

75 75 P a g e *(dm->block+i*dm->cols+j)=cellval; return(0); int mtrans(sm,dm) matrixptr sm,dm; int i,j; if (dm->rows!=sm->cols) fprintf(stderr,"\nmtrans error - dest matrix rows must = source matrix cols"); return(1); if (sm->rows!=dm->cols) fprintf(stderr,"\nmtrans error - source matrix rows must = dest matrix cols"); return(1); for (i=0;i<sm->rows;i++) for (j=0;j<sm->cols;j++) *(dm->block+j*dm->cols+i)=*(sm->block+i*sm->cols+j); return(0); double det(m) matrixptr m; double p1,p2,p3,d; int i,j,k; if (m->cols!=m->rows) fprintf(stderr,"\ndet error - matrix must be square"); return(0.0); d=0; for (i=0;i<m->cols;i++) p1=p2=1.0; p3=*(m->block+i); k=i; for (j=1;j<m->cols;j++) k=(k+1)%m->cols; p1*= *(m->block+j*m->cols+k); p2*= *(m->block+(m->cols-j)*m->cols+k); p3*=(p1-p2); d+=p3; return(d);

76 76 P a g e int nsolve(rows,data) int rows; double *data; int i,j,k; int cols; double dtemp; cols=rows+1; for (i=0;i<rows;i++) for (j=i;j<rows&&*(data+j*cols+j)==0.0;j++); if (*(data+j*cols+j)==0.0) fprintf(stderr,"\nnsolve error - singular matrix"); return 1; if (j!=i) for (k=0;k<cols;k++) dtemp=*(data+i*cols+k); *(data+i*cols+k)=*(data+j*cols+k); *(data+j*cols+k)=dtemp; for (j=cols-1;j>=0;j--) *(data+i*cols+j) /= *(data+i*cols+i); for (j=i+1;j<rows;j++) for (k=cols-1;k>=i;k--) *(data+j*cols+k)-=*(data+j*cols+i) * *(data+i*cols+k); for (i=rows-2;i>=0;i--) for (j=cols-2;j>i;j--) *(data+i*cols+cols-1)-= \ *(data+i*cols+j) * *(data+j*cols+cols-1); *(data+i*cols+j)=0; return 0; int mid(m) matrixptr m; int i,j; if (m->rows!=m->cols)

77 77 P a g e fprintf(stderr,"\nmid error - matrix must be square"); return 1; for (i=0;i<m->rows;i++) for (j=0;j<m->cols;j++) *(m->block+i*m->cols+j)=0.0; *(m->block+i*m->cols+i)=1.0; return 0; void mzero(m) matrixptr m; int i,j; for (i=0;i<m->rows;i++) for (j=0;j<m->cols;j++) *(m->block+i*m->cols+j)=0.0; double mnorm(m) matrixptr m; int i,j; double sum=0; for (i=0;i<m->rows;i++) for (j=0;j<m->cols;j++) sum += (*(m->block+i*m->cols+j)) * (*(m->block+i*m->cols+j)); return sqrtf(sum);

78 78 P a g e SWITCHING BETWEEN THE DSPACE SYSTEM TO DASA SYSTEM To start the dasa set up: Turn on the power first. Start the power supply then. Verify the switches; make sure that both are turned on. Verify green and orange LED s are ON. The CI12 base, IO48 as well as the MC unit LEDs are Verified. The monitor of the source system displays the dasa 5 application mode. Verify dasa application screen is in display or the dspace screen is in display. Typically we should use Crane box application in dasa. Then check the status button. Click on the status button we will go to a screen. Then click on Install button to make sure of which file to use. Typically we should make use of cranebox.ins5 for dasa application. Then we are able to enter into the cranebox menu. To start the pressure pump: The switch of the pump is made sure it is in OFF position. The switch lever is made to be in 90 degree position to be in OFF state. Then the pump room operation is taken up with utmost care. According to Bertil, the lab incharge this was the information. At the pump room on the machine we can find a RED button. This button is used for the STOP. So in order to stop it we need to pull the button outside. The move the switch to start knob and the goes back to low knob and after a while of 5 sec we can move the switch to high knob. Thus it sticks to high knob. This is the procedure to be followed before operating the crane. While running the crane also we need to take certain steps to avoid the sudden movement of the crane after loading the algorithm into the source system. Before moving the lever of the crane we need to check the Status o the inputs of the CI12 base. All the inputs and outputs should be zero. Those are A1-A6 and A7-A12. Then we can proceed to use the crane and also crane should be taken not to hit the ground using the links.

79 79 P a g e Changing the Sensors: Figure 29: Location of HEX screws diagram Figure 30: Location of Blots to change the sensors

80 80 P a g e Changing the Hardware setup: Figure 31: Physical representation of the Joysticks connections. Figure 32: Physical representation of the extension link connections.