dsp implementation of a-control algorithm for a …

1 | P a g e

THESIS NO.MEE10:31.

Blekinge Tekniska Högskola

SE–371 79 Karlskrona

Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

Symbols

𝜽𝟏 The swivel arm angle

𝜽𝟐 The lift arm angle

𝜽𝟑 The elbow arm angle

𝒅𝟒 Length of prismatic joint

𝜽 𝟏,𝜽 𝟐 Angular velocities of both the links

𝒅 𝟒 Joint velocity of the extension link

J(q) Jacobian function

v Velocity function

𝑶𝟒 The crane tip position

r, z Cartesian coordinates in r and z coordinates

𝒅 𝟒𝒅 The desired prismatic joint velocity

𝜽 𝟐𝒎, 𝜽 𝟑𝒎, 𝒅 𝟒𝒎 Maximum joint velocities of the joints

𝒌 Gain parameter which is adjustable

𝒅𝟒𝒄 The Center position of the prismatic joint

𝒅𝟒𝒆 The End position of the prismatic joint

T The Transformation matrix

3 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

Table of Contents

1 INTRODUCTION.............................................................................................................. 7

1.1 Forwarder Introduction ............................................................................... 7

1.2 Area of research............................................................................................................... 7

1.3 Review of State of Art…................................................................................7

1.4 Problem definition…......................................................................................8

1.5 Goals.............................................................................................................8

1.6Tools Required...............................................................................................8

1.7 Contents of Thesis.........................................................................................9

2 BACKGROUND………………........................................................................................ 11

2.1 Code Composer Studio (CCS)....................................................................... 11

2.2 D5 Developer Software……………………………………................................ 11

2.3 Automatic Conversion Tools........................................................................... 11

2.3.1 Crane Geometry.............................................................................................11

2.3.1 Crane Coordinates………...........................................................................................11

2.3.1Forwarder Kinematics..................................................................................................11

2.3.1Today’s Forwarder Operators .....................................................................................12

3 AUTOMATIC PRISMATIC JOINT ALGORITHM IMPLEMENTATION..............................18

3.1 The Jacobian Matrix ....................................................................................... 18

3.3 The Weighting Function and Gain parameter .................................................19

3.4 Parameters Chosen for Algorithm Implementation..........................................19

4 RK-62 PARAMETERS.........................................................................................................22

5 DASA LAB TESTING ENVIRONMENT……........................................................................24

5.1 d5DCL............................................................................................................. 25

5 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

5.2 d5Designer...................................................................................................... 25

5.3 d5Bundler ....................................................................................................... 26

5.4 dasa5 system units.......................................................................................... 26

5.4.1d5CI12 Base Unit........................................................................................... 26

5.4.2 TFT Monitor..................................................................................................................27

5.4.3 d5MC (Main Controller) Unit.........................................................................................27

5.4.4 d5IO12 Unit...................................................................................................................28

5.4.2 d5IO48 Unit...................................................................................................................29

5.4.2 Sensors........................................................................................................................30

6 THE CRANE LAB SET UP ENVIRONMENT.............................................................................31

6.1 Lab Crane set up with dasa Equipment.............................................................31

6.1.1 Physical connections at the Crane Lab with d5 Equipment…………..............32

6.2 Using the dSpace system in the Crane Lab Set Up...........................................36

6.3 Replacement of dSpace system with dasa5 Equipment……… ........................ 38

7 AGILITY MCS SYNTHESIZER TOOL........................................................................................42

7.1 Creation of C code automatically…………………............................................. 42

7.2 Agility MCS Characteristics………………............................................................43

7.3 Agility MCS Extensive Sustainability to Matlab................................................. 43

7.4 Agility MCS Synthesizer Block Diagram’.............................................................44

8 CONCLUSIONS……………………………………....................................................................... 45

9 REFRENCES……………………………………............................................................................ 47

10 RESULTS……………………………………............................................................................... 51

