dsp implementation of a-control algorithm for a …
Post on 20-Apr-2022
2 Views
Preview:
TRANSCRIPT
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
7 | 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
11 | 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
17 | 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
22 | 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
24 | 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
31 | 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
42 | 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
45 | 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
48 | 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
51 | 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
55 | 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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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
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 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
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 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
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 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
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 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
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 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
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 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
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 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
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 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
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 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
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 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
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 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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
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 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 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 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 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
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 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 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 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 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
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 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 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 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 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
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 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 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 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 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
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 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 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 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 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
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 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 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 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 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
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 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 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 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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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
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 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 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 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 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
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 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 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 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 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
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 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 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 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 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
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 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 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 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 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
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 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 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 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 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
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 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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
60 | 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
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
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 | 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
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 | 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
//
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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->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 | 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
}
/*--------------------------------------------------------------------------------
----------------------------------------------------
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
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 | 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
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 | 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
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 | 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
// 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 | 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
{
// 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 | 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
#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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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 (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 | 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
}
*(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 | 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
Tel.vx 0455-38 50 00
Fax 0455-38 50 57
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 | 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
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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.
Blekinge Tekniska Högskola
SE–371 79 Karlskrona
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
top related