11 UML DIAGRAMS……………………………………................................................................... 55

12 CODE DOCUMENTATION………………………………………………………………………….60

Appendix

6 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

7 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

11 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 5.

2.3 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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

2.4.1 Crane Geometry:

Lift arm Elbow arm

𝜃3

𝜃2 𝑑4

z Swivel arm 𝑂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, 𝑂4, can be

represented in Cartesian coordinates, r and z or in terms of crane coordinates 𝜃2, 𝜃3 and 𝑑4from the Figure 1.

From the crane geometry we have 𝜃1 , 𝜃2and 𝜃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,

𝑑4 is the length of the extension boom in length (meters),

and, 𝑂4 is the crane tip position.

13 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

2.4.2 Crane Coordinates:

𝒚𝟐 𝒙𝟑

𝒙𝟐

𝑦1 𝜃3 𝑧3

𝑥1 𝑑4

𝜃2𝜃1 𝑧1

𝜃2

z 𝑂4

𝜃1

𝑧0

𝑥0 r

Figure 2: Crane coordinate according to Denavite-Hartenberg Table.

2.4.3 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,

𝑇𝑖−1= 𝑇𝑖𝑖−1 =

𝑐𝑜𝑠𝜃𝑖 −𝑠𝑖𝑛𝜃𝑖𝑐𝑜𝑠𝛼𝑖

𝑠𝑖𝑛𝜃𝑖 𝑐𝑜𝑠𝜃𝑖𝑐𝑜𝑠𝛼𝑖

𝑠𝑖𝑛𝜃𝑖𝑠𝑖𝑛𝛼𝑖 𝑎𝑖𝑐𝑜𝑠𝜃𝑖

−𝑐𝑜𝑠𝜃𝑖𝑠𝑖𝑛𝛼𝑖 𝑎𝑖𝑠𝑖𝑛𝜃𝑖

0 𝑠𝑖𝑛𝛼𝑖 0 0

𝑐𝑜𝑠𝛼𝑖 𝑑𝑖

0 1

……………………. (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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

The corresponding D-H table is given in Table 1.

DH-table, Crane 𝜽 𝒅 𝒂

Swivel arm, link1 𝜃1 = 0 𝑑1 = 𝑑1∗ 𝑎1 = 0 ∝1=

𝜋

2

Lift arm, link2 𝜃2 = 0 𝑑2 = 0 𝑎2 = 𝑎2∗ ∝2= 0

Elbow arm, link3 𝜃3 = 0 𝑑3 = 0 𝑎3 = 0 ∝3=𝜋

2

Extension link, link4 𝜃4 = 0 𝑑4 = 𝑑4∗ 𝑎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,

𝑇0 =

1 00 1

0 𝑎0 0

0 00 0

1 0 0 1

𝑇1 =

𝑐𝑜𝑠𝜃 −𝑠𝑖𝑛𝜃𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃

0 00 0

0 0 0 0

1 00 1

𝑇2 =

1 00 cos(𝑎𝑙𝑝ℎ𝑎)

0 0cos(𝑎𝑙𝑝ℎ𝑎) 0

0 sin(𝑎𝑙𝑝ℎ𝑎) 0 0

cos(𝑎𝑙𝑝ℎ𝑎) 00 1

and,

𝑇4 =

1 00 1

0 0 0 0

0 00 0

1 𝑑 0 1

are the transformation matrices.

15 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

The transformation from the 𝑂4 coordinate system to the 𝑂0 coordinate system is given by the relation as,

𝑇40 = 𝑇0𝑇1𝑇2𝑇3 ..………….…………………… (2)

𝑇40 =

𝑐𝑜𝑠( 𝜃2 + 𝜃3) 0sin( 𝜃2 + 𝜃3) −1

sin( 𝜃2 + 𝜃3) 𝑎2𝑐𝑜𝑠𝜃2 + 𝑑4sin(𝜃2 + 𝜃3)

𝑐𝑜𝑠( 𝜃2 + 𝜃3) 𝑎2𝑠𝑖𝑛𝜃2 − 𝑑4cos(𝜃2 + 𝜃3)sin( 𝜃2 + 𝜃3) 0

0 0

−𝑐𝑜𝑠( 𝜃2 + 𝜃3) 𝑑1 + 𝑎2𝑠𝑖𝑛𝜃2 − 𝑑4cos(𝜃2 + 𝜃3)0 1

The 𝑂4 i.e. the origin in the fourth coordinate system, is given in the (𝑂0 , 𝑥0 . 𝑦0 . 𝑧0) system by the relation as,

𝑝0 = 𝑇0𝑇1𝑇2𝑇3𝑂4

The forwarder kinematics for the crane when 𝜃1 = 0 is given by the relation as,

𝑝0 = 𝑥0

𝑧0 =

𝑎2𝑐𝑜𝑠𝜃2 + 𝑑4sin(𝜃2 + 𝜃3)𝑑1 + 𝑎2𝑠𝑖𝑛𝜃2 − 𝑑4cos(𝜃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.

2.4.4 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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

17 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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, 𝑧 ), 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 𝑥0 coincides with the coordinate r. The Cartesian velocity relation can

be explained in the following relation as,

𝑣 = 𝐽 𝑞 𝑞 …………………………… (4)

Where 𝑣 is the Cartesian velocity and 𝑞 matrix representing the angular velocity of the each links.

Where 𝑞 𝑖𝑠 𝑞 =

𝜃2

𝜃3

𝑑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)= −𝑎2𝑠𝑖𝑛𝜃2 + 𝑑4cos(𝜃2 + 𝜃3) 𝑑4cos(𝜃2 + 𝜃3) sin(𝜃2 + 𝜃3)𝑎2𝑐𝑜𝑠𝜃2 + 𝑑4sin(𝜃2 + 𝜃3) 𝑑4sin(𝜃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,

𝑣 = 𝑣𝑟

𝑣𝑧 =

𝑑

𝑑𝑡 𝑟𝑧 =

𝑑

𝑑𝑡 𝑥0

𝑧0 …………………………………………… (6)

19 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

This can be calculated as follows,

𝑣 = 𝐽 𝑞 𝑞

Where 𝑞 =

𝜃 2𝜃 3𝑑 4

……………………..…..……….……..(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𝜽 𝟐𝒎, 𝜽 𝟑𝒎, 𝒅 𝟐𝒎.

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,

∧ 𝑑4, 𝑑 4 =

𝑑4𝑒−𝑑4

𝑑4𝑒−𝑑4𝑐 , 𝑑4 > 𝑑4𝑐 & 𝑑 4 > 0

1 , 𝑑4 > 𝑑4𝑐 & 𝑑 4 < 0

1 , 𝑑4 < 𝑑4𝑐 & 𝑑 4 > 0

1 −𝑑4𝑒−𝑑4

𝑑4𝑒−𝑑40, 𝑑4 > 𝑑4𝑐 & 𝑑 4 > 0

………………………………………(8)

Where 𝒅𝟒𝒄the centre is position of the prismatic joint and 𝒅𝟒𝒆 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 𝑎2 are explanied..The link 𝒂𝟐 has one of the parameters of choice.

20 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

Then the gain parameter 𝒌 for the parameter of choice is also considered. The value of 𝑘 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

𝒅 𝟒 Joint Velocity of the Extension link.

𝒅 𝟒𝒅 The Desired Prismatic Joint Velocity.

𝐝𝟒𝐜 The Centre Position of the Prismatic Joint.

𝒅𝒅𝟒𝒅𝒆𝒔𝒊𝒓𝒆 The desired values of the prismatic link velocities.

𝐤 The scalar gain parameter constant

𝐚𝟐 The length of the lift arm link of the crane in meters

𝐝𝟒 Length of the Prismatic Joint

Table 2: The algorithm parameters that were illustrated in tabulate form.

21 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

22 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 𝒂𝟐, the length of the extension link 𝐝𝟒 and the maximum joint velocities of the crane.

23 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

24 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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.

5.4.1 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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

Figure 9: The Physical structure of d5CI12 unit.

5.4.2 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.

5.4.3 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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

Figure 11: The main controller unit d5MC for controlling the signal flow.

5.4.4 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.

5.4.5 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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

Figure 13: The input-output unit d5IO48 of the dasa equipment.

5.4.6 Sensors:

Figure 14: Sensors for the dasa equipment.

30 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

31 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

6.1.1Physical connections at the crane lab:

+yellow +Red +yellow +Red +Blue

+brown +Green +Brown +Green

sref {8}

sref PWR 24v

S00 s90 GND

GND PWR 5V

+4 +1 +12 +2 +11 +3 tags Idtag=1

+5 +7 CAN1 bus

+8

+9 +4

+12 CAN2L {0,5 m} +13

+1 CAN2H {0, 5 m} +5

+11 +2 PWR2{0,5 m}

CAN2L {0,5m} CAN2H{0,5m} GND{0,5 m} +9

PWR2{0,5 m} +12

+3 GND{0,5 m}

+ 11 +13

GND

+2 X +1

+cyan +black +12 Y +6

+3 Z +5

+20 +5V +3

Figure 15: Overview of the lab crane system with dasa5 system

Crane Box

Port A

Port B

d5MC

CAN

PWR/IO

120

ohm

IO48

CAN/PWR

120

ohm

Port B left: Dsub

female

Left: Joystick Same pinning as for port A

Port A

Right: Dsub

female Right: Joystick

3: IdTag

2: Id

Tag

d5IO12

Valves (8pcs)

8Current Out

2mcables

(connector)

Theta2: sensors Theta3: sensors

d4: sensors

Crane Setup

33 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 d5IO48.

10 Left Dsub The Joystick controller signal at Port B of d5IO48.

11 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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

6.2 Using the dSpace System in Crane Lab Set up:

Crane

Theta2, theta3, d4… 8 current outputs

{8}

Figure 16: Overview of the lab crane system with dSpace system

CRANE SETUP

SENSORS

DSPACE system Joystick

s

Values

Crane Control

Software

d5I012

CAN bus

d5MC

35 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

6.3 Replacement of dSpace System with dasa5 Equipment:

.

Theta2, Theta3, d4…… 8currentout {8}

CAN bus

112

CAN bus

Figure 17: Overview of the lab crane system with dasa5 system

d5MC Interface

Cane Box

Crane

Box

Interface

for d5

IO48

Joysticks

d5IO12 Crane

Control SW Device

drivers

Valves {8pcs}

Sensors

Crane Setup

37 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 56000 to -56000 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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

42 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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:

Target Custom Processor

Algorithm prototype

ESL

System Simulation Verification

Software Application

Figure 21: Agility MCS block representation of the synthesizer.

M-code

Agility MCS

C-code

44 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

45 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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, 2008.

[2] Lofgren B., “Kinematic Control of Redundent Kunckle Booms eith Automatic Path-following

Functions”, Doctrol Thesis, KTH, 2009.

[3] Sigvardsson M. and Olsson T., “Modelling and Simulation of a Hydraulic Crane”, Master thesis,

University of Kalmar, February 2005.

[4] Ekevid T. „On optimal control of hydraulic cranes”, Proc. 19th Nordic Seminar on Computational

Mechanics, NSCM-19, Lund, Sweden, October 2006, pp 107-110.

[5] Fazululla M. and Srikanth K., “Mathematical Modeling and Simulation in Dymola of a laboratory

crane”, Master thesis, University of Kalmar, June 2006.

[6] Heinze A.,” Modelling and Simulating of Laboration Crane, friction and dynamics”, Master Thesis

Report, Växjö University, 2008.

.[7] Zhamykhanova A. B. “Modelling of a laboratory crane”, Master thesis, University of Kalmar, 2008.

[8] Laute M, and In den Kleef P,(2010), Measurement of the link operation of a working forwarder, Project

report, Vaxjo University,{to appear 2010}.

[9] Spong M.W., Hutchinson S., and Vidhyasagar M., Robot Modelling and Control, John Wiley and

Sons,2006.

[11] Available: http://www.dasa.se/

47 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

48 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

GRAPHICAL RELATION

Graphical diagram:

Represents relation between the C-Code and Algorithm.

Figure 22: Overview of the relationship between the algorithm and the code.

Instant Crane Tip

velocity,

𝑑 4𝑑=𝑓 𝜃2,𝜃3 ,𝜃 2 ,𝜃 3𝑑4

Jacobian

Function J(q)

K, Gain

Parameter

Maximum joint

velocities,

𝜃 2𝑚 , 𝜃 3𝑚 ,𝑑 4𝑚

Calculation of

𝒗 = 𝑱(𝒒)𝒒

𝑞 =

𝜃2

𝜃3

𝑑4

𝑞 =

𝜃 2𝜃 3𝑑 4

Derivative of

corresponding

forwarder kinematics

Crane

Coordinates 𝒙𝟎,𝒛𝟎

dd4desiredfun

Jocobian function

qdot2v

DH-Table

Algorithm.c

User.c dasa

application

interface

CraneExperiments.c

Matrix.c

49 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 𝐤 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 𝑑 4𝑑 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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

51 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 0.004945 to0.001384 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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

Theta 2 angle(radians) Theta 3 angle(radians) dd4 desire velocity(m/sec)

83 -80 1168

80 -78 8132

77 -73 7441

75 -69 6014

71 -64 4473

60 -61 4813

53 -54 4519

46 -49 3627

42 -45 3133

28 -33 2416

11 -15 2011

6 -5 1218

3 -1 0826

1 3 0413

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

55 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

11. UML DIAGRAMS

ANALYSIS DAIAGRAM:

Figure 25: Analysis diagram gives the relation between the physical and coding parts.

act algorithm

EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version






































ActivityInitial

Calculate deriv ativ es

theta2

theta3

UpdateInputValues

AndDependencies

()

multiplication

deriv ativ es(dtheta2,dtheta3,dd4)

multiplication

d4

Jacobian Matrix

qdot4m matrix

qdotm matrix

multiplication

Transpose

Matrix

V4m matrix

V23m matrix

V23 matrix

Normalization

of V4m matrix

normalization

of V23m

matrix

div ide both

matrices

desired v elocity d4 dotd

Gain K

Lambda

d4hat matrix

qdot2v()

UpdateInputValuesAnd

Dependencies()

dd4desirefun()

dd4desirefun()

Calculated4Dotd()

56 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

COMPONENT DIAGRAM

Figure 26: Component diagram gives the relation between the physical and coding parts.

cmp Components

EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version


































Algorithm

+ Calculated4Dotd() : void

+ dd4desirefun() : void

+ qdot2v() : void

+ Update3x1Matrix() : void

+ Updated4DotdData() : void

+ UpdatedPreviousTimeMs() : void

+ UpdateInputValuesAndDependencies() : void

User

Matrix

Crane experiments

CI12Base

+ Initialized4DotdCalculation() : void

+ ReadInputValue() : void

+ WriteSysVar() : void

57 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

ACTIVITY DIAGRAM:

Figure 27: Activity diagram gives the relation between the physical and coding parts.

act RUNNING THE CRANE LAB

EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version









































ActivityInitial

INITIALIZATION

Found

both

zero

positions

FIND ZERO POSITIONS

GREEN LIGHT

NORMAL OPERATION

RED LIGHT

READ

INPUTS(THETA2,THETA3,D4)

CONVERSION OF INPUTS UPDATE PARAMETERS Calculated4Dotd

VELOCITY

WRITE OUTPUTS

The Loop goes to the

point where we stil l

need to find the Zero

positions

[True]

[False]

58 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

STATE DIAGRAM:

Figure 28: State diagram gives the relation between the physical and coding parts.

stm CRANE LABEA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version EA 7.5 Unregistered Trial Version

































Initial

Conditions

Normal Operation

UNDEFINED_HAS_NO_ZERO_POS

UNDEFINED_HAS_THETA2_ONLYUNDEFINED_HAS_THETA3_ONLYZero positions of the

Theta2 and Theta 3

Angles

Theta 2 Angle

Theta 3 Angle

Theta 3 Angle

Theta 2 Angle

59 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

60 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

12. CODE DOCUMENTATION

Algorithm.h

#include "matrix.h"

//

// Defines

//

#define PI 3.14159265358979323846f

#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


the previous angle


the previous angle

61 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 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






double El4x2Block[4][2];

matrix El4x2Matrix; // elbowarm (homogeneus coordinates). TBD! Only used




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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

//

void Initialized4DotdCalculation(struct Calculated4DotData* dp);

void Updated4DotdData(double gain, double a2, struct Calculated4DotData* dp);

/*--------------------------------------------------------------------------------

----------------------------------------------------

Fucntion : Calculated4Dotd

Parameters : theta2 - ...

theta3 - ...

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

{

}

#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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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->d4 += dp->dd4;

//O4 Position Update // TBD??

dp->xold=dp->x; // TBD??

dp->zold=dp->z; // TBD??

67 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

}

/*--------------------------------------------------------------------------------

----------------------------------------------------

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

UpdateParametersFromExperimentalData(&d4DotdData);

}

}

/*--------------------------------------------------------------------------------

----------------------------------------------------

Fucntion : <craneExp1a.c>





the delta values


----------------------------------------------------------------------------------

---------------------------------------------------*/

//

//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);




d4DotdData.theta2=30*(PI/180.0); // no side effects

d4DotdData.theta3=-30*(PI/180.0); // no side effects










}

else

{

// Animation loop

currentTimeMs ++;

bOk =

Calculated4Dotd(d4DotdData.theta2,d4DotdData.theta3,d4DotdData.d4,currentTimeMs,&d

4DotdData);

69 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57




}

}

/*--------------------------------------------------------------------------------

----------------------------------------------------

Fucntion : <craneExp1c.c>





the delta values


----------------------------------------------------------------------------------

---------------------------------------------------*/

//

//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;


int bOk;

if (loop == 0)

{






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









}

else

{

// Animation loop

currentTimeMs += 500;

70 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

bOk =


4DotdData);




}

}

/*--------------------------------------------------------------------------------

----------------------------------------------------

Fucntion : <craneExp1d.c>





the delta values


----------------------------------------------------------------------------------

---------------------------------------------------*/

//

//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;


int bOk;

if (loop == 0)

{




d4DotdData.d4c=2;



d4DotdData.theta2=(45)*(PI/180.0); // no side effects

d4DotdData.theta3=(-45)*(PI/180.0); // 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





}

else

{

71 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

// Animation loop


bOk =


4DotdData);




}

}

/*--------------------------------------------------------------------------------

----------------------------------------------------

Fucntion : <craneExp1e.c>





the delta values


----------------------------------------------------------------------------------

---------------------------------------------------*/

//

//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)

{




d4DotdData.d4c=1;



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





}

else

72 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

{

// Animation loop


bOk =


4DotdData);




}

}

#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)

{





}

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);


//second loop

//update I/O

theta2 += dtheta2;

theta3 += dtheta3;

d4 += dd4;

currentTimeMs++; // Increase time

bOk = Calculated4Dotd(theta2,theta3,d4,currentTimeMs,&d4DotdData);



}

}

73 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

#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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

*(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 (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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

}

*(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;


{

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

fprintf(stderr,"\nmid error - matrix must be square");

return 1;

}


{


*(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;



*(m->block+i*m->cols+j)=0.0;

}

double mnorm(m)

matrixptr m;

{

int i,j;

double sum=0;



sum += (*(m->block+i*m->cols+j)) * (*(m->block+i*m->cols+j));

return sqrtf(sum);

78 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

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 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

Changing the Sensors:

Figure 29: Location of HEX screws diagram

Figure 30: Location of Blots to change the sensors

80 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

Changing the Hardware setup:

Figure 31: Physical representation of the Joysticks connections.

Figure 32: Physical representation of the extension link connections.

81 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

MATLAB GENERATED RESULTS OF THE CODE:

Craneexp1:

dd4desire = 0.0492

dd4desire = 0.0480

dd4desire = 0.0466

dd4desire = 0.0451

dd4desire = 0.0434

dd4desire = 0.0415

dd4desire = 0.0395

dd4desire = 0.0375

dd4desire = 0.0354

dd4desire = 0.0332

dd4desire = 0.0310

dd4desire = 0.0288

dd4desire = 0.0265

dd4desire = 0.0243

dd4desire = 0.0220

dd4desire = 0.0197

dd4desire = 0.0175

dd4desire = 0.0153

dd4desire = 0.0130

dd4desire = 0.0108

dd4desire = 0.0086

82 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

Figure 33: Simulation of the crane links while picking the log from the ground.

Figure 34: Crane trajectory while moving down to the ground.

0 5 10 15 20 25-0.05

0

0.05

0.1Crane, kinematic trajectory, go to a logg on the ground

k

o4 v

eloc

ity

-1 0 1 2 3 4 5-1

0

1

2

3

4

5Crane, kinematic trajectory, go to a logg on the ground

lateral position, x

heig

ht,

z

83 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

Craneexp2:

dd4desire = 0.0421

dd4desire = 0.0423

dd4desire = 0.0424

dd4desire = 0.0425

dd4desire = 0.0426

dd4desire = 0.0425

dd4desire = 0.0424

dd4desire = 0.0422

dd4desire = 0.0419

dd4desire = 0.0416

dd4desire = 0.0412

dd4desire = 0.0407

dd4desire = 0.0402

dd4desire = 0.0396

dd4desire = 0.0389

dd4desire = 0.0382

dd4desire = 0.0375

dd4desire = 0.0367

dd4desire = 0.0359

dd4desire = 0.0351

84 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

Craneexp3:

dd4desire = -0.0122

dd4desire = -0.0162

dd4desire = -0.0201

dd4desire = -0.0239

dd4desire = -0.0275

dd4desire = -0.0311

dd4desire = -0.0345

dd4desire = -0.0377

dd4desire = -0.0407

dd4desire = -0.0435

dd4desire = -0.0461

dd4desire = -0.0485

dd4desire = -0.0507

dd4desire = -0.0526

dd4desire = -0.0542

dd4desire = -0.0556

dd4desire = -0.0567

dd4desire = -0.0576

dd4desire = -0.0581

dd4desire = -0.0584

dd4desire = -0.0584

dd4desire = -0.0581

dd4desire = -0.0576

85 | P a g e

THESIS NO.MEE10:31.



Tel.vx 0455-38 50 00

Fax 0455-38 50 57

dd4desire = -0.0568

dd4desire = -0.0558

dd4desire = -0.0545

dd4desire = -0.0530

Figure 35: Crane link trajectory while picking the log from the ground.

-1 0 1 2 3 4 5-1

0

1

2

3

4

5

6Crane, kinematic trajectory, "load a logg from the ground"

lateral position, x

heig

ht,

z

dsp implementation of a-control algorithm for a …

Documents