Technical University of Catalonia (UPC)

Master’s Degree:

Automatic Control and Robotics

Master’s Thesis

Creating an open source platform to develop

embedded control systems, and performing a
control application on a real-time distributed
system

CARLOS XAVIER ROSERO CHANDI

Advisors: MANEL VELASCO GARCÍA & PAU MARTÍ COLOM

Academic Course 2014/15

July 2015
Abstract
In this project, it has been created a bootloader for the dsPIC33-based board named
as Full FLEX, which works jointly with the Erika Enterprise RTOS. In addition, was
designed a firmware to perform USB/UART bridging on a PIC18F microcontroller that
is also placed inside this board.
A python-based software, which decodes both ELF or HEX files into binary source
and sends it via USB, from any computer towards the bootloader on the controller, has
been created. Furthermore, at runtime the same communication channel can be used to
establish a transparent bidirectional communication between the controller and any other
software.
With this implementation, at development stage, it has been removed an additional
IDE and one programming-tool used in the past to program the controller. Furthermore,
two additional devices utilized to establish communication between the board and any
computer software at runtime stage, are also avoided. So it was possible to reduce both
development time and cost spent on tools.
It was decided to apply this tool for designing a control system of an inverted
pendulum. This application is based on four nodes linked via a CAN. Each node has
implemented the bootloader and also runs the above cited real-time operating system.
Testing and analyzing results lead to conclude that the system is reliable, efficient and
works in harmony beside the RTOS. In addition, the pendulum control system is stable
and robust, plus it has achieved the desired control objectives.
The development was carried out in the context of having a complete new platform
that will be used to facilitate the creation of real-time and/or distributed control sys-
tems that can be used both by researchers and students at the Real-Time Distributed
Control Systems Laboratory in the Automation Department of the Technical University of
Catalonia.
The whole project and its documentation are available at github.com/xavierrosero/
FlexboardBootloader.
4
Contents

1 Introduction 15
1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
1.2 Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
1.3 Document structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

2 State of the Art 19

2.1 Development of embedded systems in its beginnings . . . . . . . . . . . . . 19
2.1.1 Choosing the suitable microcontroller . . . . . . . . . . . . . . . . . 19
2.1.2 Firmware development . . . . . . . . . . . . . . . . . . . . . . . . . 20
2.1.2.1 Mastering the hardware modules via APIs . . . . . . . . . 21
2.1.2.2 Performing the main code . . . . . . . . . . . . . . . . . . 21
2.1.3 Hardware development . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.1.3.1 Prototyping the hardware . . . . . . . . . . . . . . . . . . 21
2.1.3.2 Developing the actual hardware . . . . . . . . . . . . . . . 22
2.1.4 Drawbacks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
2.2 An improvement: IDE plus open-hardware . . . . . . . . . . . . . . . . . . 22
2.2.1 Multitarget . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.2.2 Scalable . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.2.3 Multilevel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
2.2.4 Multiplatform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
2.2.5 Support . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
2.2.6 Emerging trends . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
2.2.7 Relevant examples . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
2.2.8 Drawbacks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
2.3 RT-Druid IDE and FLEX boards . . . . . . . . . . . . . . . . . . . . . . . 26
2.3.1 Conventional way to program FLEX boards . . . . . . . . . . . . . 27
2.3.2 Drawbacks of the conventional programming . . . . . . . . . . . . . 27
2.3.3 Conventional way to comunicate FLEX boards . . . . . . . . . . . . 28
2.3.4 Drawbacks of the conventional linking . . . . . . . . . . . . . . . . 28

5
3 Bootloader Application 29
3.1 New approach proposed . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.2 Hardware characterization . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.2.1 FLEX base board . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.2.2 dsPIC33FJ256MC710 digital signal controller . . . . . . . . . . . . 32
3.2.3 PIC18F2550 USB-microcontroller . . . . . . . . . . . . . . . . . . . 32
3.2.4 Connections on FLEX board . . . . . . . . . . . . . . . . . . . . . . 33
3.3 Firmware description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
3.3.1 DSC-bootloader . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
3.3.2 USB-bridge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
3.4 Software depiction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
3.4.1 Master-code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
3.4.2 Calls to master-code functions . . . . . . . . . . . . . . . . . . . . . 45
3.5 Implementing bootloader application . . . . . . . . . . . . . . . . . . . . . 47
3.5.1 Link between Bootloader and Eclipse IDE . . . . . . . . . . . . . . 47
3.5.2 New GLD-linker-script . . . . . . . . . . . . . . . . . . . . . . . . . 48
3.5.3 Version of software tools . . . . . . . . . . . . . . . . . . . . . . . . 49
3.6 Performance analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
3.6.1 General tests . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
3.6.2 Programming time . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
3.6.2.1 Analysis of results . . . . . . . . . . . . . . . . . . . . . . 50

4 Implementation and Testing on the Pendulum Control System 51

4.1 System overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
4.2 Hardware characterization . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
4.2.1 CAN interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
4.2.2 H-bridge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
4.3 Firmware description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
4.3.1 Rod-sensor node . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
4.3.2 Cart-sensor node . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
4.3.3 Controller node . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
4.3.4 Actuator node . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
4.4 Software depiction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
4.5 Implementing pendulum application . . . . . . . . . . . . . . . . . . . . . . 70
4.5.1 Description of the target pendulum . . . . . . . . . . . . . . . . . . 71
4.5.2 State calculations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
4.5.3 Down-position control . . . . . . . . . . . . . . . . . . . . . . . . . 75
4.5.3.1 Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
4.5.3.2 Controller design . . . . . . . . . . . . . . . . . . . . . . . 77
4.5.4 Up-position control . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
4.5.4.1 Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
4.5.4.2 Controller design . . . . . . . . . . . . . . . . . . . . . . . 82

6
 4.6 Testing pendulum application . . . . . . . . . . . . . . . . . . . . . . . . . 85
4.6.1 Input-output latency . . . . . . . . . . . . . . . . . . . . . . . . . . 85
4.6.2 Networking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
4.6.3 Testing down-position control . . . . . . . . . . . . . . . . . . . . . 89
4.6.4 Testing up-position control . . . . . . . . . . . . . . . . . . . . . . . 90

5 Conclusions and further work 93

5.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
5.2 Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
5.3 Further work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

A Code Listing 99
A.1 Bootloader software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
A.1.1 Master code (serialBoot.py) . . . . . . . . . . . . . . . . . . . . . . 99
A.1.2 Settings GUI (settings.py) . . . . . . . . . . . . . . . . . . . . . . . 119
A.1.3 Reset GUI (reset.py) . . . . . . . . . . . . . . . . . . . . . . . . . . 124
A.1.4 Erase GUI (erase.py) . . . . . . . . . . . . . . . . . . . . . . . . . . 125
A.1.5 Linker file (linker.sh) . . . . . . . . . . . . . . . . . . . . . . . . . . 127
A.2 Bootloader firmware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128
A.2.1 DSC main code (main.c) . . . . . . . . . . . . . . . . . . . . . . . . 128
A.2.2 DSC atomic subroutines (memory.s) . . . . . . . . . . . . . . . . . 135
A.3 USB bridge firmware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
A.3.1 PIC18F code (base.c) . . . . . . . . . . . . . . . . . . . . . . . . . . 137
A.3.2 PIC18F complementary code (base complemento.h) . . . . . . . . . 138
A.4 Pendulum control software . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
A.4.1 Pendulum GUI (pendulum.py) . . . . . . . . . . . . . . . . . . . . . 139
A.4.2 Real-time communication file (rtMonitoring.py) . . . . . . . . . . . 154
A.5 Pendulum control firmware . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
A.5.1 Controller-node main code (controller.c) . . . . . . . . . . . . . . . 159
A.5.2 Actuator-node main code (actuator.c) . . . . . . . . . . . . . . . . . 177
A.5.3 Rod-sensor-node main code (sensorROD.c) . . . . . . . . . . . . . . 181
A.5.4 Cart-sensor-node main code (sensorCART.c) . . . . . . . . . . . . . 185

7
8
List of Figures

1.1 Comparison between old way and new approach at development stage . . . 16
1.2 Comparison between old way and new approach at runtime stage . . . . . . 16

2.1 Conventional development of embedded systems . . . . . . . . . . . . . . . 20

2.2 Sketch of IDE plus open-hardware . . . . . . . . . . . . . . . . . . . . . . . 23
2.3 Conventional way to program FLEX boards . . . . . . . . . . . . . . . . . 27
2.4 Conventional way to link FLEX boards with any computer . . . . . . . . . 28

3.1 Full-implementation of bootloader . . . . . . . . . . . . . . . . . . . . . . . 30

3.2 FLEX base board . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.3 FLEX mechanical drawing with certain connections . . . . . . . . . . . . . 34
3.4 Flow of DSC-bootloader . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.5 Flow of the interpretation of composed commands on DSC-bootloader . . . 36
3.6 Program memory distribution . . . . . . . . . . . . . . . . . . . . . . . . . 38
3.7 Flow of USB-bridge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
3.8 Call with argument to run only a requested command . . . . . . . . . . . . 40
3.9 Call without argument to run proprietary console . . . . . . . . . . . . . . 41
3.10 Flow of computer-bootloader . . . . . . . . . . . . . . . . . . . . . . . . . . 42
3.11 Calls to master-code functions . . . . . . . . . . . . . . . . . . . . . . . . . 45
3.12 Erase GUI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
3.13 Reset GUI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
3.14 Settings GUI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

4.2 CAN interface daughter board . . . . . . . . . . . . . . . . . . . . . . . . . 52

4.1 Pendulum control application . . . . . . . . . . . . . . . . . . . . . . . . . 53
4.3 MD03 H-bridge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
4.4 Sampling strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
4.5 Pendulum schedule . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
4.6 Flow of rod-sensor node . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
4.7 Flow of cart-sensor node . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
4.8 Flow of controller node . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
4.9 Control thread in controller node . . . . . . . . . . . . . . . . . . . . . . . 63
4.10 Short calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
4.11 Large calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

9
4.12 Flow of actuator node program . . . . . . . . . . . . . . . . . . . . . . . . 68
4.13 Pendulum GUI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
4.14 Real pendulum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
4.15 Mechanical part and sensors of pendulum-cart setup . . . . . . . . . . . . . 72
4.16 Implementation of the actual-pendulum’s control system . . . . . . . . . . 72
4.17 Illustration of down-position control . . . . . . . . . . . . . . . . . . . . . . 76
4.18 Step-response simulation of down-position control . . . . . . . . . . . . . . 79
4.19 Illustration of up-position control . . . . . . . . . . . . . . . . . . . . . . . 80
4.20 Simulation of up-position control . . . . . . . . . . . . . . . . . . . . . . . 84
4.21 Real pendulum at work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
4.22 Measurement of input-output latency . . . . . . . . . . . . . . . . . . . . . 86
4.23 CAN frame sent from rod-sensor node towards cart-sensor node . . . . . . 87
4.24 CAN frame sent from cart-sensor node towards controller node . . . . . . . 87
4.25 CAN frame sent from controller node towards actuator-node . . . . . . . . 88
4.26 Sequence of CAN frames managed in the sampling-processing-actuating
process . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
4.27 Step-response of the down-position control . . . . . . . . . . . . . . . . . . 89
4.28 Test under perturbation of down-position control . . . . . . . . . . . . . . 90
4.29 Step-response of down-position control . . . . . . . . . . . . . . . . . . . . 90
4.30 Test under perturbation of up-position control . . . . . . . . . . . . . . . . 91

10
List of Tables

2.1 Criteria for selecting the right microcontroller . . . . . . . . . . . . . . . . 20

2.2 Comparison between different platforms . . . . . . . . . . . . . . . . . . . 26

3.1 dsPIC33FJ256MC710 features . . . . . . . . . . . . . . . . . . . . . . . . . 32

3.2 PIC18F2550 features . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.3 Serial commands from computer-bootloader to DSC-bootloader . . . . . . 36
3.4 General settings for DSC-bootloader microcontroller . . . . . . . . . . . . . 37
3.5 General settings for USB-bridge microcontroller . . . . . . . . . . . . . . . 39
3.6 User commands of computer-bootloader application . . . . . . . . . . . . . 43
3.7 External tools attached to Eclipse IDE . . . . . . . . . . . . . . . . . . . . 48
3.8 Version of software tools . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
3.9 Test of programming time . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

4.1 General settings for Erika Enterprise RTOS into DSCs . . . . . . . . . . . 56

4.2 CAN network settings . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
4.3 CAN messages description . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
4.4 CAN acceptance filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
4.5 Setup commands used by controller node . . . . . . . . . . . . . . . . . . . 59
4.6 Threads of rod-sensor node . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
4.7 Threads of cart-sensor node . . . . . . . . . . . . . . . . . . . . . . . . . . 61
4.8 Description of controller-node’s UART-incoming-messages . . . . . . . . . 64
4.9 Threads of controller node . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
4.10 Description of controller-node’s UART-outgoing-messages . . . . . . . . . . 66
4.11 States of calibration process . . . . . . . . . . . . . . . . . . . . . . . . . . 67
4.12 Threads of actuator node . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
4.13 Parameters of the pendulum . . . . . . . . . . . . . . . . . . . . . . . . . . 73
4.14 States description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73

11
12
Listings

A.1 Bootloader file . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99

A.2 Settings-GUI file . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
A.3 Reset-GUI file . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
A.4 Erase-GUI file . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125
A.5 Linker file . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
A.6 DSC main code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128
A.7 DSC atomic subroutines . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135
A.8 PIC18F code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
A.9 PIC18F complementary code . . . . . . . . . . . . . . . . . . . . . . . . . . 138
A.10 Pendulum-GUI file . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
A.11 Real-time communication file . . . . . . . . . . . . . . . . . . . . . . . . . 154
A.12 Controller-node main code . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
A.13 Actuator-node main code . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177
A.14 Rod-sensor-node main code . . . . . . . . . . . . . . . . . . . . . . . . . . 181
A.15 Cart-sensor-node main code . . . . . . . . . . . . . . . . . . . . . . . . . . 185

13
Acknowledgment
Foremost I want to thank Dr. Manel Velasco and Dr. Pau Marti for their excellent
leadership and also for providing valuable technical and scientific support to this work.
In addition for letting me belong to the research group of Real-Time Distributed Control
Systems of ESAII-UPC.

Last but not least, I would like to express my infinite gratitude to my parents for their
love and wholehearted support.
Chapter 1

Introduction

This master thesis has been made with the Group of Research in Real-Time Distributed
Control Systems from the Department of Automation at the Technical University of Cat-
alonia.

1.1 Motivation

The development of real-time systems via FLEX, Erika and RT-Druid, is usually a tedious
process as it involves numerous steps for developing and debugging; this includes addi-
tional hardware and software than those only needed. For example the ELF file produced
from the user’s code must be translated into a binary file through another proper IDE and
then go through a programming-hardware in order to reach the final target. As it can be
seen, is a very long process, sometimes non user-friendly and expensive as it needs more
hardware. Apart from that, while an application is running, the communication between
the computer and the board is achieved using even more hardware increasing the cost
much more.
All these reasons lead to the creation of a Bootloader application for replacing
the middle software and hardware. In order to achieve that, a python-based code and
a bootloader firmware are developed to decode the ELF files into binaries, and access
directly the programming board. In order to achieve that, a microcontroller-based bridge
is designed to link the computer with the bootloader target (Figure 1.1). This solution
results to avoid also the additional hardware needed to communicate any software at
runtime stage (Figure 1.2).

15
 Figure 1.1: Comparison between old way and new approach at development stage

Furthermore it is proposed to use the new platform for implementing the control
system of an outdated inverted pendulum which resides in the laboratory of the research
group. With this application it is possible to demonstrate the robustness of the created
platform and also that a real-time distributed system can control a fast-dynamic device.

Figure 1.2: Comparison between old way and new approach at runtime stage

The new version of the pendulum serves at once, for students and developers to try
new approaches like kernel-based and network-based real-time systems, or ones based on

16
different ways of delay modeling or on sampling criteria, scheduling algorithms, among
others.

1.2 Objectives
The overall objective of this project is to create an open source platform for the develop-
ment of embedded control systems in order to perform control applications on real-time
distributed systems. The following objectives are also fulfilled:

• Create a software tool on GNU/Linux-based operating system which serves for de-
coding ELF-files, handling USB communication, reading and writing program code
on the FLEX board microcontroller, and as a human-machine interface.

• Perform a bootloader for the DSC which allows to manage its internal flash memory,
as well as UART communication and initialization of the RTOS1 .

• Develop a USB/UART bridge with the USB microcontroller (also embedded inside
the FLEX board), used to switch in a transparent and fast way, the information
between the main microcontroller and the computer.

• Design and implement a real-time distributed system (through a CAN network) to

control an inverted pendulum, and thus test the reliability and robustness of the
created platform.

1.3 Document structure

The document consists of four chapters beside to this chapter of introduction, and an
appendix.
Chapter 2 introduces the state of the art. There, it is explained in a general way the
development of applications through IDEs plus open-hardware, making a short comparison
between some platforms that exist in the marketplace. Also it describes the conventional
way used to develop projects through FLEX boards.
The framework that has been used to create the bootloader application, and a de-
scription of the developed hardware, software and firmware are detailed in Chapter 3.
Additionally, there is a performance analysis after its implementation. That is the cen-
tral and most extensive part of the document and is described from the point of view of
computer science.
In Chapter 4 both the methodology taken to implement the pendulum control system,
as the mathematics used in the design, are described. Beside that, tests to check its
perfect operation are explained and analyzed. There, control criteria are used.
1
Real Time Operating System

17
 Chapter 5 shows the conclusions of this development and also it outlines some possible
lines of future work.
Finally, the Appendix shows the code of the whole project.

18
Chapter 2

State of the Art

Here, it is explained in a general way the development of applications through IDEs plus
open-hardware, making a short comparison between some platforms that exist in the
marketplace. Also it describes the conventional way used to develop projects through
FLEX boards.

2.1 Development of embedded systems in its begin-

nings

Considering the microcontroller as the basis for design of embedded systems, very few
years ago, the development of applications appeared very different from what exists nowa-
days. Every project had to start from scratch in both hardware and firmware. Everything
starts with a problem to be solved and for this, a long chain of processes composed of
two branches follows: hardware design and firmware programming (see Figure 2.1). Both
lead to develop the final application and as can be seen, they are developed in parallel,
since the construction of the final system is found in continuous debugging. Below, each
of the steps is slightly explained.

2.1.1 Choosing the suitable microcontroller

After the problem to solve, the primary step comes: choosing the microcontroller. This
requires to apply a tradeoff among several criteria, which actually depend on the point of
view of the designer. In Table 2.1, the author presents certain alternatives to group some
criteria used, to choose the best microcontroller:

19
 Figure 2.1: Conventional development of embedded systems

Table 2.1: Criteria for selecting the right microcontroller

Working
Performance Budget Support
conditions
−Libraries
−Physical size of −Commercial
− Size of memories available to drive
the application availability
internal modules
−Electrical
−Cost of tools for −Libraries to drive
−Throughput characteristics and
development external elements
isolation
−Rejection to −Availability of
−Hardware
electromagnetic help and examples
modules
interference online
−Professionals in
the local
−Number of
−Architecture environment who
inputs/outputs
can provide advice
and training
−Arithmetic-logic −Working
operations temperature
−Hardware to
support processor
(DSP1 engine,
DMA2 )

2.1.2 Firmware development

Two steps which actually are not entirely sequential are known. These are the program-
ming of APIs and programming of the final application.
1
Digital Signal Processor
2
Direct Memory Access

20
2.1.2.1 Mastering the hardware modules via APIs

Each brand and each family of microcontrollers brings different peripherals and even
different microprocessors and architecture. Therefore, whenever it has been dabbled with
something new, much effort is required to understand the insight and learn the pragma
of new hardware.
Finally, as a result of debugging the subroutines that are now able to manage the
hardware microcontroller, the APIs are obtained. These are called from the main program
and provide a hardware abstraction layer.
If higher-level languages are used, this step practically disappears. But this is not
entirely true because for very specific applications, the APIs that come by default in
compilers do not provide sufficient contact with the hardware. The developer is bound
to know low-level language, at least to initialize the specific purpose RAM3 intrinsically
linked to the hardware configuration.
More sophisticated libraries, such as those for handling mathematical operations are
provided by the microcontroller’s manufacturer.

2.1.2.2 Performing the main code

It is not quite true that there is sequentiality between developing of the APIs and working
on the final application. Actually the process is a continuous debugging of the APIs, while
the final application is developed.
Despite being the most important step, it is typically one of the simplest. Often, it
takes longer to develop the hardware and libraries, rather than solving the problem di-
rectly. Note that the problem itself is solved through algorithms or mathematical models.

2.1.3 Hardware development

It is a manual process and usually it can be ordered. It consists of two parts: testing
the hardware through breadboards or hardware for development, and then producing the
final application on a PCB4 .

2.1.3.1 Prototyping the hardware

Prototyping starts with a schematic diagram, which is the result of a design that involves
arithmetic and logic calculations, a characterization of analog and digital electronics.
Among many other things, the design of the power supply as well as input/output in-
terfaces are considered primordial. Also a vital part is the design of all hardware-related
communications, both internal and external.
3
Random Access Memory
4
Printed Circuit Board

21
 Once a sketch is done directly into the breadboard, element by element, module by
module, then the whole system is tested. In the end, it is tested on the hardware already
mounted. The proper operation of the steps are described in sections 2.1.2.1 and 2.1.2.2.

2.1.3.2 Developing the actual hardware

Once the whole experiment is tested as described above, the PCB layout is designed
through CAD software5 and manufactured by CNC6 router or any other similar method.
This process would be assigned to specialized companies, however the cost of proto-
typing could be relatively high.

2.1.4 Drawbacks
• Such a conventional process demands longer time to develop and require greater
financial investment.

• It is more likely to get bugs because the process is more extensive.

• It needs to have expertise and is beyond the reach of a developer without academic
or professional training in this area.

• There is low scalability because slight changes in hardware, could demand redesign-
ing the PCB (this is relative but sometimes happens).

2.2 An improvement: IDE plus open-hardware

Both IDE 7 and open-hardware did not arise at the same time. In fact the first to appear
was the IDE in which, program edition and compilation8 gathered in one application.
Also details such as comfort, efficiency and support were added stepwise.
Subsequently open-hardware appeared as a solution to many setbacks encountered
by developers. Its statement is simple. Develop hardware and simultaneously leave free
everything that was a secret before: schematics, PCB layouts, and files used for design.
Furthermore, the open-hardware may be considered as a free source for anyone for using,
modifying, or even selling under GPL9 -like.
By putting together IDE and open-hardware, complete platforms were obtained. A
general sketch is shown in Figure 2.2. Despite the fact that is not right to generalize
any architecture, the illustration figures out a general idea. Inside a computer, a single
software is used both to develop the application and to download it on the microcontroller.
5
Computer-Aided Design
6
Computer Numerical Control
7
Integrated Development Environment
8
Translation from source code into executable program by a compiler
9
General Public License

22
 Figure 2.2: Sketch of IDE plus open-hardware

Furthermore, the open-hardware board has an embedded device to program the main
microcontroller. The most generalized form is through a bridge which can communicate
with both the computer and the main microcontroller, using different protocols. Into the
microcontroller a proprietary bootloader10 runs.
After analyzing several environments that are available nowadays, it follows that there
are certain standards that every platform of this kind should overcome: multitarget,
scalable, multilevel, multiplatform and support.

2.2.1 Multitarget
The choice depends on the user skills. Advanced users can choose the board that fits
their needs, from several options, applying certain selection criteria as seen in Table 2.1.
However, for novice users it is common the use of basic boards intent for use in general
applications.
Among the available options, someone could find boards with differences in processor
architecture, size, number of pins, communication ports and cost, among others.

2.2.2 Scalable
There are widgets created by the manufacturer, or other third-party manufacturers, pro-
viding the touch of versatility to the platform. Thus, once the user has chosen a board,
several options of widgets that can expand the capabilities of the board to immeasurable
levels.
10
Special firmware for transferring user’s application from the computer to the microcontroller memory
and handing over the control to this program afterwards

23
 Widgets have their own APIs ready to be attached to the main code, so that the
developer follows the paradigm of solving the problem directly, instead of starting with a
development from scratch.
On the other hand, if a widget does not exist and the user has sufficient knowledge,
he can create his own hardware, connecting properly and developing its libraries.

2.2.3 Multilevel
It is possible to use the same open-hardware but with other IDEs. Thus the following
options arise:
• With own IDE, by using language and software tools that come by default.
• With an IDE from the microcontroller’s manufacturer, even using the same pro-
gramming hardware that is included on the board, or by an official programming
hardware from the brand. This type of development is done by experienced users
as this can involve deleting the bootloader, among other practices.
• With an IDE provided by third parties, but all depends on the availability of this
option.
Note that using each of the possibilities, should deal with different levels of abstrac-
tion. Thus, advanced users can virtually work directly with the hardware creating or
customizing their own APIs, using low-level languages.
On the other hand, users who do not have deep knowledge of hardware nor program-
ming skills, can use IDEs with higher-level languages, using libraries provided by default.
This provides a level of abstraction such that they can write easily the application with-
out spending hours or even days learning pragma, configuration bits or command line
compilers.

2.2.4 Multiplatform
Given the diversity of operating systems used around the world, this feature is very
important when trying to massively sell such products. The IDEs, compilers and libraries
are usually available for Unix-like, Windows and other kind of operating systems, both
for 32-bit as well as 64-bit.

2.2.5 Support
Prototyping has become fashionable, allowing even people with different background
than an engineer, to translate their ideas quickly. It is a process that sometimes called
DIY11 . This has increased the number of open-hardware applications and therefore the
entrepreneurship.
11
Do It Yourself

24
 Support is based on examples and libraries provided by the manufacturer, developers
in forums and third parties. The trick is that, having released all that information, the
more expert users are able to provide debugging and support, just by participating in
forums.

2.2.6 Emerging trends

Some of the new trends are:

• For microcontroller platforms such as Arduino, operating systems are developed

that can be attached to the user code as libraries. NilRTOS12 or ChibiOS13 are
some examples. The drawback is that they are cooperative OS.

• For development platforms, increasingly complex processors are used, becoming vir-
tually SoCs14 , which integrate CPU cores (latency based), GPU cores (throughput
based), DSP, HW IPs15 cores, On-chip memories, among others. Such devices allow
the installation of large operating systems and provide an IDE on board, where the
IDE is into the operating system itself.

2.2.7 Relevant examples

Is too bold to say that the examples cited here are the most important, however, are
some of the platforms tested by the author. There are many other platforms (and many
to emerge) that are gradually improving and/or changing the concepts. Table 2.2 shows
a comparison between some platforms.

2.2.8 Drawbacks
As the hardware is open, there are already established companies that have started to
copy the hardware, attach it to their own prototypes, and use the IDE from original
manufacturer, without paying for it.
For teaching processes, the contents and the methodology must be restructured by the
teachers, as virtually all problems are solved and are already available on the network.
12
https://github.com/greiman/NilRTOS-Arduino
13
https://github.com/greiman/ChibiOS-Arduino
14
System on a Chip
15
Hardware Intellectual Property
16
Hardware
17
Operating System
18
https://www.arduino.cc
19
http://www.pinguino.cc
20
https://micropython.org
21
https://www.raspberrypi.org
22
http://beagleboard.org

25
 Table 2.2: Comparison between different platforms
IDE+Open-
Platform Architecture IDE on HW OS17
HW16
AVR, ARM
18 by Atmel, Python only Some with
Arduino Arduino tool
Quark SoC by on Linux-like Linux-like
Intel
PIC18F,
Pinguino tool,
Pinguino19 PIC32M by
MPLAB X
Microchip
ARM Cortex MicroPython
Low-level
MicroPython20 by STMicro- based on
Python OS
electronics Python 3
Idle, Scratch, Raspbian
Raspberry
ARM Python, (official),
pi21
Squeak/Linux among others
Idle, Scratch,
Angstrom,
Python,
BeagleBone22 ARM Cortex Ubuntu and
Squeak,
Debian
Cloud9/Linux

Something very important, especially when one wants to evaluate a student in most
fundamental languages such as assembler.

2.3 RT-Druid IDE and FLEX boards

This type of IDE plus open-hardware environment is widely used in academia and research.
So, users can develop general applications through MPLAB IDE, but for real-time projects
it is suggested the use of RT-Druid and Erika. In the second case, the development is a
bit more complicated since Erika RTOS involves a relatively small layer of abstraction.
For example, the user needs, among other things, to know how to configure timers to
adjust the granularity of the system.

RT-Druid Based on Eclipse23 , it is the development environment for Erika Enterprise

real-time operating system. It allows writing, compiling, and analyzing the applications
in one environment.

Erika Enterprise RTOS Erika kernel [2] supports task management, events, alarms,
resources, semaphores and error handling. It provides support for EDF24 and FP 25 , as
well as stack-sharing techniques and one-shot task model to reduce stack usage. There
23
Set of open-source multi-platform programming tools to develop rich client applications, refer to [3]
24
Earliest Deadline Scheduling
25
Fixed Priority Planning

26
is support for resource sharing between tasks. Erika has GPL and supports among other
microcontrollers, the Microchip dsPIC33 family.

2.3.1 Conventional way to program FLEX boards

The old process for creating a project can be seen in Figure 2.3 and is outlined as follows:
1. A new project is created using RT-Druid Eclipse.
2. After fixing syntactic and logical bugs, the code in C language is compiled and the
result is an ELF-extension executable file.
3. A new project is created in MPLAB using the executable file obtained in the previous
step.
4. After creating the project, it is proceeded to compile and upload the binary code into
microcontroller, this process is performed through the ICD3 programmer/debugger.
5. Every time a change in the code is needed, the use of both Eclipse and MPLAB
IDEs is mandatory.

Figure 2.3: Conventional way to program FLEX boards

2.3.2 Drawbacks of the conventional programming

As it can be noticed, the set of steps described above bring some operating problems such
as:
• Elongation of time for developing and debugging the project, due to ta large set of
steps.
• Premature fatigue of user due to lack of ergonomics. With fatigue, lack of effective-
ness comes from the hand.
• High development costs due to necessary acquisition of a programmer/debugger.

27
2.3.3 Conventional way to comunicate FLEX boards
Figure 2.4 shows the conventional way of linking the FLEX boards with any computer.
Note that for purposes of supervision26 the DSC typically is using the UART port; fur-
thermore almost all computers today have USB ports. So, this process is followed:

1. A program is running on the DSC and the information is transmitted via UART.

2. The UART data is converted to RS232 levels through the homemade RS232/UART
board.

3. The RS232 data is transformed into USB packets through the USB/RS232 cable.

4. The software receives the data.

5. When the software transmits back, the bridging process is repeated but in reverse.

Figure 2.4: Conventional way to link FLEX boards with any computer

2.3.4 Drawbacks of the conventional linking

Such process described above brings the following problems:

• A large amount of hardware leads to greater likelihood of errors in communication.

• The homemade board is unreliable because of its type of manufacturing.

• Various hardware components make the process, besides being expensive, difficult
to assemble.

26
Monitoring and parameter setting

28
Chapter 3

Bootloader Application

This chapter regards the detailed design of the bootloader application. Thus, particulars
of hardware, firmware and software used to develop it are described. This is the central and
most extensive part of the document and is depicted from the point of view of computer
science.

3.1 New approach proposed

The main goal of this development is to simplify the process of creating projects, by
eliminating the use of especialized software and hardware tools and offering alternatives
to replace them.
To perform the above, it is necessary to remove from the middle of the development
chain, both MPLAB IDE as ICD3 programmer. Thus, is proposed the use of the imple-
mentation in Figure 3.1. There are two main groups corresponding to a host computer
(with GNU/Linux) and a target board (FLEX board). Regarding the process represented
in this section, the following is the procedure to take:

1. A new project is created using RT-Druid Eclipse.

2. After fixing syntactic and logical bugs, the code in C language is compiled and the
result is an ELF-extension executable file.

3. From Eclipse a call to bootloader application is made. Thus, the bootloader inter-
prets the ELF-extension file, creates a binary one, and sends it via USB towards
the target board.

4. Every time a change in the code is needed, only using Eclipse is mandatory.

As can be seen from the above steps, this process is much simpler, with less time and
effort to develop applications.

29
 Figure 3.1: Full-implementation of bootloader

Within some particulars of the proposed system is the use of another microcontroller
for bridging between USB and UART1 . This microcontroller is located inside the FLEX
board but has not been used so far, coming empty from factory. By writing firmware for it
and making some connections inside the board, it could be enabled for this entrustment.
Furthermore, considering bootloader’s software and firmware2 , following observations
are made:

• Bootloader application on computer is made to decode the ELF or HEX exe-

cutable files, transforming them into binary code to be sent via USB to the card. It
must be called through command line to be easily appendable to Eclipse environ-
ment.

• Bootloader firmware must be programmed into DSC previously and only once,
to start working. It is located at certain dedicated program memory region (not
available for user access). On start-up, the bootloader sorts between either loading
user application or staying in configuration mode3 .

3.2 Hardware characterization

There are two main hardware components: computer and FLEX board. On the former
there is not much to talk about hardware because any operating system implements layers
of abstraction between software and hardware. This allows to be aware only whether a
computer has the required hardware, but not a direct touch with it.
1
Universal Asynchronous Receiver/Transmitter
2
Matter to be detailed in sections 3.3 and 3.4
3
Configuration mode regards the time when bootloader is able to communicate via UART and to
manage program memory

30
3.2.1 FLEX base board
FLEX base board is designed to exploit and externalize all the connections of the Mi-
crochip dsPIC DSC microcontroller inside. This development tool allows to implement
both, user own applications or ones implemented using Erika RTOS. The board is shown
in Figure 3.2.

Figure 3.2: FLEX base board

Some features of FLEX board taken from [15] are the following:

• Microchip dsPIC DSC microcontroller dsPIC33FJ256MC710 inside.

• Microchip PIC18F2550 microcontroller inside, for USB connection.

• ICD3 in-circuit program connector.

• USB connector for communication.

• Set of LEDs for monitoring the board functioning status.

• Set of connectors for daughter boards piggybacking.

• Power supply connectors.

• Power supply circuitry with resettable fuses.

• Extra-robust switching power supply (9 - 36V).

31
3.2.2 dsPIC33FJ256MC710 digital signal controller

Inside FLEX board is located the dsPIC33FJ256MC710 16-bit microcontroller. Some

relevant features are shown in Table 3.1. Detailed information about this hardware device
can be obtained from [9].

Table 3.1: dsPIC33FJ256MC710 features

Main feature Description
Operating range Up to 40 MIPS operation.
Indirect, modulo and bit-reversed addressing modes. 16x16 fraction-
High-performance DSC CPU al/integer multiply operations. 32/16 and 16/16 divide operations.
Single-cycle multiply and accumulate.
DMA4 8-channel hardware DMA. Most peripherals support DMA.
Interrupt controller 5-cycle latency. Seven priority levels. Five processor exceptions.
Output pins can drive from 3.0V to 3.6V. All digital input pins are
Digital I/O
5V tolerant.
On-chip flash and SRAM5 256 Kbytes flash program memory. 30 Kbytes data SRAM.
Flexible clock options (external, crystal, resonator, internal RC with
System management
fully integrated PLL6 ).
Nine 16-bit timers that can pair up to make four 32-bit timers. Input
Timers/Capture/Compare
capture (8 channels). Output compare (eight channels)
Communication modules SPI7 , I2C8 , UART, ECAN9 .
Two modules in a device. 10-bit @ 1.1 Msps or 12-bit @ 500 Ksps
ADC10
conversion.
Motor control PWM (8 channels). QEI with phase A, phase B and
Motor control peripherals
index pulse input.

3.2.3 PIC18F2550 USB-microcontroller

Other important piece of hardware used for USB/UART switching also inside the FLEX
board is the PIC18F2550, details of which are found at [8]. Table 3.2 depicts some of its
whole set of features.

4
Direct Memory Access
5
Static Random Access Memory
6
Phase-Locked Loop
7
Serial Peripheral Interface
8
Inter-Integrated Circuit
9
Enhanced CAN
10
Analog-to-Digital Converter

32
 Table 3.2: PIC18F2550 features
Main feature Description
V2.0 compliant. Low speed (1.5 Mb/s) and full speed (12 Mb/s).
USB
Supports control, interrupt, isochronous and bulk transfers.
Flexible oscillator structure Four crystal modes, including high precision PLL for USB.
Four timer modules (one 8-bit, three 16-bit). Enhanced USART11
Peripheral highlights
module. SPI. I2C. 10-bit, 10-channel ADC.
On-chip flash and SRAM 32 Kbytes flash program memory. 2 Kbytes data SRAM.
Special microcontroller
8x8 single-cycle hardware multiplier. Priority levels for interrupts.
features

A relevant issue into this implementation regards the configuration of this microcon-
troller within the USB12 CDC13 . It is used primarily for devices with the need to stay
in a certain baudrate while performing a certain data transfer, despite the bus-workload.
Modems, fax machines and telephony applications for performing regular voice calls, use
it as a base for communication.
This device class is also for computer networking devices like a network card, providing
an interface for transmitting frames onto some physical media. It also can be used for
industrial equipment to allow upgrading from older RS-232 serial controllers, since they
can keep software compatibility.

3.2.4 Connections on FLEX board

FLEX has certain jumpers and terminals that allow reconfigure its hardware at the time
of creating an application. This versatility is exploited heavily in this project.
So that the bootloader can be programmed into the FLEX board, is necessary to move
some jumpers and make few connections with straps. Once this has been done, board can
work in developing any application, virtually transparently to the user.
Figure 3.3 shows in red and green, connections that enable communication between
UART ports from PIC18F and dsPIC33F. Also there are in orange certain jumper posi-
tions. All information related to hardware operation, including schematics, can be found
at [15].

11
Universal Synchronous/Asynchronous Receiver/Transmitter
12
Universal Serial Bus
13
USB Communications Device Class

33
 Figure 3.3: FLEX mechanical drawing with certain connections

Although FLEX board comes equipped for using the minimum amount of hardware,
CAN interface widgets are necessary to develop the network.

3.3 Firmware description

Bootloader comprises two microcontrollers and therefore two pieces of firmware. DSC-
bootloader is the primordial part as it is inside the same controller where the user will
put his application. USB-bridge is an aid, but very useful; it serves as a communication
interface between computer and DSC-bootloader.

3.3.1 DSC-bootloader
After a hard-reset occurs, DSC starts executing such reset vector located at address 0x00.
This vector contains a jump instruction, towards an address which is the one where user
code is located.
Taking advantage of this well known behaviour, such address is replaced by a new one
that corresponds to location of bootloader’s code at 0x400. So, every time DSC starts, it
will execute the bootloader rather than any other application.
Once bootloader is running, it waits for a guard time until an UART command is
received14 . If does not receive any command within this time, jumps to a fixed program
14
Via UART messages, the application on computer sends commands to read/write/erase program
memory

34
memory address 0xc0015 .
Therefore, it is understood that any user application that is loaded through boot-
loader, must be stored from address 0xC00. So into compilation process, some modifica-
tions should be made so that ELF or HEX files being loaded via bootloader, meet this
requirement.
Diagram in Figure 3.4 shows the flow of bootloader. After initialization of all the
hardware modules, a countdown timer is always active. If such timer has expired, a
variable named command is loaded with a run16 instruction. Subsequently the command
interpreter gets out from bootloader towards user application.

Figure 3.4: Flow of DSC-bootloader

If contrary, an UART command16 has been received, countdown timer will get disabled
once and for all iterations. Command is loaded with the already received byte and then
command interpreter executes a certain task. This means that after first UART command
is received, bootloader can not get out by itself, until computer sends an actual run.

15
At this address henceforth without exception is located the user application
16
See Table 3.3 for better understanding

35
 Figure 3.5: Flow of the interpretation of composed commands on DSC-bootloader

Commands interpretation Flowchart in Figure 3.4 only shows the execution of cer-
tain commands on a single byte long. For understanding those commands enclosed inside
block compound-command interpretation, is necessary to refer flow diagram from Figure
3.5 and information available in Table 3.3.
Table 3.3: Serial commands from computer-bootloader to DSC-bootloader
Command Format Return Comments
Read Memory-
[0x02],
memory- page18 512 location * 3 bytes/location = 1536 bytes
[Address17 ]
page contents
Actually does it in two steps: deletes whole
[0x03],
Write page and then writes new data. When trying
[Address],
memory- ACK19 to rewrite page 0, bootloader’s reset vector over-
[New
page lies but the rest of memory is programmed with
memory-page]
new data.
It is used both automatically starting user’s ap-
plication (when countdown timer gets over), or
Run [0x08] ACK
upon request from user through software on com-
puter.
Each microcontroller’s family has an identifica-
tion. This is read by computer as first step to
Read ID [0x09] ACK
find out whether bootloader is ready to commu-
nicate.
Erasing pages at 0x400 and at 0x800 (bootloader
place) is not allowed. When trying to delete page
Erase
[0x0a] ACK 0, bootloader’s reset vector overlies, the rest of
memory
memory is erased preserving bootloader’s reset
vector.
Starts certain intermittency on a LED (SOS
morse code). It is used for physically testing
Blink LED [0x0b] ACK whether certain board is connected to a port,
very useful when having attached several boards
(up to 5).

36
 Among some clarifications are the following:

• Reading, writing and erasing can not be performed byte-wise, on the contrary,
should be performed page-wise20 . One page has 512 4-byte memory locations, each
of which has only 24 useful bits instead of 32 bits logically understood.

• A subroutine is implemented to rewrite fuses21 but not used. This happens be-
cause being a prototype, fixed hardware settings for both the bootloader and user
application are utilized.

• By default, when a command is not recognized, or when a command has been

recognized but its argument is invalid, bootloader responds a NACK22 .

Hardware initialization Table 3.4 enlists several features and configurations of hard-
ware modules used by bootloader’s firmware.

Table 3.4: General settings for DSC-bootloader microcontroller

Module Settings
Input/output 1 output for showing flash sets corresponding to several working states.
Timer 3 configured to count a top of 2 seconds, incrementing the counter every
Timer
instruction cycle, an amount of 5e-8 seconds.
Oscillator Device operates at 40 Mhz = 20 MIPS for 4 Mhz input crystal clock via PLL.
UART 115200 bps fixed baudrate, 8-bit long, no parity, no flow control.

Notice in Figure 3.6 how interruption tables (IVT23 and AIVT24 ) are in green. This
means user application, loosely and transparently, uses all the facilities provided by in-
terruptions. This also explains why not use interruptions to develop the bootloader, that
is, to avoid having a big impact on hardware.

Code structure This issue goes beyond the description given in this written work. But
the fact remains on that every subroutine used to access DSC’s memory was developed
using atomic instructions mostly. Moreover, the calls to this subroutines are made in C
language from MPLAB. Reader is encouraged to find and analyze them at Annexes.
17
24-bit long unsigned integer
18
1536-byte long
19
Acknowledgment byte (0x01)
20
Guideline given by hardware manufacturer
21
Hardware configuration words
22
Non-Acknowledgment byte (0xFF)
23
Interrupt Vector Table
24
Alternate Interrupt Vector Table

37
Program-memory distribution In Figure 3.6, localities in red are those where boot-
loader has been settled absolutely and definitively, and could not be replaced by user25 .
Sections in green are those in which user’s code can be placed via bootloader without
disrupting the system. Blue localities are not physically available except the device con-
figuration registers. Regarding the latter, although has been implemented a subroutine
to access them, it is not explicitly used in the project.

Figure 3.6: Program memory distribution

3.3.2 USB-bridge
This program is implemented by using internal USB module from PIC18F2550. Oscilla-
tion frequency required by USB 2.0 is 48 Mhz, and as FLEX brings by default a crystal
of 20 MHz, an internal PLL module is used to obtain it.
This device has been set up within bounds of USB communications device class due
to the following reasons:

• It has priority over other devices to accomplish a fixed bandwidth.

• It is useful for data transfers where time is critical.

• Libraries needed are supplied by microcontroller’s manufacturer.

As shown in flowchart from Figure 3.7, hardware modules including USB are initialized
and then microcontroller waits to be attached and enumerated by host computer, as well
as waits for a carrier is detected, at which time sets interruptions due to UART reception.
Note that by means of carrier detection, it can know whether communication channel is
active and therefore exists flow of information.
25
Until microcontroller is programmed from scratch

38
 Figure 3.7: Flow of USB-bridge

Most important parameters configured in hardware modules can be seen in Table 3.5.

Table 3.5: General settings for USB-bridge microcontroller

Module Settings
One output is connected to DSC’s master clear reset pin used for resetting DSC
Input/output
just before starting communication with bootloader.
UART 115200 bps fixed baudrate, 8-bit long, no parity, no flow control.
USB Vendor ID: 0x04D8, Product ID: 0x0033, communication device class.
Interruptions USB events, UART reception.

Into the enumeration process, microcontroller answers a series of questions asked by

host. Herein, it sends information about identification of manufacturer, device version,
USB version that it supports, power supply requirements, among others. Host computer
provides an address to identify it.
As long as there are adequate conditions26 , two primary functions are performed by
microcontroller:

• Every byte which is received from computer via USB is forwarded via UART towards
DSC. This process is performed by main program loop.

• Every byte being received from DSC via UART, is forwarded towards computer
using USB. This is done through a serial-reception interrupt subroutine.
26
Attached and listed device, and carrier detected

39
 A very important clarification to be done is that USB and UART baudrates into USB
microcontroller are fixed in 115200 bps. This is because DSC baudrate is also fixed in
the same value due to robustness to noise and processing limitations of USB microcon-
troller. This rate is enough for both data transfer between bootloader and computer, as
sending supervisory information from user application. Communication reliability was
tested without problems in small sampling times of up to 5 milliseconds with frames up
to 33-byte long.

3.4 Software depiction

There are two ways to access the program by means of a console: via commands with
their respective arguments and by directly executing the script.
In the first, a call to the main script is needed, using the format [program] [script]
[-command]. With this method, one runs only a requested function from the script and
once completed, it leaves. In Figure 3.8, for example, a call is made to Python interpreter
with administrator privileges to run the h command from serialBoot.py script. As result
the deployment of such help menu is obtained. See Table 3.6 for further information
about commands.

Figure 3.8: Call with argument to run only a requested command

In the second way, the program runs displaying a console which remains active until
user types an appropriate command to leave it. Here, it is running on an indefinite
loop, processing commands as many times as one wants. Figure 3.9 details a proprietary
console displayed by the application; a brief explanation of what is seen at the picture is
the following:

• usbcan shows how many FLEX boards are attached to computer and at which
USB ports. In this case, there exist one board at /dev/ttyACM0 and a second at
/dev/ttyACM1.

40
 Figure 3.9: Call without argument to run proprietary console

• usbport assigns the USB port /dev/ttyACM0 to group 0 ; w means write.

• fileaddr, assigns the executable file .../workspace/0 controller/Debug/pic30.elf to

group 0 ; w means write.

• reset, reboots the board from group 0.

• erase, deletes a memory page from 0x00 to 0x3FF on the board from group 0. Notice
that in the representation 00x00, the first left 0 is not part of address, instead it
represents group 0.

• exit, ends the program.

3.4.1 Master-code
At diagram in Figure 3.10, the dynamics describing software operation is shown briefly.
Then, the most important blocks are described:

• Load stored settings, defines the script self location in order to know where the
rest of support files are located. One of these is a settings-file named configSerial-
Boot.config, in which five directory-port pairs are placed. Each directory corresponds
to the location of an *.elf or *.hex file (user application); each port regards the USB
port where a FLEX board is plugged.

• Check argument, allows verify whether the call to execution was done with or
without arguments.

41
 • In absence of arguments, the flow turns to the left half of the diagram. Here a
virtual console is emulated, which repeatedly waits for the user to enter a command
followed by the enter key. For commands help, usbscan and clear, being simpler, it is
clearly shown what they do in the figure. To deal with compound commands (with
arguments) the flow turns to the block apply certain actions based on commands.
These commands are explained in detail through Table 3.6.

• When there are arguments, each one of these in turn can contain either a com-
mand plus an argument, or a single command. Similarly as in previous item, for
compound commands, flow turns to the block apply certain actions based on com-
mands (see again Table 3.6).

Figure 3.10: Flow of computer-bootloader

Some stage directions that may clarify the picture, so it becomes more understandable
to reader, are:

• System is designed to work with up to five boards at once. That is the reason
to form five groups numbered from 0 to 4. Each group, as said before, has been
assigned the name and address of the file to be uploaded and also the USB port
where it is connected.
27
G is a number between 0 and 4 to represent the group
28
FILE is the full path of the file to be uploaded
29
PORT is the full path of the expected USB device

42
 Table 3.6: User commands of computer-bootloader application
Commands
Commands called
called from
from user Details and comments
external
interface
console
help −h Displays help menu.
clear Clears console screen.
reset G27 −sG Causes a hard reset and establishes connection with DSC-
bootloader at group G.
run G −rG Runs the program currently loaded on DSC device at group
G.
test G −bG Blinks the current device’s LED to identify it among others
attached to computer.
fileaddr Gr −xGr r: Shows the current .elf or .hex assigned to group G.
fileaddr GwF ILE 28 −xGwF ILE w: Assigns the FILE target, to group G.
usbscan −u Searches for available FLEX devices attached to computer.
usbport Gr −tGr r: Shows the current USB port assigned to group G.
usbport GwP ORT 29 −tGwP ORT w: Assigns the USB PORT target, to group G.
erase Ga −eGa a: Deletes the whole flash memory on device at group G.
erase G0xiiii −eG0xiiii Erases only a page from 0xiiii until 0xiiii+0x400, on device
at group G.
program Ga −pGa a: Programs the whole flash memory on device at group G.
program Gf −pGf f: Programs only the memory locations that contain data
(fast programming), on device at group G.
program G0xiiii −pG0xiiii Programs only a page from 0xiiii until 0xiiii + 0x400, on
device at group G.
read G0xiiii −dG0xiiii Reads a page from 0xiiii until 0xiiii + 0x400 on device at
group G.
exit Ends the software application.

43
 • Once groups are assigned, this setting will be kept because it is stored in a flat text
file.

• All the user commands allow both uppercase and lowercase letters, as well as the
mix between them.

• Accessing from external console, one might send multiple arguments at a time in one
call. This allows to perform various processes sequentially, with a single command
line.

• Accessing from internal console, it can be noted that software is made such that it
can overcome mistakes, treating them through very intuitive error messages.

• For program, read and erase commands, if user access to a memory address which is
not a multiple of 0x400 (pagewide), software approaches to the next lower multiple
of 0x400.

Several software features have been detailed so far. However, it is considered very
important to briefly describe some benefits obtained from certain libraries freely available.

Decoding ELF files That is to say taking ELF files and converting them to actual
memory locations. For this, a library called pyelftools is used, being a pure-python imple-
mentation for parsing and analyzing ELF files and DWARF debugging information. See
the user’s guide for more details at [1].

C-style parser for command line options To make this possible, the library getopt
is used. This module is a parser for command line options whose API is designed to be
familiar to users of the Unix getopt() function, allowing access functions inside of a python
script, only from console. Further information is available at [5].

Serial communication Were taken the network parameters previously shown in Table
3.5 as reference. For this the pyserial module is used, which encapsulates the access for
the serial port and provides backends for python and other operating systems. Detailed
information can be viewed in [12].

Miscellaneous operating system interfaces Needs of using operating-system-dependent

functionalities appeared, for example, reading and opening files, and manipulating paths.
OS module is used to perform them, whose information is available at [6].

44
3.4.2 Calls to master-code functions
The present is a third alternative to utilize the functionality given by the bootloader
master-code. This consists on creating various GUIs that enable more easily to handle
the processes for setting up groups and accessing the DSC-bootloader. As will be discussed
in chapter 4, these GUIs also facilitate the software to be attached to the Eclipse IDE.
Note that the main code is structured in different functions. When a command exe-
cution is done (external or internal), this is interpreted by a call to function which finally
makes the actual execution of required process. Functions can be appealed using one of
the following types of call:
1. External commands simply by console commands.
2. Internal commands, using the proprietary console interface.
3. Direct calls to functions, when the master-code file is attached as a library to
the new file from where the calls are made.
In Figure 3.11, one can observe a call to programming function which does not use a
GUI, is type 1. On the other hand, the calls to settings, erase and reset, are direct ones
and therefore type 3; each of them has its own GUI. Calls type 3 are those executed by
means of the propietary console.

Figure 3.11: Calls to master-code functions

The GUIs are fully written in Python by using the pyGTK library, which provides
all kind of visual elements and utilities for developing full featured applications for the
GNOME Desktop. More information about features offered by this module, can be found
in [13].
Below are made brief descriptions of those GUIs used to perform direct calls to func-
tions. For all the cited functions, please refer to Table 3.6:

45
Erase Figure 3.12 shows this GUI. User must select the group that owns the device to
be cleared (0 to 4). Each button makes a call to function erase all memory, obviously
with different group argument. There is also a button to close the interface.

Figure 3.12: Erase GUI

Reset It makes a call to function reset with different argument to choose group (see
Figure 3.13). Then it performs a call to function run to allows the user to execute a
program previously loaded on the DSC. The foregoing is abstracted to user, and what
can be seen is just the microcontroller restarting.

Figure 3.13: Reset GUI

Settings This GUI is a collection of different functions as can be seen in Figure 3.14.
Button Search for attached devices calls the master-code function usbscan, to look for
all the FLEX boards attached to computer. ButtonsUSB device Select X are used to
navigate and search which USB will be added to each group; text boxes on left side of
these buttons allow do the same, but manually. Buttons Test X call the test function to
perform light flashes on each board and recognize which of them is the target. Finally,
buttons Select file address X allow to search the full path of the file to be programmed
for each group; if desired, it is also done manually using the boxes on left side.

46
 Figure 3.14: Settings GUI

3.5 Implementing bootloader application

Below the set of procedures taken to deploy the application on a computer with GNU/Linux
is described. Fundamentally, a brief description of the tests performed on the application,
during the debugging is done as well.

3.5.1 Link between Bootloader and Eclipse IDE

Eclipse IDE has an option called External Tools which allows running external programs
through creating buttons with their calls on the interface itself. Using this feature eight
buttons have been created, each of which calls to a file called linker.sh using different
arguments in order to perform interaction with the bootloader’s scripts.
For creating buttons on Eclipse, the following process is performed:

1. Select Run − > External Tools − > External Tools Configurations... to open
the application for creating and managing configurations in order to run external
programs.

2. Select New launch configuration and set the certain name for the new button.

3. Enter into the Location box, the path of the linker.sh file (this will be discussed
later).

4. Write into the Arguments box, those corresponding to the desired call to be made
(see Table 3.7).

47
 Table 3.7: External tools attached to Eclipse IDE
Button
Argument Description
name
Program 0 0 [root password30 ] Call to bootloader script to program device of group 0.
Program 1 1 [root password] Call to bootloader script to program device of group 1.
Program 2 2 [root password] Call to bootloader script to program device of group 2.
Program 3 3 [root password] Call to bootloader script to program device of group 3.
Program 4 4 [root password] Call to bootloader script to program device of group 4.
Erase e [root password] Call to Erase GUI.
Reset r [root password] Call to Reset GUI.
Settings s [root password] Call to Settings GUI.

Finally, the script called linker.sh programmed with Bash , is used as linker file between
Eclipse IDE and Python scripts to access the bootloader structure. Performs the following
specific functions:

• Takes current path of the bootloader scripts in order to dynamically permit that
despite the location of the bootloader folder-project, the link remains available.

• Takes the argument of calls made through the Program 0...4 buttons, and directly
calls the serialBoot.py script. It uses the necessary arguments for this purpose (see
section 3.4.2).

• For all the other buttons, it calls directly to erase.py, reset.py or settings.py scripts,
respectively.

• For all the calls to scripts, it uses the root password to run them as superuser and
guarantee open access to hardware resources such as USB ports.

3.5.2 New GLD-linker-script

A GLD-linker-script is used by the MPLAB C Compiler to compile program logic for
digital signal controllers or other microcontroller units; contains instructions that specify
how the linker merges program code and data from multiple input files into a resulting
output file.
GLD linker scripts are used to control the locations where microcontroller logic and
variables are stored in registers and other memory on the chip.
Based on the foregoing, it is necessary to detail the following:

• Eclipse through RT-DRUID plugin, although is a platform to develop final appli-

cations, is aided by C compilers provided by Microchip (available for 8, 16 and 32
bits) to compile the code written by user.
30
Root password regards the superuser one

48
 • Every C compiler being called either by MPLAB, Eclipse or any other IDE, use
a GLD file that corresponds to the DSC or MCU with which project is working.
This file contains the compiling guidelines related in particular to the distribution
of program memories, as mentioned above.

• It is required to replace the original GLD file, which is used in the compilation of
programs based on dsPIC33FJ256MC71031 , by a new GLD file following the rules
described in section 3.3.1.

Thus, the new file called p33FJ256MC710.gld is placed in the folder ...16-bit compil-
er/support/dsPIC33F/GLD and replaces the original with the same name. Using this
file basically allows the user application can be compiled from address 0xC00 onwards in
the program memory, thus skipping memory located between 0x400 and 0xBFF where is
located the bootloader.

3.5.3 Version of software tools

Versions of the main software tools used to develop this project are detailed in Table 3.8.
The use of same versions is recommended in order to ensure proper operation of whole
system.

Table 3.8: Version of software tools

Tool Version
GNU/Linux OS 3.16.0, 64 bits
Eclipse with RT Druid plugin 2.4.nb
Python 2.7.9
MPLAB X 2.00
MPLAB XC16 C Compiler 1.20

3.6 Performance analysis

Tests already done to bootloader can be described as somewhat subjective. This is said
considering that after debugging32 is checked), it is redundant to perform deeper testing
to obtain data only.

3.6.1 General tests

Refer to tests carried out while debugging was performed. Here the aspects detailed in
subsequent lines have been proven.
31
DSC native on FLEX board
32
In which coherence between what one wants and what actually happens

49
Consistency of program memory It was verified during debugging, using the boot-
loader application to program the DSC memory and then reading it back through ICD3
programmer. The following observations were made:

• Reset vector remains unchanged.

• Interrupt and exception vectors change as required.

• Bootloader location is not modified.

• User application is loaded into the memory section dedicated to this purpose.

Use of hardware resources Has been tested the compilation of various programs
using different hardware resources. Each application was made with both the conventional
method and the new method. Subsequently, through laboratory tests it was found that
using both methods, each application performs the same.
Have been tested resources like timers/counters, communication modules (CAN, UART,
I2C), motor control PWM, ADC, among others.

3.6.2 Programming time

For a specific computer, the programming time is a relative value which depends on the
processor throughput, amount of open processes into the operating system, version of
USB port, number of devices attached to it, among others.
However, tests have carried out by measuring the time taken by both bootloader ap-
plication and MPLAB to send a binary file (project already compiled) towards the com-
puter. This procedure was repeated for different-sized binaries. Measurements obtained
are shown in Table 3.9.
Table 3.9: Test of programming time
Eclipse + MPLAB +
File size
Test Bootloader ICD3
(bytes)
(seconds) (seconds)
1 14056 3,2 10,8
2 10542 2,1 8,6
3 5432 1,1 7,1

3.6.2.1 Analysis of results

While compilation time is almost the same because both methods use same compiler
for this purpose, is noted a marked difference in the time required for loading binaries
on the DSC. Time needed by bootloader is significantly smaller because this method
only programs those memory pages where information exists but not the entire program
memory. This happens even though the access velocity is very slow (115.2 Kbps).

50
Chapter 4

Implementation and Testing on the

Pendulum Control System

In this chapter the bootloader is tested on, by designing a controller for an inverted
pendulum. Both the methodology taken to implement it, as well as the mathematics
used in the design, are depicted. Beside that, tests to check its perfect operation are
explained and analyzed. Moreover, control criteria are used.

4.1 System overview

The proposed application is the control of an inverted pendulum, not in the conventional
manner using only one piece of hardware, but rather using the following configuration
(see Figure 4.1):

• Pendulum There exists only one actuator (DC motor) which through a transmis-
sion strap drives the cart in which the pendulum is located. There are two sensors,
one on the motor shaft and other on the pendulum’s rotating shaft.

• Rod-sensor node Rod angle is sampled through a quadrature-encoder sensor. This

signal is interpreted by the DSC through its internal QEI1 , and sent via CAN to
the cart-sensor node.

• Cart-sensor node Same procedure than the one used by rod-sensor node is taken
there. Both the own state as rod angle are sent via CAN towards controller node.

• Controller node Cart and rod positions are firstly taken from CAN, and then
transformed applying certain conversion factors. Control action is calculated and
transmitted via CAN to the actuator node. It is also responsible for sending to
computer via USB, the current values both the states as control action.
1
Quadrature Encoder Interface module

51
 • Actuator node A control action is received from CAN and transformed into a
DC signal by using PWM2 . Then, this signal is amplified in power through a full
H-bridge and applied to drive the DC motor.

• Host computer A script that allows graphical supervision runs on the computer.
Can be observed the evolution both the control actions as states. It can be updated
online: type of control, controller gains, sampling time, and reference.

It could be thought the use of four boards instead of one is waste of hardware. However
this application serves as a didactic one in which concepts concerning network control
systems are tested.

4.2 Hardware characterization

4.2.1 CAN interface
This widget has been developed at UPC’s laboratory of real-time systems (Figure 4.2).
It is a daughter board containing a double integrator circuit (not used for this implemen-
tation) and a MCP2551 CAN transceiver from Microchip.
MCP2551 a high-speed CAN, fault-tolerant device that serves as interface between
a CAN protocol controller and the physical bus. It provides differential transmit and
receive capability for the CAN protocol controller. It operates at speeds of up to 1Mb/s.
Further information is available in [10].

Figure 4.2: CAN interface daughter board

Each node has a daughter board to convert digital signals managed by ECAN modules
inside dsPICs, into differential voltage suitable for transmission over the bus cabling.
2
Pulse Width Modulation module

52
 Figure 4.1: Pendulum control application

4.2.2 H-bridge
MD03 (see Figure 4.3) is a medium power motor driver, used to supply power in a forward
or reverse way, to the cart motor. Main features are ease of use and flexibility. The motor’s
power is controlled by PWM of the H-Bridge at a frequency of 15Khz. The following are
some other important features:
• A standard 5V supply for the control logic, only 50mA maximum is required.

53
 • The H-Bridge has a rating of 60V allowing motor voltages up to 24Vdc.
• Among different ways, in this project is used the one which consists on a 0V - 5V
analog input with separate direction control, in order to drive the H-bridge.
More relevant information are available in [14].

Figure 4.3: MD03 H-bridge

4.3 Firmware description

It is described below from an operational point of view all the analysis supported by math-
ematical calculations in reference to the treatment of the pendulum as a plant to control.
This is an educational application that is ready, after doing certain calculations regarding
control strategy, to implement user’s controller. However, the following modifications can
be performed:
1. Minor, where only by selecting a control strategy previously established on GUI3
and adjusting feedback gains, pendulum is able to start.
2. Moderate, when one wants to put a control strategy different from those set by
default, has to reprogram a new controller via bootloader, only on controller-node
board.
3. Maximum, when user in addition to changing a control strategy, wishes to change
sampling criteria and/or network topology.
In reference to the above, one can argue that the following description is based on cer-
tain pre-established guidelines regarding control strategies, sampling strategy and network
topology.
3
Graphical User Interface

54
Sampling strategy Sampling period determines changes in system dynamics, and
therefore sampling strategy plays a vital role, so that the former is satisfied. Thus, it
is ensured that processors as data bus are both used efficiently. Here are some stage
directions about what was said:
• Data bus is used as quickly as possible, almost without collisions and with minimum
communication delays.

• RTOSs implemented on controllers have fewer tasks, besides having more organized
schedules without overlaps between tasks.
The sampling strategy, described as follows, is figured out through Figure 4.4:
1. Rod-sensor node sets the pace for sampling. It has a high priority task running each
customizable sampling period, taking a sample of state and sending it to cart-sensor
node through a high-priority CAN message.

2. Cart-sensor node receives the CAN message via interruption, and triggers a high
priority task for sensing its own state. Then it joins the previously received state
with own state to form a new combined message and send it towards controller
node.

3. Controller node receives through interruption, the new CAN message incoming and
executes a highly prioritized task in which, based on states (both rod’s and cart’s
positions), derivative of states, reference, accumulative error, controller gains and
control strategy, calculates a new control value, which is finally sent via CAN to
actuator node.

4. Finally, actuator node by an interruption receives the CAN message and triggers a
high priority task. Here, the numerical value of control action is transformed into a
suitable signal to be applied onto actuator.

5. User through a GUI, from computer is on faculty to stop or restart the process
comprising the steps above.

Figure 4.4: Sampling strategy

55
 Figure 4.5 details the full schedule used to perform the process. It can be noticed
a sequentiality in the execution of tasks, considering delays on both networking and
computation.

Figure 4.5: Pendulum schedule

Real-time operating system into controllers In the following sections, flowcharts

are used to describe the programs running into the four nodes; to understand them, it is
useful to consider the deal with real-time operating systems. Therefore, their multitasking
structure requires drawing processes under paradigm of a scheduling policy. Thus, on each
microcontroller will exist several processes triggered sequentially, remaining latent even
when not running.
Erika Enterprise multitasking real time kernel, the one used here, was described briefly
in chapter 2. Table 4.1 can be used to note general settings for all RTOSs implemented
on DSCs.
Table 4.1: General settings for Erika Enterprise RTOS into DSCs
Parameter Setting Description
Stack Only a single stack exists in the system. No blocking primitives are
Monostack
handling supported, and all the tasks and interrupts execute on the same stack.
There are regular ISRs which can call RTOS primitives (e.g., CAN
Interrupt or UART reception can call ActivateTask command to activate a
Fast ISR4
handling periodic task). These ISRs always have great hardware interrupt
priority.
EDF5 or Whenever a scheduling event occurs (task finishes, new task released,
Kernel type least time to etc.) the queue will be searched for the process closest to its deadline.
go This process is the next to be scheduled for execution.
Interrupt A higher-priority interrupt can be serviced before completion of the
Nested
request current interrupt service routine.
25 This timing reference depends on clock setup. Thus, in this case: Fcy
Tick length
nanoseconds = 40 MIPS, T icklength = 1/Fcy = 25ns.

4
Interrupt Service Routines

56
CAN network Code used to work with CAN network on DSCs has been taken from
APIs belonging to previous work performed in the research group. Below in Table 4.2
some network configurations are summarized.

Table 4.2: CAN network settings

Parameter Setting Description
Transmission delay6 is minimal in relation to the minimum config-
Baudrate 250 Kbps
urable sampling period (10ms).
Acceptance
Yes See Table 4.4 for more information.
filters
Message
Normal only No need to use RTR7 messages.
type
Even though network is small and unique on the cabling, has been
Frame type Extended
considered using an extended frame8 .

A brief description of all the messages managed into the network is made in Table 4.3.
One should consider that payload is synonymous with data length (up to 8 bytes); as is
stablished by CAN, message ID is also used as priority9 .

Table 4.3: CAN messages description

Message ID Flow Payload Comments
[command10 ], Command: changes status either working
Rod-sensor Controller to
0x00 [sampling or standby. Sampling time: used for read-
settings rod-sensor
time11 ] ing sensor.
Cart-sensor Controller to
0x01 [command] Command: ditto the above case.
settings cart-sensor
Actuator Controller to
0x02 [command] Command: ditto the above case.
settings actuator
Cart-sensor Rod-sensor to
0x03 [rod-position12 ] Rod position: current state.
state cart-sensor
Cart-sensor to [rod-position], Rod position and cart position: current
Both states 0x04
actuator [cart-position13 ] states.
Control Controller to [PWM14 ], PWM: current control action. Sign: di-
0x05
action actuator [sign15 ] rection.
Status: current work condition, either
Rod-sensor Rod-sensor to [status16 ],
0x06 working or standby. Sampling time: cur-
status controller [sampling time]
rent one.
Cart-sensor Cart-sensor to
0x07 [status] Status: ditto the above case.
status controller
Actuator Actuator to
0x08 [status] Status: ditto the above case.
status controller

5
Earliest Deadline First
6
Propagation time taken by data on a network
7
Remote Transmit Request
8
With a 29-bit long identifier
9
This could lead to poor real-time performance but in this particular case, network traffic is small

57
 Finally, Table 4.4 shows acceptance filters used by each node. Thanks to them, each
DSC focuses only on listening those messages of its own interest, without wasting time
on interruptions due to invalid incoming messages.

Table 4.4: CAN acceptance filters

Node Filter name Filter ID Buffer Mask
Rod-sensor configToSensorROD 0x00 0x01 0x00
Cart-sensor configToSensorCART 0x01 0x01 0x00
synchroToSensorCART 0x03 0x02 0x00
Controller configFromSensorROD 0x06 0x01 0x00
configFromSensorCART 0x07 0x02 0x00
configFromActuator 0x08 0x03 0x00
bothStates 0x04 0x04 0x00
Actuator changePWM 0x05 0x01 0x00
configToActuator 0x02 0x02 0x00

Setup algorithm Controller node is master player. Whenever this node reboots or
when user through HMI turns on or off the control process, setup algorithm is called.
Basically two types of setting are made:

• Awakening nodes, where controller node sends a configuration message using

cmdStart command to each of the three existing nodes on network. Then it waits
for individual acknowledgment, so as to know whether whole configuration has been
done. Sensor-rod is the main node because besides receiving a start command (see
Table 4.5), receives also the sampling time to be used for timing the entire network.
Both cart-sensor as actuator nodes, receive only a start command.
• Sleeping nodes, where controller node sends a configuration message using cmd-
Stop command to each of the three nodes, waiting then for respective individual
acknowledgment. Then, each of them kills all periodical threads running internally.
Again, rod-sensor node is the main one, as turning off its sampling does not send
its state to cart-sensor node. Then the latter, does not send a two-states-composed
message, and therefore controller node does not send a control action to actuator
node. All this because of a sampling-chain configuration (see section 4.3).

For detailed information, reader is encouraged to carefully observe flowcharts in figures

4.6, 4.7, 4.8, 4.9 and 4.12. Additionally, in Table 4.5 is shown the set of commands sent
via CAN and used by controller node to setup the rest of nodes working into the network.
10
1-byte long unsigned char
11
2-byte long unsigned integer
12
4-byte long floating
13
4-byte long floating
14
2-byte long unsigned integer
15
1-byte long unsigned char
16
1-byte long unsigned char

58
 Table 4.5: Setup commands used by controller node
Command Format Description
Turns node to standby mode (all threads are killed and only inter-
cmdStop 0xF0
ruptions remain enabled)
Turns node to working mode. Node works, and threads for sensing,
cmdStart 0xF1 sending information, control calculation, among others, are enabled
(depends on which node).
Used to ask what is the current configuration of any node. It is
cmdAskSettings 0xF2
implemented but has not been used.
Used to initialize QEI counters of both rod-sensor and cart-sensor
cmdInitSensor 0xF3
nodes, in sensor-calibration processes.

4.3.1 Rod-sensor node

In the flowchart from Figure 4.6 the main program initializes all the hardware modules
and remains on an endless loop, checking for work status and showing it through LED
flashes.
Microcontroller remains without any further process than the main program until re-
ceiving a CAN message from controller node. Then, it is treated via interruption triggering
the thread applying settings once.

Figure 4.6: Flow of rod-sensor node

59
 When applying settings is active, any CAN message is analized in order to know the
incoming command which could be either stop or start (see Table 4.5). A stop command
obviously comes after system was working previously and involves killing all the threads
running so far. A start command begins the work, in which case a recurrent thread named
sensor sampling is triggered, with a sampling period value taken also from the same CAN
message.

Sensor sampling reads the rod’s angular position (available through QEI module), and
sends it via CAN towards cart-sensor node.

Table 4.6: Threads of rod-sensor node

Task pre-
Relative
Thread emption Stack Schedule Execution
deadline
level
Applying Full pre- Single, triggered by interruption
10ms 1 Shared
settings emptive event.
Sensor Full pre- Cyclic, with user-customizable
10ms 2 Shared
sampling emptive period (> 10ms).

4.3.2 Cart-sensor node

Main program and applying configurations thread operate in the same way as in rod-
sensor node, so for a better description, see section 4.3.1. In Figure 4.7 once the node has
received an order to start, it triggers the sensor-sampling thread only once. This task, in
turn, reads the cart-motor’s angular position from QEI but now, unlike the above section,
a message composed by the self state and such state from rod-sensor node is created. It
sends the message via CAN and ends the process.

60
 Figure 4.7: Flow of cart-sensor node

Table 4.7: Threads of cart-sensor node

Task pre-
Relative
Thread emption Stack Schedule Execution
deadline
level
Applying Full pre- Single, triggered by interruption
10ms 1 Shared
settings emptive event
Sensor Full pre- Single, triggered by interruption
10ms 2 Shared
sampling emptive event

4.3.3 Controller node

As shown in Figure 4.8, main program triggers the network configuration thread only
once, which is responsible for sending messages via CAN to all the nodes that have not
yet been set. At once it triggers the configuration checking thread that runs cyclically for
a predetermined number of times.

61
 Figure 4.8: Flow of controller node

Whether at the end of configuration-checking cycles, before killing self process, at least
one node has not been configured, network configuration is triggered once again. The new
turned on process sends messages only to those nodes that have not confirmed the fact of
having been configured.
To answer the question of how to know whether a node has been configured properly,
the following explanation comes. Whenever a remote node receives a configuration mes-
sage via CAN, it sends back an acknowledgment message. At this time, the controller
node receives one single ACK at a time via interruption and sets certain flag representing
each node. Otherwise, flags are clear and represent no configuration.
The interruption subroutine also has to deal with reception of messages from cart-
sensor node, recalling what was said in section 4.3.2 that cart and rod positions are sent
together. After saving the values of states in RAM, control thread is triggered as shown
in Figure 4.9.

62
Figure 4.9: Control thread in controller node

63
 Table 4.8: Description of controller-node’s UART-incoming-messages
Command Format Comments
Start crank Down-position control is triggered via certain flags and
[0xF0], [null value17 ]
control registers.
Start inverted Up-position control is triggered via certain flags and
[0xF1], [null value]
control registers.
Start other First control set by user is triggered via certain flags
[0xF2], [null value]
control and registers.
Start another Second control set by user is triggered via certain flags
[0xF3], [null value]
control and registers.
Runs the state machine shown in Figure 4.11. Carries
Start large the cart to the left stop, waits for the cart and rod stop
[0xF4], [null value]
calibration moving, sends messages to sensor nodes to set them to
zero. Then moves cart to the middle of path.
Start short Sends directly messages towards sensor nodes to set
[0xF5], [null value]
calibration them to zero. Figure 4.10 describes it.
A recurrent square signal is put into control action in
order to move the cart alternatively. Data taken from
Start modeling [0xF6], [null value]
sensors are sent back to computer. There, data are
stored and used for modeling purposes.
Stop control [0xFC], [null value] Stops any process that is running at the time.
Reference Reference is received. This value is used for tracking
[0xFD], [reference18 ]
incoming processes especially in down-position control.
[0x10], [gain19 0], [gain
Crank-control
1], [gain 2], [gain 3], [gain Ditto the above case.
gains incoming
4]
Inverted-control [0x11], [gain 0], [gain 1],
Ditto the above case.
gains incoming [gain 2], [gain 3], [gain 4]
Other-control [0x12], [gain 0], [gain 1],
Ditto the above case.
gains incoming [gain 2], [gain 3], [gain 4]
Another-control [0x13], [gain 0], [gain 1],
Ditto the above case.
gains incoming [gain 2], [gain 3], [gain 4]

Another interruption source is the one caused by receiving a byte string via UART.
For its treatment, there is an algorithm responsible for carrying out different actions
depending on the received message, which can be summarized through Table 4.8. the
treatment of each command, supervision thread can be triggered or killed either the case
to start or end a control task, respectively.
Table 4.9 provides a better understanding of threads managed by this node. The name
Thread is used to identify each one on the flowchart.

17
20-byte long
18
32-bit long floating
19
32-bit long floating

64
 Table 4.9: Threads of controller node
Task pre-
Relative
Thread emption Stack Schedule Execution
deadline
level
Network
Full pre- Single, triggered by interruption
configura- 10ms 1 Shared
emptive event.
tion
Cyclic, with period of 500ms
Configuration Full pre-
10ms 2 Shared and finite iterations, triggered by
checking emptive
Task Remote Config thread.
Full pre- Cyclic, with period of 100ms,
Supervision 10ms 3 Shared
emptive triggered by interruption event.
Full pre- Single, triggered by interruption
Control 10ms 4 Shared
emptive event.

Control thread This process is executed only once when the event of a message coming
from cart-sensor node occurs.
It serves both to apply any control action but also to run calibration subroutines.
Flowchart in Figure 4.9 clearly explains the dynamics. It starts calculating both the rod
and cart position using raw data from sensor nodes and then, the derivative of positions
in order to find velocities20 .
Flags which are modified by interruption according to messages received through
UART, are used to see if the user wants to start a control task or a task for calibra-
tion. Finally, once the control action is calculated, this value becomes an appropriate
PWM value that will be sent towards actuator node.

Supervision thread Its execution is recurrent with fixed period and depends on whether
any control or calibration command is executed. It is enabled or disabled inside the
UART-reception interrupt, and used to send via UART and therefore via USB towards
the computer, the information currently managed by control thread. To do this, the frame
format described in Table 4.10 is used. It can be seen a 33-byte long format, in which the
position of data serves as a marker to identify among fields.

20
These calculations are explained in section 4.5.2

65
 Table 4.10: Description of controller-node’s UART-outgoing-messages
Length
Field Type
(bytes)
Current system time [ticks] 4 Unsigned long
Cart linear position [meters] 4 Floating point
Rod angular position [radians] 4 Floating point
Cart linear velocity [meters/sec] 4 Floating point
Rod angular velocity [radians/seconds] 4 Floating point
Control action [volts] 4 Floating point
Position accumulated-error [meters] 4 Floating point
Calibration state [current state21 ] 1 Unsigned char
End of frame 4 ASCII sequence: ”cXrC”
TOTAL 33

Some clarifications Finally, there are some further clarifications to do:

• All the processes either control or calibration, are executed inside the control thread.
• For safety, if sensors have not been zero-calibrated after a hard-reset, the control
action will be null until the user runs one of the two available calibrations.
• Calibration is a process ordered by user through the GUI on computer, via serial
commands (see Table 4.8).
• There are two types of calibration, short and a large, which run as state machines
and are detailed through Figures 4.10 and 4.11.

Figure 4.10: Short calibration

• Table 4.11 shows the meaning of the states involved in the calibration state-machines.
Thus, the pendulum goes from the state 0 where is uncalibrated until 6, when

66
 the calibration has been fully done. These identifiers are handled both within the
embedded control process and HMI. Within the control process they provide insight
into the current status of the system, while in the HMI are used to associate certain
information messages to each state. The messages are utilized by user to show
through a more easily understandable way, what is happening while calibration
runs.

Table 4.11: States of calibration process

Number
Calibration state
associated
0 Pendulum has not been calibrated.
1 Positioning cart at the track start.
2 Waiting until rod gets stop.
3 Resetting the cart sensor.
4 Resetting the rod sensor.
5 Positioning cart at the track center.
6 Pendulum is already calibrated.

• There is a total of four control actions: down-control, up-control and two more free
to be configured by user. Selecting which type of control to use, is also managed via
serial commands sent by GUI. The control math related to this issue is explained
under the heading of implementation and testing.

Figure 4.11: Large calibration

67
4.3.4 Actuator node
There is nothing new to say that has not been said so far while describing the above
flowcharts, except for thread named as applying new control action. Thus, each time a
message is received via CAN by interruption, it is triggered being responsible to translate
any control action into PWM-based DC voltaje. This value in turn is amplified via a
H-bridge to drive the DC motor that moves the cart. Figure 4.12 shows what is explained
here.

Figure 4.12: Flow of actuator node program

Table 4.12 shows briefly all the threads working inside the node with its corresponding
features taking into account the scheduling.

Table 4.12: Threads of actuator node

Task pre-
Relative
Thread emption Stack Schedule Execution
deadline
level
Applying Full pre- Single, triggered by interruption
10ms 1 Shared
settings emptive event.
Sensor Full pre- Single, triggered by interruption
10ms 2 Shared
sampling emptive event.

68
4.4 Software depiction
This interface uses almost the same libraries than those utilized to develop the bootloader
application described in section 3.4.1. Here, the user is able to initialize, set up and
monitor the whole pendulum control process, through the GUI shown in Figure 4.13.

Figure 4.13: Pendulum GUI

Below different groups of widgets used into the interface are briefly described.

Menu buttons for USB connection Are utilized to find and choose the USB port
at which the control-node’s board is connected. It is suggested make use of button Reset
USB Device the first time the board is plugged because this allows tuning the USB buffer
into the microcontroller.

Gains, sampling-time and reference entries User can manually enter values of both
the five gains as sampling time, individually for each control strategy. Then must click on
button Save Gains, so that the new entered values, being saved into the configGainsCon-
trol.config file. If system is running, gains are also sent towards the board and updated
online, changing the performance of the control system.
The same applies to sampling time: after entering a time, click on button Save Sam-
pling so that the new value being stored into the configSampling.config file, and whether
the system is running, to be updated on line. It is suggested choosing a value greater
than 10ms due to time constraints in embedded hardware and communications.

69
 As there are four control strategies, every time the checkbuttons for process selection
change, different gains and sampling time will be updated for each of them.
Reference slide updates online the desired value for cart position. It is used further in
down-control process.

Checkbuttons for process selection and calibration buttons Are selected off-
line only, before clicking the Start button. There are four control strategies, which are
described exhaustively in the next chapter.
Two additional buttons, used for Short and Long calibration appear at the interface.
Whenever the system starts from scratch (turned off or hard reset), the GUI asks for
calibration through the message box.

Message box This is a banner indicating via messages, the current calibration state of
the pendulum, i.e. along any of the calibration processes, shows the action performed by
the control system.

Execution buttons They are responsible for Start and Stop a control or calibration
process and for Close the pendulum interface.
Also there exists a Clear button in order to bring the interface to its original state.
Effort has been made to handle enabling/disabling of these buttons according to working
conditions of the interface, to avoid mistakenly pressing a key.

Plots Full details of all the states and the control action, can be watched graphically
and in real-time through the six plot charts.
Putting together all six plots and updating them in real time, is a challenge. For it,
pyGTK library has been changed by Tkinter, a Python’s de-facto standard GUI which is
more primitive but allows creating the canvas which contains plots.
It has also been made use of FigureCanvasTkAgg and pyplot functions from Matplotlib
library to update data into the plots. Readers are encouraged to refer to [4] and [7] to
understand the process.

Other widgets In the upper part of the interface is located a menubar which when
deployed, allows make a snapshot of the current state of the plots by using the PdfPages
function from Matplotlib library. There are other less important widgets whose operation
can be observed by handling the actual interface.

4.5 Implementing pendulum application

Regarding the pendulum used to implement the application, a brief description of the
mechanical part is presented. In addition, several simple but important calculations to

70
measure and adjust the states are briefly described. Moreover, two control problems are
analyzed separately: down-position and up-position.

4.5.1 Description of the target pendulum

The pendulum at Figure 4.14 was built by Inteco22 almost twenty years ago. The rod
is mounted on a cart swinging freely in one plane. The cart moves on a straight rail,
propelled by a DC-motor by means of a transmission belt. Twin pendulums are attached
to the cart according to Figure 4.15 for balancing purposes.

Figure 4.14: Real pendulum

For sensing purposes there exist two optical incremental sensors; one for the pendulum
angle, and one for the cart position. Using these signals any computer system might
calculate and generate control actions to the DC motor so that desired control is achieved.

22
Polish company that provides training systems (www.inteco.com.pl)

71
 Figure 4.15: Mechanical part and sensors of pendulum-cart setup

Figure 4.16 shows the four boards interconnected via CAN, as well as the power con-
nection and some interfaces. As FLEX boards are elements designed for rapid prototyping,
physical implementation of the system does not require much time.

Figure 4.16: Implementation of the actual-pendulum’s control system

72
 Generally the pendulum control problem is to bring the pole to one of the equilibrium
positions: stable (down) or unstable (up). Preferably to do so as fast as possible, with
few oscillations, and without letting the angle and velocity become too large.
It is lucky enough to have all the necessary information about the pendulum in ques-
tion, thanks to [16] and [11]. Thus there are the mathematical model and the linearized
state space descriptions of both up-position as down-position. Table 4.13 lists the actual
parameters of this pendulum.

Table 4.13: Parameters of the pendulum

Name Description Value
l Distance from pendulum center of gravity to pivot point 0.325 [m]
m Mass of pendulum 0.120 [kg]
M Cart mass 0.572 [kg]
Ip Moment of inertia for pendulum with respect to pivot point 0.0116 [kg/rad]
fp Viscous friction for pendulum at pivot point 0.01 [Ns/rad]
fc Dynamic friction for cart 1.1 [Ns/m]
g Gravity 9.8 [m/s2 ]

4.5.2 State calculations

The states of the plant and their nomenclature are shown in Table 4.14. The states x
and θ are taken directly from sensors (measured), but ẋ and θ̇ are calculated through the
derivative (referring to time) of the measured states.

Table 4.14: States description

State Description
x Kart position [m]
ẋ Kart velocity [m/s]
θ Rod angular position [rad]
θ̇ Rod angular velocity [rad/s]

To convert values from counts per revolution (CPR) to position of cart on the rail
(meters), the left end of the rail is taken as zero-reference for sensor. By contrast, the
zero-reference of the kart into the system is in the middle of the rail, as seen in Figure
4.17. Therefore, it must be subtracted an offset equal to the rail length divided by 2
(halfway) as can be seen in model (4.1). Notice that each time the encoder completes
one full revolution, the QEI continues incrementing/decrementing the counter, without
resetting.

73
 2π lrail
x = QEIvalue φpulley − (4.1)
sres · n 2
2π
x = QEIvalue 0.0375 − 0.55 [meters] (4.2)
1024 · 4
where,
x : cart position
QEIvalue : raw measurement (ticks number) taken from sensor through the
quadrature encoder interface (QEI)
φpulley : diameter of the traction sheave
sres : resolution of the optical-encoding sensor
lrail : rail length
n : 4, because the QEI counter increases by 4 per each 1 tick counted
To calculate the angular displacement θ the type of control (up or down) is considered,
as the zero-reference is different in each control process. See Figures 4.17 and 4.19 to
clarify the above, where the zero-reference for down-control considers the rod vertically
downward, and for up-control the vertical rod upward. However the zero-reference for the
sensor relies on the positive horizontal axis and because of this an offset must be used as
shown in model (4.3), whose value depends on the type of control. In the model (4.5) the
offset is 0.5π radians (90 degrees) while in (4.5) the offset is 1.5π radians (270 degrees). Is
worth noting that the setting of this sensor is made such that, for each tapped revolution
the counter is reset, unlike the cart-position sensor.

2π
θ = of f set − QEIvalue (4.3)
sres · n
2π
θdown = 1.5π − QEIvalue [radians] (4.4)
1024 · 4
2π
θup = 0.5π − QEIvalue [radians] (4.5)
1024 · 4
where,
θdown : rod’s angular position for down-control
θup : rod’s angular position for up-control
of f set : either 1.5π or 0.5π depending on control type
QEIvalue : raw measurement (ticks number) taken from sensor
sres : resolution of the optical-encoding sensor
n : 4, because the QEI counter increases by 4 per each 1 tick counted
For calculating both xdot as θdot the backguard difference method has been used. Models
in (4.6) and (4.7) illustrate what was explained.

74
  
x − xold rads
ẋ = (4.6)
h s
where,
ẋ : cart velocity
x : current cart position
xo ld : cart position one sampling-period ago
h : sampling period

 
θ − θold rads
θ̇ = (4.7)
h s
where,
θ̇ : rod velocity
θ : current rod position
θold : rod position one sampling-period ago
h : sampling period

Finally, a very important value, the accumulated error eacc is calculated using the
formula in (4.8). Keep in mind that whenever a control process starts, this value is set to
zero.

eacc = (xref − x) + eaccold [meters] (4.8)

where,
eacc : accumulated error
xref : reference
x : current state
eaccold : accumulated error one sampling-period ago

4.5.3 Down-position control

The objective of this control is getting the cart to slide towards a position set by the
user, keeping the pendulum down and absorbing oscillations. Furthermore, once located
at the required point on the horizontal rail, keeping the cart position and rod down
position under disturbance. The sign convention for both linear displacement x and
angular displacement θ is shown in Figure 4.17. Reference zeros are in the middle of the
track and with the pendulum completely down, respectively.

75
 Figure 4.17: Illustration of down-position control

4.5.3.1 Model
The continuous plant model is described by the matrices (4.9).

0 1 0 0
   
0
 0 − (Ip +ml2 )fc 2 2
− m αl g − mlf p   Ip +ml2 
Adown =
 α α  Bdown = α  (4.9)
0 0 0 1   0 
mlfc (M +m)mgl (M +m)fp ml
0 − α − α
− α α

where,
α = Ip (M + m) + ml2 M

Substituting the parameters from Table 4.13 into matrices (4.9) yields matrices in
(4.10).

   
0 1.0 0 0 0
 0 −1.748 −0.9757 −0.02553   1.589 
Adown =
 0
 Bdown =  (4.10)
0 0 1.0   0 
0 −2.808 −17.31 −0.453 2.553

This system is discretized with a sampling time of 10 milliseconds, leading to (4.11).

   
1.0 0.009913 −4.842e − 5 −1.429e − 6 7.898e − 5
 0 0.9827 −0.009647 −0.0003008   0.01575 
Φdown =
 0 −0.0001394
 Γdown = 
0.9991 0.009975   0.0001267 
0 −0.02777 −0.1725 0.9946 0.02524
(4.11)

76
 Finally, the state space description in down position is shown in (4.12).
(
Ẋ = Adown X + Bdown U,
(4.12)
Y = CX + DU

where,
T
X = [ x ẋ θ θ̇ ]
 
1 0 0 0
 0 1 0 0 
C = 
 0

0 1 0 
0 0 0 1
D = [ 0 ]
U = F → Force

The eigenvalues of the open-loop discrete system are:

λ1 = 1.0
λ2 = 0.984
λ3 = 0.9962 + 0.0411i
λ4 = 0, 9962 − 0.0411i

Although the system is stable, the poles are close to the boundary of unit circle. There
is an asymptotic oscillation on the cart position.

4.5.3.2 Controller design

For stabilizing pendulum in down-position, the control system to be designed includes
state feedback and an integrator in the closed loop, shown in Equation 4.13. The design
variables form a matrix Kdown comprising a gain constant ki and feedback gains k0 to k3 .
Xext is the extended state vector where xref is the desired position along the track, udown
is the output (control action). This is an intuitive and powerful control strategy which is
well stablished by now.
 
x


 ẋ 

udown = −Kdown Xext = − k0 k1 k2 k3 −ki   θ 
 (4.13)
 θ̇ 
xref − x
In this system the main purposes are:

77
 • Approach the current cart position x to the position xref set by user, trying to keep
a small error between them.

• Keep the rod angle θ as small as possible when cart is moved in the step fashion.

Firstly, it is understood that has been previously determined whether the system is
controllable. Then, using quadratic optimal control, there exist the following contempla-
tions:

• The displacement of the cart is considered as the output of the system, then the
output matrix becomes,

 
Cnew = 1 0 0 0

• Matrices Φdown and Γdown must be reformulated to get an extended version which
is compatible with the five-states control strategy, i.e. with a new states regarding
the error between x and xref ,

 
Φdown 0
Φdownext =
−Cnew Φdown 1
 
1.0 0.009913 −4.842e − 5 −1.429e − 6 0
 0
 0.9827 −0.009647 −0.0003008 0 

Φdownext = 
 0 −0.0001394 0.9991 0.009975 0 

 0 −0.02777 −0.1725 0.9946 0 
−1.0 −0.009913 4.842e − 5 1.429e − 6 1.0
 
Γdown
Γdownext =
−Cnew Γdown
 
7.898e − 5
 0.01575 
 
Γdownext = 
 0.0001267 

 0.02524 
−7.898e − 5

• Finally, the matrices Qdown and Rdown are chosen properly so that the response of
the system is acceptable. Qdown accounts for the importance of error during the
control process (here has been placed greater effort on x and θ), Rdown accounts for
the expenditure of the energy of the control signals,

78
  
100 0 0 0 0
 0 30 0 0 0 
 
Qdown =  0
 0 100 0 0 

 0 0 0 30 0 
0 0 0 0 1
 
Rdown = 1

Once calculated matrices Φdownext , Γdownext , Qdown and Rdown , they are used to calcu-
late the gain matrix Kdown which is applied to the actual system. This process is done
using the Steady-State Ricatti Equation and the simplest way to perform that is using the
dlqr command from Matlab. Then, the new gain matrix is shown in (4.14).
 
Kdown = 56.67 16.65 13.59 −2.505 0.8967 (4.14)
In Figure 4.18 the step-response of the system is shown. The simulation involves
changing periodically xref and observing the evolution of states. Position of the sliding
cart x follows the reference with a slight overshoot, but converges with a steady state
error of almost zero. The angular displacement θ is corrected and converges to zero after
each impulse of motion. Both the cart velocity ẋ and the angular velocity of the rod θ̇
converge to zero after the disturbance appears.

Position of the cart (x) Angular displacement (theta)

0.2 0.2

0.1 0.1
x1(k)

x2(k)

0 0

−0.1 −0.1

−0.2 −0.2
0 10 20 30 40 50 60 0 10 20 30 40 50 60
k k
Velocity of cart (xdot) Angular velocity (thetadot)
0.1 0.4

0.05 0.2
x3(k)

x4(k)

0 0

−0.05 −0.2

−0.1 −0.4
0 10 20 30 40 50 60 0 10 20 30 40 50 60
k k

Output of integrator
10

5
x5(k)

−5

−10
0 10 20 30 40 50 60
k

Figure 4.18: Step-response simulation of down-position control

79
 In addition, the eigenvalues of the closed loop system (applying the control strategy)
show that there is stability because the poles are inside the unit circle. Notice that there
is an additional state and therefore one eigenvalue more:

λ1 = 0.8551 + 0.0000i
λ2 = 0.9676 + 0.0219i
λ3 = 0.9676 − 0.0219i
λ4 = 0.9905 + 0.0302i
λ5 = 0.9905 − 0.0302i

In summary, the controller works perfectly in simulation. For practical application

slight changes in the gains to compensate for uncertainty, can be performed heuristically.

4.5.4 Up-position control

The objective of this control is stabilizing the pendulum in the up-position, being able
to deal with load disturbances and impulses without falling down. It is only necessary
that all states approach zero instead of tracking a reference. Figure 4.19 shows the sign
convention for both linear displacement x and angular displacement θ. Reference zeros
are in the middle of the track and with the pendulum completely upright, respectively.

Figure 4.19: Illustration of up-position control

80
4.5.4.1 Model
The continuous plant model is described by the matrices in (4.15).

0 1 0 0
   
0
 0 − (Ip +ml2 )fc 2 2g
− m αl mlfp   Ip +ml2 
Aup = α α  Bup = α  (4.15)
 0 0 0 1   0 
0 mlfc
α
(M +m)mgl
α
− (M +m)f
α
p
− ml
α

where,
α = Ip (M + m) + ml2 M

Substituting the parameters from Table 4.13 into matrices (4.15) leads to matrices in
(4.16).
   
0 1.0 0 0 0
 0 −1.748 −0.9757 0.02553   1.589 
Aup =  0
 B up =   (4.16)
0 0 1.0   0 
0 2.808 17.31 −0.453 −2.553
Discretizing the system from (4.16) for a sampling period of 10 milliseconds, yields(4.17).

   
1.0 0.009913 −4.843e − 5 1.106e − 6 7.898e − 5
 0 0.9827 −0.009653 0.0002041   0.01575 
Φup =
 0 0.0001394
 Γup =
 0.0001267  (4.17)

1.001 0.00998 
0 0.02778 0.1726 0.9963 0.02526

Finally, the state space description in upright position is described in (4.18).

(
Ẋ = Aup X + Bup U,
(4.18)
Y = CX + DU

where,
T
X = [ x ẋ θ θ̇ ]
 
1 0 0 0
 0 1 0 0 
C = 
 0

0 1 0 
0 0 0 1
D = [ 0 ]
U = F → Force

81
 The eigenvalues of the open-loop discrete system are:

λ1 = 1.0
λ2 = 0.9845
λ3 = 0.9558
λ4 = 1.0396

This system is unstable just as expected, because the open-loop eigenvalues are outside
the unit circle.

4.5.4.2 Controller design

To solve this problem, it has been decided using a slightly more sophisticated controller,
considering the time delay between the moment when the measurement of the states
is made and the instant of application of the control signal onto the actuator. Thus,
the control law uup given in (4.19) regards an extended state vector Xext where the
control action previously applied in the last iteration uup old , has been added. Therefore
additionally to feedback gains k0 to k3 , an extended gain k4 must be used into Kup .

 
x


 ẋ 

uup = −Kup Xext = − k0 k1 k2 k3 k4 
 θ 
 (4.19)
 θ̇ 
uup old

To apply the control law described in (4.19) must be considered discretizing the same
continuous system for both a sampling time of h − τ as for a sampling of τ . So the
following is taken into account:

• Matrices Φup and Γup from (4.17) must be reformulated to get their extended version
in which delays are also modeled. Then, a sampling time h of 10 milliseconds and a
time delay τ of 1.425 milliseconds are considered. The later has been obtained from
measurements and regards both processing as networking latencies.

82
  
Φup (h − τ )Φup (τ ) Φup (h − τ )Γup (τ )
Φup ext =
0 0
 
1.0 0.009913 −4.843e − 5 1.106e − 6 2.086e − 5
 0
 0.9827 −0.009653 0.0002041 0.002227 

Φup ext = 
 0 0.0001394 1.001 0.00998 −3.345e −5 

 0 0.02778 0.1726 0.9963 −0.003567 
0 0 0 0 0
 
Γup (h − τ )
Γup ext =
1
 
5.812e − 5
 0.01352 
 
Γup ext = 
 −9.328e − 5 

 −0.02169 
1.0

• For controller design, poles placement is used, taking into account choosing the extra
pole in discrete time domain to be 0. Poles chosen as |z| < 0.9 gave very aggres-
sive behaviour with saturated control signals and, although stable, not a pleasant
show. When the theoretical poles were moved closer to the unit circle the pendulum
behaved much better. The poles chosen are the following,

z0 = 0.9137 − 0.0183i
z1 = 0.9822 − 0.0044i
z2 = 0.9822 + 0.0044i
z3 = 0.0000 + 0.0000i

The control design via pole placement has been made using the well known Ackermans’
formula. This procedure is easily implemented through command acker from Matlab.
The new gain matrix Kup shown in (4.20) will be applied to the actual system with minor
adjustments to suit uncertainty.
 
Kup = −10.62 −14.83 −72.52 −16.43 0.02608 (4.20)
The simulation of closed-loop response is shown in Figure 4.20 where the four states
are initialized with values different to zero, waiting for they return to equilibrium (zero
for all). As can be shown the system response is satisfactory despite a slight overshoot on
the position state x. The eigenvalues of the closed-loop system show that there is stability
because the poles are inside the unit circle:

83
 λ0 = 0.0000 + 0.0000i
λ1 = 0.9137 + 0.0183i
λ2 = 0.9137 − 0.0183i
λ3 = 0.9822 + 0.0044i
λ4 = 0.9822 − 0.0044i

Position of the cart (x) Angular displacement (theta)

1.8 0.3

1.6
0.2
1.4

1.2 0.1

1
0
x1(k)

x2(k)

0.8
−0.1
0.6

0.4 −0.2
0.2
−0.3
0

−0.2 −0.4
0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500
k k

Velocity of cart (xdot) Angular velocity (thetadot)

3 1

2.5 0.5

2 0

1.5 −0.5

1 −1
x3(k)

x4(k)

0.5 −1.5

0 −2

−0.5 −2.5

−1 −3

−1.5 −3.5
0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500
k k

Figure 4.20: Simulation of up-position control

84
4.6 Testing pendulum application

Figure 4.21: Real pendulum at work

4.6.1 Input-output latency

This has been practically the first test applied as through its results, has been obtained
critical information to determine the delay τ noted in section 4.5.4.2. The test consists
on programming two digital outputs, one into the rod-sensor node and the other into
the actuator node. In the former, the output stays high as long as the sensing task is
performed and in the latter, during the execution of actuation task.
Both signals at once are observed through an oscilloscope; the time between first rising
edge and last falling edge is measured in order to obtain the total latency τ comprising
processing and transmission times. Furthermore, measuring between two rising edges of
the signal from rod-sensor node leads to determine the sampling period.
As shown in Figure 4.22 a very stable sampling period of 10 milliseconds met ex-
actly. The most important thing is that the delay has a value of 1.45 milliseconds, much
lower than the sampling. This indicates that scheduling, networking and processing are
performing perfectly.

85
 Figure 4.22: Measurement of input-output latency

The findings are concerning the type of control that is used because the computational
workload would vary depending on how large and complex is the program code. However,
this measurement is used to get a clear picture that the total system latency is less than
the sampling time. With this certainty can be considered even reducing the sampling
period for applications that require it.

4.6.2 Networking
This test uses a CAN analyzer to verify if the network is working under standards estab-
lished in the design. That is, allows to determine the actual baud rate as well as the CAN
frames with their respective identifiers and payloads
Although a total of nine different CAN frames is handled in the entire system, this
test was carried out with the three frames that most of time are present, which are:

• Frame sent from the rod-sensor node towards the cart-sensor node containing rod-
state information (Figure 4.23). The ID is 0x03, 3 bytes are sent whose values are
0x0C, 0x09 and 0x00. Notice that the values are not fixed.

• Frame sent from the cart-sensor node towards the controller node with information
about rod and cart states (Figure 4.24). The ID es 0x04, 6 bytes have been sent,
whose values are 0x0C, 0x09, 0x00 0x32, 0xA0 and 0x01. Note again that payload
not fixed.

• Frame sent from the controller node towards the actuator node with information
regarding the control action (Figure 4.25). The ID is 0x05, 3 bytes whose values
are 0x00, 0x00 and 0x00 have been sent. Again, data is not a fixed value.

86
Figure 4.23: CAN frame sent from rod-sensor node towards cart-sensor node

Figure 4.24: CAN frame sent from cart-sensor node towards controller node

87
 Figure 4.25: CAN frame sent from controller node towards actuator-node

In summary, with this test can be corroborated that the baud rate in the network
is actually 250 Kbps as well as the lengths of the payloads are those required when
programming the application. It can be additionally verified through Figure 4.26, that the
algorithm used for sampling and triggering works perfectly and without mishap, because
there exists sequencing of the CAN frames.

Figure 4.26: Sequence of CAN frames managed in the sampling-processing-actuating

process

88
4.6.3 Testing down-position control

This control system can be subjected to the step test because it is designed to follow a
position reference on the rail. In Figure 4.27 the system response is shown, where the
state x describes a path with a slight overshoot, as predicted in the simulation. One can
see that the steady-state error is minimal, but the accumulated error is still present and
stable. Also, while the cart moves the rod position is corrected, allowing the pendulum to
swing but only with a very small amplitude. Velocities of both the cart and the pendulum
converge to zero, showing signs of stability.

1.0 1.0

CART VELOCITY [m/ ]

CART POSITION [m]

0.5 0.5

0.0 0.0

−0.5 −0.5

−1.0 −1.0
100 150 200 250 100 150 200 250
ACCUMULATED ERROR [m] ROD VELOCITY [rad/ ] 6
3
ROD POSITION [rad]

2 4
1 2
0 0
−1 −2
−2 −4
−3
−6
100 150 200 250 100 150 200 250
CONTROL ACTION [V]

10 40
5 20
0 0
−5 −20
−10 −40
100 150 200 250 100 150 200 250

Figure 4.27: Step-response of the down-position control

The next test consists in exercising a disturbance on the pendulum, such as pushing
the cart and/or the rod. It can be seen in Figure 4.28 the states and control signal with
two impulses in either direction on the pendulum angle at samples 80 and 140. Thus, the
cart makes a sudden movement to combat external disturbance, and then returns to its
initial position by controlling the pendulum down with soft dynamics. Again, it is stable.
To conclude about the first test, the accumulated error is stable but not disappear,
because there is minimal steady-state error. As an improvement it could be used an
weighted accumulated error, to test if it can reduce this value.
Regarding the second test, in open loop the system is poorly damped but using the
feedback gain the poles of the closed loop system give much better performance resulting
in a faster damping.
Finally the control signal works within its limits, unsaturated, indicating that it is
well dimensioned.

89
 1.0 1.0

CART VELOCITY [m/ ]

CART POSITION [m]

0.5 0.5

0.0 0.0

−0.5 −0.5

−1.0 −1.0
50 100 150 200 50 100 150 200
6

ACCUMULATED ERROR [m] ROD VELOCITY [rad/ ]

3
ROD POSITION [rad]

2 4
1 2
0 0
−1 −2
−2 −4
−3
−6
50 100 150 200 50 100 150 200
CONTROL ACTION [V]

10 40
5 20
0 0
−5 −20
−10 −40
50 100 150 200 50 100 150 200

Figure 4.28: Test under perturbation of down-position control

4.6.4 Testing up-position control

Figure 4.29 shows the behaviour of the pendulum under real work conditions. There exists
a low-frequency oscillation in the cart position but the pendulum relies up-straight and
stable indefinitely. Tuning heuristically the gains while the pendulum is working, yields
a better and more smooth behaviour.

1.0 1.0
CART VELOCITY [m/s]
CART POSITION [m]

0.5 0.5

0.0 0.0

−0.5 −0.5

−1.0 −1.0
700 750 800 850 900 700 750 800 850 900
6
ACCUMULATED ERROR [m] ROD VELOCITY [ra /s]

3
ROD POSITION [ra ]

2 4
1 2
0 0
−1 −2
−2 −4
−3
−6
700 750 800 850 900 700 750 800 850 900
CONTROL ACTION [V]

10 40
5 20
0 0
−5 −20
−10 −40
700 750 800 850 900 700 750 800 850 900

Figure 4.29: Step-response of down-position control

In Figure 4.30 it can be seen how the control system responds when a disturbance is
applied; so while the pendulum is keeping its up position, the rod is pushed. Then, one

90
can see that the pendulum immediately responds and compensates the disturbance with
a sudden movement of the cart returning to remain stable.

1.0 1.0

CART VELOCITY [m/ ]

CART POSITION [m]

0.5 0.5

0.0 0.0

−0.5 −0.5

−1.0 −1.0
150 200 250 300 150 200 250 300
6

ACCUMULATED ERROR [m] ROD VELOCITY [rad/ ]

3
ROD POSITION [rad]

2 4
1 2
0 0
−1 −2
−2 −4
−3
−6
150 200 250 300 150 200 250 300
CONTROL ACTION [V]

10 40
5 20
0 0
−5 −20
−10 −40
150 200 250 300 150 200 250 300

Figure 4.30: Test under perturbation of up-position control

Regarding the oscillation, the stable limit cycle is originated apparently by unmodeled
dynamics that includes, among others, the following issues:

• The transmission belt has a certain elastic component, besides giving lashes every
time the DC motor changes direction or accelerates suddenly, absorbing certain
amount of movement.

• The aluminium rod is not perfectly stiff.

• The model is a linearized one.

91
92
Chapter 5

Conclusions and further work

This chapter shows the conclusions of this development and also outlines some possible
lines of future work. Also, explains some limitations of the system.

5.1 Conclusions
It has been created a project based on open and free software and firmware that al-
lows, through a simple and intuitive process and without additional hardware, loading
directly a final application on the FLEX boards. As open code it is accessible to suit user
requirements, and to modify so that it can be used in more sophisticated developments.
In addition, it was performed the control of an inverted pendulum on a real-time dis-
tributed system, using the bootloader application itself as a development tool. The control
system has its own interface to adjust online important parameters such as sampling time
and feedback gains.
In addition to reaching the main objective, the secondary objectives have been mate-
rialized, resulting in the following:

• Python scripts were developed to work under a GNU/Linux operating system, which
use its own as well as third party libraries to decode ELF-files, handle USB, and
generate a HMI.

• The bootloader programmed on the FLEX boards is compatible with applications

developed using ERIKA Enterprise RTOS; both the bootloader and the application
are placed together inside the microcontroller without overwriting, sharing resources
orderly and one at a time; the handling of internal memory and UART is performed
through atomic libraries.

• The USB microcontroller works as an USB/UART bridge transparently, however,

also it analyzes the incoming information to recognize commands that allow it to
perform additional tasks such as restarting the DSC by hardware.

93
 • The pendulum control system, which works with two algorithms, one for down-
position and another for up-position, is robust and reliable from the point of view of
control; the scheduling has not been overlapped, and the CAN network is managed
without collisions or loss of data.

5.2 Limitations
While it has been implemented an atomic function to change the hardware configuration
bits (fuses) from the DSC, it is not possible to change the clock frequency. So far it
has not been considered useful this aspect because laboratory practices for which it was
created, work with a fixed throughput of 40 MIPS.
The transmission rate of UART/USB is fixed to 115200 bps, and from being unable
to perform the clock settings via the bootloader, the speed can not be changed. This is
because a change in the clock leads to a change in the timing used by UART module,
thereby causing a change in baudrate.
It has been utilized a modified GLD-linker script to replace the original used by
MPLAB C Compiler in the process of compilation of the applications made via Eclipse
IDE (see section 3.5.2). This is not versatile when installing all the software tools from
the scratch; one could implement a call to a local GLD fiel directly from the user project.
It was attempted to use this method at the beginning but without success due to the lack
of information on how the RT-Druid Eclipse IDE works.
It has not been implemented a debugging function, such as the one which MPLAB
in conjunction with ICD3 programmer can do so. Developing this would take much time
and effort, however anyone can do it because there all the information of this work are
available.

5.3 Further work

The bootloader application while has already been used in several subjects as a develop-
ment tool, continues in its version 1. There is much to do, so information about possible
errors or suggestions for improvement are welcome.
The bootloader works under GNU/Linux platform as a native operating system. But
when some people have used it through virtual machine, some random errors were re-
ported. Bugs are related especially to the establishment of the USB communication
between the computer and the Flex boards. These failures will be analyzed.
In the near future, the system will be modified so it can work on Windows OS to give
the user more options to choose from. This requires a relatively extensive work.
Python, while allows programming applications with amazing speed and ease, it has
the disadvantage of using a relatively high amount of ram and lose fluidity. Another
disadvantage is that one, can not have direct contact with the USB hardware, which
makes it impossible to perform a more personalized USB communication. It is proposed

94
to make the same applications made so far, but using a lower level language, a compiled
language such as C.
It is clear that is possible to obtain very good results with a much simpler model of
the pendulum. This would have resulted in easier computations of the different transfer
functions. In addition, so far have been implemented two control problems (pendulum
up/down), but it would be nice to solve the problems of swing up and swing down based
on energy criteria, testing them on the pendulum.

95
96
Bibliography

[1] Eli Bendersky. Pyelftools. https://github.com/eliben/pyelftools, 2015.

Pyelftools is a pure-python library for parsing ELF and DWARF.

[2] Evidence. Erika. http://erika.tuxfamily.org/. ERIKA is an open source imple-

mentation of the API OSEK / VDX.

[3] The Eclipse Foundation. Eclipse. https://eclipse.org/. ECLIPSE is an open

source programming platform to develop RCP.

[4] The Python Software Foundation. Tkinter wiki. http://tkinter.unpythonic.net/

wiki/, 2014. Tkinter is Python’s de-facto standard GUI (Graphical User Interface)
package.

[5] The Python Software Foundation. Getopt. https://docs.python.org/2/library/

getopt.html, 2015. Getopt module is a parser for command line options like C-
language getopt() function.

[6] The Python Software Foundation. Os. https://docs.python.org/2/library/os.

html, 2015. Os is a python module that provides a portable way of using operating
system dependent functionality.

[7] John Hunter. Matplotlib. http://matplotlib.org/, 2015. Matplotlib is a Python

2D plotting library which produces publication quality figures in a variety of hardcopy
formats and interactive environments across platforms.

[8] Microchip Technology Incorporated. Pic18f2455/2550/4455/4550 data sheet:

High-performance, enhanced flash, usb microcontrollers with nanowatt technol-
ogy. http://ww1.microchip.com/downloads/en/devicedoc/39632c.pdf, 2006.
PIC18F2455/2550/4455/4550 is a family of USB controllers developed by Microchip.

[9] Microchip Technology Incorporated. dspic33fjxxxmcx06/x08/x10 data sheet:

High-performance, 16-bit digital signal controllers. http://ww1.microchip.com/
downloads/en/DeviceDoc/70287C.pdf, 2009. DSPIC33F is a family of digital sig-
nal controllers developed by Microchip.

97
[10] Microchip Technology Incorporated. Mcp2551: High-speed can transceiver. http://
ww1.microchip.com/downloads/en/DeviceDoc/21667f.pdf, 2010. MCP2551 is a
high-speed CAN device that serves as the interface between a CAN protocol controller
and the physical bus.

[11] INTECO. Pendulum-cart system: User’s manual. http://http://www.inteco.

com.pl/Docs/Pend_um.pdf, 2014. The Pendulum & Cart System consists of a pole
mounted on a cart in such a way that the pole can swing freely only in the vertical
plane; the cart is driven by a DC motor.

[12] Chris Liechti. Pyserial 2.7 documentation. http://pyserial.sourceforge.net/

index.html, 2013. This module encapsulates the access for the serial port, provid-
ing backends for Python running on Windows, Linux, BSD (possibly any POSIX
compliant system).

[13] The GNOME Project and PyGTK Team. Pygtk: Gtk+ for python. http://www.
pygtk.org/, 2011. PyGTK is a tool for easily creating programs with a graphical
user interface using the Python programming language.

[14] Robot-Electronics. Md03: 24volt 20amp h bridge motor drive datasheet. http://
www.robot-electronics.co.uk/htm/md03tech.htm, 2012. The MD03 is a medium
power motor driver, designed to supply power beyond that of any of the low power
single chip H-Bridges that exist.

[15] Evidence S.r.l. Flex: Modular solution for embedded applications.

http://download.tuxfamily.org/erika/webdownload/manuals_pdf/flex_
refman_1_0_2.pdf, 2012. FLEX board is a development tool for embedded
applications based on dsPIC33Fj256MC710 DSC.

[16] Mathias Svensson. Hard real-time control of an inverted pendulum using rtlinux/free.
Technical report, Departments of Automatic Control, Technical University of Cat-
alonia and Lund Institute of Technology, 2014.

98
Appendix A

Code Listing

All code used in this development is located in the GiHub server at github.com/xavierrosero/
FlexboardBootloader.
This appendix enlists software and firmware from both the bootloader and the pen-
dulum control application. Furthermore, matlab files used in the design of controllers are
appended.
Only have been annexed the most important files. For full code, refer to the repository
or attached media.

A.1 Bootloader software

A.1.1 Master code (serialBoot.py)

Listing A.1: Bootloader file

#! / u s r / b i n / env python
# −∗− c o d i n g : u t f −8 −∗−
"""
Created on Tue May 13 13:05:56 2014

@authors : carlos xavier rosero

manel velasco garcia

best performace tested with python 2.7.3 , GCC 2.6.3 , pyserial 2.5
"""

im por t s y s

s y s . path [ 0 : 0 ] = [ ’. ’ , ’ .. ’ ]

from e l f t o o l s . e l f . e l f f i l e im po rt ELFFile

from p r o g r e s s b a r imp ort P e r c e n t a g e , P r o g r e s s B a r , Timer

im por t b i n a s c i i
im por t time

99
im po rt s e r i a l
im po rt o s
im po rt g e t o p t

#o s . c h d i r ( " / home / carlos / B o o t l o a d e r I n t e r f a c e 1 / " )

#u s e t h i s p a r t o n l y with o l d v e r s i o n s o f python and l i b r a r i e s

#t e s t e d s p e c i f i c a l l y with python 2 . 6 . 5 , GCC 4 . 4 . 3 , s e r i a l 1 . 3 . 5
c l a s s adapt ( s e r i a l . S e r i a l ) :
d e f w r i t e ( s e l f , data ) :
super ( s e l f . class , s e l f ) . w r i t e ( s t r ( data ) )

#im po rt numpy a s np
#from f u t u r e i mp ort d i v i s i o n #f o r i n t e g e r d i v i s i o n c a l c u l a t i o n s
#ON=b y t e a r r a y ( [ 0 x9FH , 0 x80H , 0 x60H , 0 x4EH , 0 x00H , 0xCDH ] )

#COMMANDS
#g l o b a l c o n s t a n t s and d e f i n i t i o n s
c l a s s command :
nack = 0 x00
ack = 0 x01
readPM = 0 x02 #r e a d program memory
writePM = 0 x03
readEM = 0 x04 #r e a d eeprom memory
writeEM = 0 x05
readCM = 0 x06
writeCM = 0 x07
runProg = 0 x08
readID = 0 x09 #r e a d d e v i c e ID
erasePM = 0 x0a
t e s t D e v i c e = 0 x0b

#b u f f e r s i z e : 4096
count = 0
Buffer = [ ]
RowSize = 64 ∗ 8
welcome = " DISTRIBUTED CONTROL SYSTEMS LAB . - UPC \ r \ nEvidence Flex Board - Bootloader
Application - Version 1.1 , February 2015\ r \ nDeveloped by CARLOS XAVIER ROSERO & MANEL
VELASCO GARCIA "
vtxNumber = 115200
currentLoc = 0

usbPort = [ ’ ’ , ’ ’ , ’ ’ , ’ ’ , ’ ’ ]
f i l e A d d r = [ ’’ , ’’ , ’’ , ’’ , ’’ ]

# 128 k f l a s h f o r t h e dsPIC33FJ256MC710
f l a s h s i z e = 175104 ∗ 2 #175104−−>0x2ac00 ( ∗ 2 ’ cause each location has only 1 byte instead
of 2)

def helpMessage ( typ ) :

if typ == 0: # call from user interface

print ’\ r \n h e l p Shows t h i s menu’

print ’ r e s e t [ # ] Causes a hard r e s e t and e s t a b l i s h e s c o n n e c t i o n t o
bootloader ’
print ’ c l e a r Clears console screen ’
print ’ run [ # ] Runs program l o a d e d by u s e r ’
print " test [#] Blinks device ’ s l e d t o i d e n t i f y i f i t i s working on t h i s
port "
print ’ fileaddr [#] [ r ] Shows current assigned . elf or . hex to be programmed \ r \
n [ w ] [ filename ] Sets a new target file ’
print ’ usbscan Searches for available USB ports ’

100
 print ’ usbport [#] [ r ] Shows current assigned USB target \ r \ n [w]
[ portname ] Sets a new USB target ’
print ’ erase [#] [ a ] Erases whole target flash memory \ r \ n [0
xXXXX ] Erases only page from 0 xXXXX (0 x400 step length ) ’
print ’ program [#] [ a ] Programs whole target flash memory \ r \ n [f
] Programs only memory locations that contain data ( fast programming ) ’
print ’ [0 xXXXX ] Programs only page from 0 xXXXX (0 x400 step length )
’
print ’ read [#] [0 xXXXX ] Reads page from 0 xXXXX (0 x400 step length ) ’
print ’ exit Ends this program \ r \ n ’
print ’ Notice that [#] describes the device to access , it is a value between 0
and 4. ’
print ’ Furthermore , among several arguments , spaces are not allowed , for
example : ’
print ’ " program 0 f " will launch the fast programming task over device 0 ’

elif typ == 1: # call from console

print ’\ r \ nCOMMANDS AND EXTENSIONS \ r \ n ’

print ’ -h Shows this menu ’

print ’ -s [#] Causes a hard reset and establishes connection to bootloader ’
print ’ -r [#] Runs program loaded by user ’
print " −b [#] B l i n k s d e v i c e ’s led to identify if it is working on this port "
print ’ −x [#] [ r ] Shows c u r r e n t a s s i g n e d . e l f o r . hex t o be programmed \ r \n
[ w ] [ f i l e n a m e ] S e t s a new t a r g e t f i l e ’
print ’ −u S e a r c h e s f o r a v a i l a b l e USB p o r t s ’
print ’ −t [ # ] [ r ] Shows c u r r e n t a s s i g n e d USB t a r g e t \ r \n [w] [
portname ] S e t s a new USB t a r g e t ’
print ’ −e [ # ] [ a ] E r a s e s whole t a r g e t f l a s h memory\ r \n [ 0xXXXX]
E r a s e s o n l y page from 0xXXXX ( 0 x400 s t e p l e n g t h ) ’
print ’ −p [ # ] [ a ] Programs whole t a r g e t f l a s h memory\ r \n [f]
Programs o n l y memory l o c a t i o n s t h a t c o n t a i n data ( f a s t programming ) ’
print ’ [ 0xXXXX] Programs o n l y page from 0xXXXX ( 0 x400 s t e p l e n g t h ) ’
print ’ −d [ # ] [ 0xXXXX] Reads page from 0xXXXX ( 0 x400 s t e p l e n g t h ) \ r \n’
print ’ N o t i c e t h a t [ # ] d e s c r i b e s t h e d e v i c e t o a c c e s s , i t i s a v a l u e between 0
and 4 . ’
print ’ Furthermore , among s e v e r a l arguments , s p a c e s a r e not a l l o w e d , f o r
example : ’
print ’ " -p 0 f " w i l l l a u n c h t h e f a s t programming t a s k o v e r d e v i c e 0 ’

def asc2hex ( value ) :

if value >= 0 x30 and value <= 0 x39 : # ’0 ’ to ’9 ’

return value - 0 x30
elif value >= 0 x41 and value <= 0 x46 : # ’A’ to ’F’
return value - 0 x37
elif value >= 0 x61 and value <= 0 x66 : # ’a ’ to ’ f ’
return value - 0 x57
else :
return 0 x00

def changeUSB ( portName ) :

global usbPort , fileAddr

usbPort [ currentLoc ] = portName [2: len ( portName ) ] # removes " w " part and updates usbport
[]

defaultConfig = open ( ’ c o n f i g S e r i a l B o o t . c o n f i g ’ , ’w’) # it is going to replace existing

file content

for i in range (0 , 5) :
defaultConfig . write ( usbPort [ i ] + ’\n’) # writes all usb ports

101
 defaultConfig . write ( fileAddr [ i ] + ’\n’) # writes all file addresses

defaultConfig . close ()

print ’USB p o r t ’ ,
print ’" ’ + usbPort [ currentLoc ] + ’" a s s i g n e d t o group ’ ,
print currentLoc

return portName [2: len ( portName ) ]

def scanUSB () : # scan for available ports . return a list of tuples ( num , name )

available = []

ports =( ’/ dev /ttyACM’ , ’/ dev /ttyUSB ’ , ’/ dev / t t y S ’)

for j in range (3) :

for i in range (256) :
try :
s = serial . Serial ( ports [ j ] + ’%d’ % i )
available . append (( i , s . portstr ) )
s . close () # explicit close ’ c a u s e o f d e l a y e d GC i n j a v a
except s e r i a l . SerialException :
pass

usbDevices = l i s t ( )
i f l e n ( a v a i l a b l e ) != 0 :
p r i n t " Devices found at : "
for n , s in a v a i l a b l e :
p r i n t " [% d ] % s " % ( n , s )
u s b D e v i c e s . append ( s )
else :
p r i n t " Has not been found any attached device ! "

return u s b D e v i c e s

d e f changeAddress ( address ) :

g l o b a l usbPort , f i l e A d d r

i f ( a d d r e s s [ l e n ( a d d r e s s ) −3: l e n ( a d d r e s s ) ] == ’ elf ’ ) o r \
( a d d r e s s [ l e n ( a d d r e s s ) −3: l e n ( a d d r e s s ) ] == ’ hex ’ ) :

f i l e A d d r [ c u r r e n t L o c ] = a d d r e s s [ 2 : l e n ( a d d r e s s ) ] #removes " w " p a r t and u p d a t e s

fileAddr [ ]

d e f a u l t C o n f i g = open ( ’ c o n f i g S e r i a l B o o t . config ’ , ’w ’ ) #i t i s going to r e p l a c e

e x i s t i n g f i l e content

for i in range (0 , 5) :
d e f a u l t C o n f i g . w r i t e ( usbPort [ i ] + ’\ n ’ ) #w r i t e s a l l usb p o r t s
d e f a u l t C o n f i g . w r i t e ( f i l e A d d r [ i ] + ’\ n ’ ) #w r i t e s a l l f i l e a d d r e s s e s

defaultConfig . close ()

# p r i n t ’ *. file address ’ % ( a d d r e s s [ l e n ( a d d r e s s )−3 : l e n ( a d d r e s s ) ] ) ,

p r i n t ’ File " ’ + f i l e A d d r [ c u r r e n t L o c ] + ’" assigned to group ’ ,
print currentLoc
return a d d r e s s [ l e n ( a d d r e s s )−3 : l e n ( a d d r e s s ) ]

else :
p r i n t ’ File extension not accepted at group ’ ,
print currentLoc
return ’ ’

102
def loadConfig () :

g l o b a l usbPort , f i l e A d d r

s e l f N a m e = o s . path . basename ( file ) #s c r i p t ’s self location

selfPath = os . path . realpath ( __file__ ) # script ’ s s e l f name

i f s e l f P a t h . e n d s w i t h ( s e l f N a m e ) : #removes s c r i p t ’s name from path

selfPath = selfPath [: - len ( selfName ) ]

os . chdir ( selfPath ) # sets current directory in order to access the config file
defaultConfig = open ( ’ c o n f i g S e r i a l B o o t . c o n f i g ’ , ’ r ’)

for i in range (0 , 5) :
usbPort [ i ] = defaultConfig . readline () [: -1] # reads line and removes last character
fileAddr [ i ] = defaultConfig . readline () [: -1] # reads line and removes last
character

defaultConfig . close ()

def openPort () : # opens serial port

# use this part , only with old python versions

#### ######## ##
ser = adapt ( port = usbPort [ currentLoc ] , baudrate = vtxNumber , parity = serial . PARITY_NONE ,
stopbits = serial . STOPBITS_ONE )
# serial . Serial ( port = usbPort , baudrate =115200 , parity = serial . PARITY_NONE , stopbits =
serial . STOPBITS_ONE )
# ## ## ## # ## ## ## #

ser . open
ser . isOpen
return ser

def closePort ( ser ) : # closes serial port

ser . close ()

def searchCommand ( inpt ) : # separates command and argument

spaceFound = False
i = 0
endCommand = 0
sizeSpace = 0
userCommand = ’ ’
userArgument = ’ ’

for i in range (0 , len ( inpt ) ) :

if spaceFound == False :
if inpt [ i ] == ’ ’: # search space to isolate command from argument
spaceFound = True # first space has been found
endCommand = i
sizeSpace = 1 # so far , one space
else :
endCommand = i +1
else :
if inpt [ i ] == ’ ’: # check more spaces
sizeSpace = sizeSpace + 1
else :
break # spaces finished , starts argument

userCommand = inpt [0: endCommand ]

if spaceFound == True :

103
 if len ( inpt ) > ( endCommand + sizeSpace ) :
userArgument = inpt [ endCommand + sizeSpace : len ( inpt ) ]
succ = True
else :
succ = False
else :
succ = False
return [ succ , userCommand , userArgument ]

def resetSys () :

print " Resetting hardware ... " ,

ser = openPort ()
Buffer = bytearray ([ ’∗ ’ , ’C’ , ’x ’ , ’R’ , ’ c ’ ])
# Buffer = b ’∗CxRc’
ser . write ( Buffer )
time . sleep (0.5)

inputS = [] # initializes buffer

while ser . inWaiting () > 0: # neglects the trash code received
inputS . append ( ser . read (1) ) # appends a new value into the buffer

print " Done "

print " Searching for a compatible device ..."

ser . write ( chr ( command . readID ) )
time . sleep (0.25)

inputS = [] # initializes buffer

while ser . inWaiting () > 0:
receive = hex ( ord ( ser . read (1) ) ) # from received data to ascii hex repre sentatio n
data = int ( receive , 16) # from ascii hex repres entatio n to number
inputS . append ( data ) # appends a new value into the buffer

if inputS != ’ ’ and len ( inputS ) == 8:

procId = hex (( inputS [1] << 8) | inputS [0])
devId = hex (( inputS [5] << 8) | inputS [4])

if procId == ’0 x b f ’ and devId == ’0 x3040 ’:

print " Evidence Flex Board has been detected - Processor ID " , procId , " -
Device ID " , devId
else :
print " Error , no compatible device has been detected !"
else :
print " Error , no compatible device has been detected !"
closePort ( ser )

def runProg () :

print " Starting to run program ... " ,

ser = openPort ()
Buffer = bytearray ([ command . runProg ])
ser . write ( Buffer ) # sends reset command
time . sleep (0.18)

inputS = [] # initialize buffer

while ser . inWaiting () > 0:
inputS . append ( ser . read (1) ) # appends a new value into the buffer

closePort ( ser )

if len ( inputS ) != 0: # avoids empty buffer

# no problem with what value comes
print " Done "

104
 else :
print " Error trying to access the board !"

def r e a d P r o g r a m M e m o r y ( readAddress , typeProg ) :

ser = openPort ()

if typeProg == 1:
print " Entered address : 0 x %06 x " % readAddress

readAddress = readAddress - ( readAddress % ( RowSize * 2) )

if typeProg == 1:
print " Reading program memory at address 0 x %06 x ..." % readAddress

Buffer = bytearray ([ command . readPM , readAddress & 0 xff ,

( readAddress >> 8) & 0 xff , ( readAddress >> 16) & 0 xff ])
ser . write ( Buffer )
time . sleep (0.5)

inputS = [] # initialize buffer

while ser . inWaiting () > 0:
receive = hex ( ord ( ser . read (1) ) ) # from received data to ascii hex repres entatio n
data = int ( receive , 16) # from ascii hex repre sentatio n to number
inputS . append ( data ) # appends a new value into the buffer
closePort ( ser )

retValue = 0

if typeProg == 1:
print "% d bytes from memory locations have been received " % len ( inputS )

for i in range (0 , ( len ( inputS ) ) , 24) :

print "%06 x : %02 x %02 x %02 x %02 x %02 x %02 x %02 x %02 x %02 x %02 x %02 x %02 x " %(
readAddress , inputS [ i ] , inputS [ i +1] , inputS [ i +2] , inputS [ i +3] ,
inputS [ i +4] , inputS [ i +5] , inputS [ i +6] , inputS [ i +7] , inputS [ i +8] ,
inputS [ i +9] , inputS [ i +10] , inputS [ i +11]) ,

print "%02 x %02 x %02 x %02 x %02 x %02 x %02 x %02 x %02 x %02 x %02 x %02 x " %(
inputS [ i +12] , inputS [ i +13] , inputS [ i +14] , inputS [ i +15] ,
inputS [ i +16] , inputS [ i +17] , inputS [ i +18] , inputS [ i +19] ,
inputS [ i +20] , inputS [ i +21] , inputS [ i +22] , inputS [ i +23])
readAddress = readAddress + 16

elif typeProg == 0:
retValue = inputS

return retValue

def e r a s e P r o g r a m M e m o r y ( eraseAddress , typeProg ) : # erases program memory ( without

bootloader part )

if typeProg == 0:

print " Entered address : 0 x %06 x " % eraseAddress

eraseAddress = eraseAddress - ( eraseAddress % ( RowSize * 2) )

print " Clearing program memory from 0 x %06 x to 0 x %06 x ... " %( eraseAddress ,
eraseAddress +0 x3ff ) ,

Buffer = bytearray ([ command . erasePM , eraseAddress & 0 xFF ,

( eraseAddress >> 8) & 0 xFF , ( eraseAddress >> 16) & 0 xFF ])
ser = openPort ()

105
 ser . write ( Buffer )
time . sleep (0.18)

inputS = [] # initialize buffer

while ser . inWaiting () > 0:
inputS . append ( ser . read (1) ) # appends a new value into the buffer

closePort ( ser )

if len ( inputS ) != 0: # avoids empty buffer

if inputS [0] == ’\ x01 ’:
print " Done "
else :
print " Error , trying to access a forbidden memory page !"
else :
print " Error , has not been detected any incoming message !"

elif typeProg == 1:

print " Clearing all program memory locations , please wait ... "

pbar = ProgressBar ( widgets =[ ’ P r o g r e s s ’ , Percentage () ,

’ − ’ , Timer () ] , maxval =170) . start ()

ser = openPort ()

flagError = 0

for page in range (0 , 171) :

pbar . update ( page ) # updates progressbar state

if page == 1 or page == 2: # avoids to access to bootloader locations

pass
else :
eraseAddress = page * 0 x400
Buffer = bytearray ([ command . erasePM , eraseAddress & 0 xff ,
( eraseAddress >> 8) & 0 xff , ( eraseAddress >> 16) & 0 xff ])
ser . write ( Buffer )
time . sleep (0.18)

inputS = [] # initialize buffer

while ser . inWaiting () > 0:
inputS . append ( ser . read (1) ) # appends a new value into the buffer

if len ( inputS ) != 0: # avoids empty buffer

if inputS [0] == ’\ x01 ’:
pass
else :
flagError = 1
break
else :
flagError = 2
break

closePort ( ser )
pbar . finish ()

if flagError == 0: # no error
print " Process has been completed successfully !"
elif flagError == 1: # writing error
print " Error at page from 0 x %06 x !" % eraseAddress
elif flagError == 2:
print " No response from board !"

106
def sendData ( progMemory , progAddress , typeProg ) :

if typeProg == 0: # will program only the desired page

print " Entered address : 0 x %06 x " % progAddress

progAddress = progAddress - ( progAddress % ( RowSize * 2) )

print " Programming memory from 0 x %06 x to 0 x %06 x ... " %(

progAddress , progAddress +0 x3ff ) ,

page = progAddress / 0 x400

# here , check if this page is empty

blank = 1
for i in range (0 , RowSize *2) :
if progMemory [ page ][ i ] != 0 xffff :
blank = 0

ser = openPort ()

###################
inputS = [] # initializes buffer
while ser . inWaiting () > 0: # neglects the trash code received
inputS . append ( ser . read (1) ) # appends a new value into the buffer
##################

if blank == 1: # empty page

Buffer = bytearray ([ command . erasePM , progAddress & 0 xff ,
( progAddress >> 8) & 0 xff , ( progAddress >> 16) & 0 xff ])

elif blank == 0: # filled page

Buffer = bytearray ([ command . writePM , progAddress & 0 xff ,
( progAddress >> 8) & 0 xff , ( progAddress >> 16) & 0 xff ])

ser . write ( Buffer ) # sends address

Buffer = bytearray ([])

for i in range (0 , RowSize *2 ,2) : # total ammount of data : 1536 bytes

# only 3 bytes per location , 4 th is always 0
Buffer . append (( progMemory [ page ][ i ] >> 8) & 0 xff )
Buffer . append ( progMemory [ page ][ i ] & 0 xff )
Buffer . append (( progMemory [ page ][ i +1] >> 8) & 0 xff )

ser . write ( Buffer )

time . sleep (0.18)

inputS = [] # initialize buffer

while ser . inWaiting () > 0:
inputS . append ( ser . read (1) ) # appends a new value into the buffer

closePort ( ser )

if len ( inputS ) != 0: # avoids empty buffer

if inputS [0] == ’\ x01 ’:
print " Done "
else :
print " Error , trying to access a forbidden memory page !"
else :
print " Error , has not been detected any incoming message !"

elif typeProg == 1: # will program all memory pages

107
print " Programming all memory locations , please wait ... "

pbar = ProgressBar ( widgets =[ ’ P r o g r e s s ’ , Percentage () ,

’ − ’ , Timer () ] , maxval = 170) . start ()

flagError = 0

ser = openPort ()

###################
time . sleep (0.25)
inputS = [] # initializes buffer
while ser . inWaiting () > 0: # neglects the trash code received
inputS . append ( ser . read (1) ) # appends a new value into the buffer
##################

for page in range (0 , 171) :

pbar . update ( page ) # updates progressbar state

if page == 1 or page == 2: # avoids the access to bootloader locations

pass
# print " Bootloader at page %02 d , skipped !" % page
else :
progAddress = page * 0 x400

blank = 1
for i in range (0 , RowSize *2) :
if progMemory [ page ][ i ] != 0 xffff :
blank = 0

if blank == 1: # empty page

# print " Erasing page %03 d of 170 , from 0 x %06 x to 0 x %06 x ... " %(
# page , ( page *0 x400 ) , ( page *0 x400 ) +0 x3ff ) ,
Buffer = bytearray ([ command . erasePM , progAddress & 0 xff ,
( progAddress >> 8) & 0 xff ,
( progAddress >> 16) & 0 xff ])

elif blank == 0: # filled page

# print " Programming page %03 d of 170 , from 0 x %06 x to 0 x %06 x ... " %(
# page , ( page *0 x400 ) , ( page *0 x400 ) +0 x3ff ) ,
Buffer = bytearray ([ command . writePM , progAddress & 0 xff ,
( progAddress >> 8) & 0 xff ,
( progAddress >> 16) & 0 xff ])

ser . write ( Buffer ) # sends address

Buffer = bytearray ([])

for i in range (0 , RowSize *2 ,2) : # total ammount of data : 1536 bytes

# only 3 bytes per location , 4 th is always 0
Buffer . append (( progMemory [ page ][ i ] >> 8) & 0 xff )
Buffer . append ( progMemory [ page ][ i ] & 0 xff )
Buffer . append (( progMemory [ page ][ i +1] >> 8) & 0 xff )

ser . write ( Buffer ) # sends data

time . sleep (0.3)

inputS = [] # initialize buffer

while ser . inWaiting () > 0:
inputS . append ( ser . read (1) ) # appends a new value into the buffer

if len ( inputS ) != 0: # avoids empty buffer

108
 if inputS [0] == ’\ x01 ’:
pass
else :
flagError = 1
break
else :
flagError = 2
break

closePort ( ser )

pbar . finish ()

if flagError == 0: # no error
print " Process has been completed successfully !"
elif flagError == 1: # writing error
print " Error at page from 0 x %06 x !" % progAddress
elif flagError == 2:
print " No response !"

elif typeProg == 2: # will program only changed pages

print " Programming only locations that contain data , please wait ... "

pbar = ProgressBar ( widgets =[ ’ P r o g r e s s ’ , Percentage () ,

’ − ’ , Timer () ] , maxval =170) . start ()

flagError = 0

ser = openPort ()

for page in range (0 , 171) :

pbar . update ( page ) # updates progressbar state

if page == 1 or page == 2: # avoids the access to bootloader locations

pass
# print " Bootloader at page %02 d , skipped !" % page
else :
progAddress = page * 0 x400

blank = 1
for i in range (0 , RowSize *2) :
if progMemory [ page ][ i ] != 0 xffff : # if at least 1 location is
different to 0 xffff
blank = 0

if blank == 0: #0 means filled page

# print " Programming page %03 d of 170 , from 0 x %06 x to 0 x %06 x ... " %(
# page , ( page *0 x400 ) , ( page *0 x400 ) +0 x3ff ) ,
Buffer = bytearray ([ command . writePM , progAddress & 0 xff ,
( progAddress >> 8) & 0 xff ,
( progAddress >> 16) & 0 xff ])

ser . write ( Buffer ) # sends address

Buffer = bytearray ([])

for i in range (0 , RowSize *2 , 2) : # total ammount of data : 1536 bytes

# only 3 bytes per location , 4 th is always 0
Buffer . append (( progMemory [ page ][ i ] >> 8) & 0 xff )
Buffer . append ( progMemory [ page ][ i ] & 0 xff )
Buffer . append (( progMemory [ page ][ i +1] >> 8) & 0 xff )

109
 ser . write ( Buffer ) # sends data
time . sleep (0.3)

inputS = [] # initialize buffer

while ser . inWaiting () > 0:
inputS . append ( ser . read (1) ) # appends a new value into the buffer

if len ( inputS ) != 0: # avoids empty buffer

if inputS [0] == ’\ x01 ’:
pass
else :
flagError = 1
break
else :
flagError = 2
break

closePort ( ser )

pbar . finish ()

if flagError == 0: # no error
print " Process has been completed successfully !"
elif flagError == 1: # writing error
print " Error at page from 0 x %06 x !" % progAddress
elif flagError == 2:
print " No response !"

def sendElfFile ( progAddress , typeProg ) : # takes information from elf file and send through
USB

print " Reading data from ELF file " , fileAddr [ currentLoc ]

with open ( fileAddr [ currentLoc ] , ’ r ’) as f :

# get the data

elffile = ELFFile ( f )

print ’SECTIONS FOUND IN FILE : ’

# prints all the sections found

for s in elffile . iter_sections () :
print "[% d ] % s % s start addr :0 x %06 x size :% d offs :% d " %(
s . header [ ’ sh name ’] , s . name , s . header [ ’ s h t y p e ’] ,
s . header [ ’ s h a d d r ’] , s . header [ ’ s h s i z e ’] ,
s . header [ ’ s h o f f s e t ’ ])
print ""

# prepares the memory

# flashMemory = bytearray ( flashsize )
flashMemory = bytearray ([])

# fills the bytearray with 0 xff

for i in range (0 , flashsize ) :
flashMemory . append (0 xff )

for s in elffile . iter_sections () :

if ( s . header [ ’ s h t y p e ’] == ’SHT PROGBITS’ and s . header [ ’ s h a d d r ’] < 0 xf80000
):
addr = s . header [ ’ s h a d d r ’]

if s . name == ’ . const ’:
addr = addr - 0 x8000 # eliminates an error , I don ’ t know why

110
 s i z e = s . h e a d e r [ ’ sh_size ’ ]
v a l = s . data ( )
p r i n t ’% s section from 0 x %06 x to 0 x %06 x appended to file ’ %( s . name , addr ,
( addr − 1 + s i z e / 2 ) )

flashMemory [ addr ∗2 : addr ∗2 + s i z e ] = v a l

#c r e a t e s an a r r a y with p r o p e r f o r m a t
progMemory = [ [ 0 x f f f f f o r x i n x r a n g e ( RowSize ∗ 2 ) ] f o r x i n x r a n g e ( 1 7 1 ) ] #171
#512∗2∗171=175104−−>0 x2ac00

for i in xrange ( 0 , 171) :

a = i ∗ 0 x400 ∗ 2
f o r j i n x r a n g e ( 0 , RowSize ∗ 2 ) :
progMemory [ i ] [ j ] = ( flashMemory [ a + j ∗ 2 ] << 8 ) | flashMemory [ a + j ∗2 + 1 ]

#s e n d s data

sendData ( progMemory , progAddress , typeProg )

d e f s e n d H e x F i l e ( progAddress , typeProg ) : #t a k e s a s c i i i n f o r m a t i o n from hex f i l e and

t r a n s l a t e s t o n u m e r i c a l one

p r i n t " Reading data from HEX file ... " , f i l e A d d r

f i l e = open ( f i l e A d d r [ c u r r e n t L o c ] , ’r ’ )

f i l e R a w = f i l e . r e a d ( ) #t a k e s i n f o r m a t i o n from f i l e

f i l e . c l o s e ( ) #c l o s e s opened f i l e

fileRawHex = [ ]
for i in range (0 , l e n ( fileRaw ) ) :
data = hex ( ord ( f i l e R a w [ i ] ) ) #from r e c e i v e d data t o a s c i i hex r e p r e s e n t a t i o n
fileRawHex . append ( i n t ( data , 1 6 ) ) #from a s c i i hex r e p r e s e n t a t i o n t o number

countLF = 0
f o r i i n r a n g e ( 0 , l e n ( fileRawHex ) ) : #c o u n t s number o f l i n e s i n t o t h e hex f i l e
i f fileRawHex [ i ] == 0 x0a :
countLF = countLF + 1

mat = [ [ ] f o r x i n r a n g e ( countLF ) ] #c r e a t e s countLF t i m e s an empty a r r a y

#h e r e , t a k e s i n o r d e r , t h e i n f o r m a t i o n from . hex f i l e
addr = 1
f o r i i n r a n g e ( 0 , countLF ) : # f i l l s each a r r a y with l i n e s from hex f i l e
a c c 1 = ( a s c 2 h e x ( fileRawHex [ addr ] ) << 4 ) | a s c 2 h e x ( fileRawHex [ addr + 1 ] )
mat [ i ] . append ( a c c 1 ) #number o f data b y t e s ( e l e m e n t 0 o f mat [ i ] )
a c c 1 = ( a s c 2 h e x ( fileRawHex [ addr + 2 ] ) << 4 ) | a s c 2 h e x ( fileRawHex [ addr + 3 ] )
a c c 2 = ( a s c 2 h e x ( fileRawHex [ addr + 4 ] ) << 4 ) | a s c 2 h e x ( fileRawHex [ addr + 5 ] )
mat [ i ] . append ( ( a c c 1 << 8 ) | a c c 2 ) #s t a r t i n g a d d r e s s o f t h e data r e c o r d
#( e l e m e n t 1 o f mat [ i ] )
a c c 1 = ( a s c 2 h e x ( fileRawHex [ addr + 6 ] ) << 4 ) | a s c 2 h e x ( fileRawHex [ addr + 7 ] )
mat [ i ] . append ( a c c 1 ) #r e c o r d t y p e ( e l e m e n t 2 o f mat [ i ] )

i f mat [ i ] [ 0 ] != 0 :
f o r j i n r a n g e ( addr + 8 , addr + 8 + 2∗mat [ i ] [ 0 ] , 4 ) : #data
a c c 1 = ( a s c 2 h e x ( fileRawHex [ j ] ) << 4 ) | a s c 2 h e x ( fileRawHex [ j + 1 ] )
a c c 2 = ( a s c 2 h e x ( fileRawHex [ j + 2 ] ) << 4 ) | a s c 2 h e x ( fileRawHex [ j + 3 ] )
mat [ i ] . append ( ( a c c 1 << 8 ) | a c c 2 )
l a s t A d d r = j + 3 #s a v e s l a s t a d d r e s s ( depends on q u a n t i t y o f data , a l w a y s
changing )
else :
l a s t A d d r = addr + 7

111
 #p r i n t l a s t A d d r
a c c 1 = ( a s c 2 h e x ( fileRawHex [ l a s t A d d r + 1 ] ) << 4 ) | a s c 2 h e x ( fileRawHex [ l a s t A d d r +
2])
mat [ i ] . append ( a c c 1 ) #two ’s complement of the preceding line ( checksum )

addr = lastAddr + 5 # address of the first value in the next line

# print "\ r "

# for i in range (0 , countLF ) :
# for j in range (0 , len ( mat [ i ]) ) :
# print "%06 x " % mat [ i ][ j ] ,
# print "\ r "

# here , starts information formatting

lba = 0 # linear base address

# scans record type and makes changes in addresses

for i in range (0 , countLF ) :
if mat [ i ][2] == 0: # data record ?
# adds lba to data record offset and divide by two
mat [ i ][1] = ( mat [ i ][1] + lba ) / 2

elif mat [ i ][2] == 1: # end of file record ?

pass

elif mat [ i ][2] == 4: # extended address record ?

lba = mat [ i ][3] # takes upper linear base address ( ulba )
lba = lba << 16 # lba = ulba (16 bits ) |0 x00 (16 bits )
# print "%08 x " % lba
else :
print " Unknown hex record type !"

# print "\ r "

# for i in range (0 , countLF ) :
# for j in range (0 , len ( mat [ i ]) ) :
# print "%06 x " % mat [ i ][ j ] ,
# print "\ r "

# creates and initializes virtual memory maps

#171*512 matrix filled with 0 xffff

progMemory = [[0 xffff for x in xrange ( RowSize *2) ] for x in xrange (171) ] #171
#512*2*171=175104 - - >0 x2ac00
configMemory = [0 xffff for x in xrange (12) ]

# stores data taken from hex file , into new memory matrices
for i in range (0 , len ( mat ) ) :
if mat [ i ][2] == 0: # data record ?

if mat [ i ][1] < (512*171) : # if address is into the program memory

mAddr = mat [ i ][1] // ( RowSize *2) # row ( int division )
nAddr = mat [ i ][1] % ( RowSize *2) # column ( modulo )

if mAddr < 171 and ( nAddr +( mat [ i ][0]) ) < ( RowSize *2) :
# print mat [ i ][1]
# print mAddr ,
# print nAddr ,

for j in range (0 , ( mat [ i ][0]) /2) :

progMemory [ mAddr ][ nAddr + j ] = mat [ i ][3+ j ] # saves in program memory
buffer
# print j ,
# print "% x " % progMemory [ mAddr ][ nAddr + j ] ,

112
# print ""
else :
print " Hex record decoding error at 0 x % x !" % mat [ i ][1]

elif mat [ i ][1] >= 0 xf80000 : # if address is into the configuration registers
mAddr = ( mat [ i ][1] - 0 xf80000 ) /2 # row
# print mAddr ,

for j in range (0 , ( mat [ i ][0]) /2) :

configMemory [ mAddr + j ] = mat [ i ][3+ j ] # saves in config memory buffer
# print "% x " % configMemory [ mAddr + j ]
# print ""
else :
print " Unknown hex record type !"

elif mat [ i ][2] == 1: # end of file record ?

pass
elif mat [ i ][2] == 4: # extended address record ?
pass
else :
print " Unknown hex record type !"

# print "\ r "

# m =3
# for i in range (0 , len ( progMemory [ m ]) ,8) :
# print "%04 x :" %( i + m *0 x400 ) ,
# for j in range (0 ,8) :
# print "%04 x " % progMemory [ m ][ i + j ] ,
# print "\ r "

# sends data
sendData ( progMemory , progAddress , typeProg )

def blinkLed () :

resetSys ()

print " Testing device ... " ,

ser = openPort ()
Buffer = bytearray ([ command . testDevice ])
ser . write ( Buffer ) # sends test command
time . sleep (0.18)

inputS = [] # initialize buffer

while ser . inWaiting () > 0:
inputS . append ( ser . read (1) ) # appends a new value into the buffer

closePort ( ser )

if len ( inputS ) != 0: # avoids empty buffer

# no problem with what value comes
print " Done "
else :
print " Device not available "

#######################################
########## callings to subroutines
#######################################

def callResetSys ( userArgument ) :

global currentLoc

if ’0 ’ <= userArgument [0] <= ’4 ’:

113
 if len ( userArgument ) == 1:
currentLoc = int ( hex ( ord ( userArgument [0]) ) , 16) - 0 x30 # from ascii number
repr esentati on to integer
resetSys ()
else :
print ’No more arguments r e q u i r e d ! ’
else :
print ’Wrong p o r t l o c a t i o n ! ’

def callRunProg ( userArgument ) :

global currentLoc

if ’0 ’ <= userArgument [0] <= ’4 ’:

if len ( userArgument ) == 1:
currentLoc = int ( hex ( ord ( userArgument [0]) ) , 16) - 0 x30 # from ascii number
repr esentati on to integer
runProg ()
else :
print ’No more arguments r e q u i r e d ! ’
else :
print ’Wrong p o r t l o c a t i o n ! ’

def callChangeUSB ( userArgument ) :

global currentLoc

retVal = ’ ’
if ’0 ’ <= userArgument [0] <= ’4 ’:
currentLoc = int ( hex ( ord ( userArgument [0]) ) , 16) - 0 x30 # from ascii number
repr esentati on to integer
if userArgument [1] == ’ r ’:
if len ( userArgument ) == 2:
print ’ Port ’ ,
print ’" ’ + usbPort [ currentLoc ] + ’" a s s i g n e d t o group ’ ,
print currentLoc
retVal = usbPort [ currentLoc ]
else :
print ’Too many arguments ! ’
elif userArgument [1] == ’w’:
if len ( userArgument ) > 2:
retVal = changeUSB ( userArgument )
else :
print ’ Port d e s c r i p t i o n l o s t a t group ’ ,
print currentLoc
else :
print ’Wrong argument f o r m a t ! ’
else :
print ’Wrong group ! ’

return retVal

def c a l l C h a n g e A d d r e s s ( userArgument ) :

global currentLoc

retVal = ’ ’
if ’0 ’ <= userArgument [0] <= ’4 ’:
currentLoc = int ( hex ( ord ( userArgument [0]) ) , 16) - 0 x30 # from ascii number
repr esentati on to integer
if userArgument [1] == ’ r ’:
if len ( userArgument ) == 2:
print ’ Address ’ ,
print ’" ’ + fileAddr [ currentLoc ] + ’" a s s i g n e d t o group ’ ,

114
 print currentLoc
retVal = fileAddr [ currentLoc ]
else :
print ’Too many arguments ! ’
elif userArgument [1] == ’w’:
if len ( userArgument ) > 6: # at least 1 character , dot and the extension
retVal = changeAddress ( userArgument )
else :
print ’Too short a d d r e s s d e s c r i p t i o n a t group ’ ,
print currentLoc
else :
print ’Wrong argument f o r m a t ! ’
else :
print ’Wrong group ! ’

return retVal

def callProgram ( userArgument ) :

global currentLoc

if len ( userArgument ) >= 1:

if ’0 ’ <= userArgument [0] <= ’4 ’:
currentLoc = int ( hex ( ord ( userArgument [0]) ) , 16) - 0 x30 # from ascii number
repre sentati on to integer
if len ( userArgument ) >= 2:
if userArgument [1] == ’0 ’ and userArgument [2] == ’x ’:
# the program ensures only ascii numbers are entered
error = True
num = []
for i in range (3 , len ( userArgument ) ) :
n = int ( hex ( ord ( userArgument [ i ]) ) , 16)
if (0 x30 <= n <= 0 x39 ) or (0 x41 <= n <= 0 x46 ) or \
(0 x61 <= n <= 0 x66 ) : #0 to 9 , A to F , a to f
error = False # no format error
num . append ( asc2hex ( n ) ) # changes to values
else :
error = True # format error , breaks
break

if error == False : # whether the characters are entered correctly

address = 0
m = len ( userArgument ) - 3
for i in range ( m ) :
address = address | ( num [ i ] << 4*( m -i -1) )

if fileAddr [ currentLoc ][ len ( fileAddr [ currentLoc ]) -3 : len (

fileAddr [ currentLoc ]) ] == ’ hex ’:
sendHexFile ( address , 0) # address - > location from programming
starts , 0 - > only the address will be programmed
else :
sendElfFile ( address , 0) # address - > location from programming
starts , 0 - > only the address will be programmed
else :
print ’Wrong a d d r e s s f o r m a t ! ’

elif userArgument [1] == ’a ’: # whole memory

if fileAddr [ currentLoc ][ len ( fileAddr [ currentLoc ]) -3 : len ( fileAddr [
currentLoc ]) ] == ’ hex ’:
sendHexFile (0 , 1) #0 no meaning , 1 - > all memory will be
programmed
else :
sendElfFile (0 , 1) #0 no meaning , 1 - > all memory will be
programmed

115
 elif userArgument [1] == ’ f ’: # only changed memory pages
if fileAddr [ currentLoc ][ len ( fileAddr [ currentLoc ]) -3 : len ( fileAddr [
currentLoc ]) ] == ’ hex ’:
sendHexFile (0 , 2) #0 no meaning , 2 - > all memory will be
programmed
else :
sendElfFile (0 , 2) #0 no meaning , 2 - > all memory will be
programmed
else :
print ’Wrong a d d r e s s f o r m a t ! ’
else :
print ’Wrong argument f o r m a t ! ’
else :
print ’Wrong l o c a t i o n ! ’
else :
print ’Wrong argument f o r m a t ! ’

def callErase ( userArgument ) :

global currentLoc

if len ( userArgument ) > 1:

if ’0 ’ <= userArgument [0] <= ’4 ’:
location = int ( hex ( ord ( userArgument [0]) ) , 16) - 0 x30 # from ascii number
repr esentati on to integer
if len ( userArgument ) >= 2:
if userArgument [1] == ’0 ’ and userArgument [2] == ’x ’:
# the program ensures only ascii numbers are entered
error = True
num = []
for i in range (3 , len ( userArgument ) ) :
n = int ( hex ( ord ( userArgument [ i ]) ) , 16)
if (0 x30 <= n <= 0 x39 ) or (0 x41 <= n <= 0 x46 ) or \
(0 x61 <= n <= 0 x66 ) : #0 to 9 , A to F , a to f
error = False # no format error
num . append ( asc2hex ( n ) ) # changes to values
else :
error = True # format error , breaks
break

if error == False : # whether the characters are entered correctly

address = 0
m = len ( userArgument ) - 3
for i in range ( m ) :
address = address | ( num [ i ] << 4*( m -i -1) )
e r a s e P r o g r a m M e m o r y ( address , 0) # address - > location from erasing
starts , 0 - > only the address will be erased
else :
print ’Wrong a d d r e s s f o r m a t ! ’
elif userArgument [1] == ’a ’: # whole memory
e r a s e P r o g r a m M e m o r y (0 , 1) #0 no meaning , 1 - > all memory will be erased
else :
print ’Wrong a d d r e s s f o r m a t ! ’
else :
print ’Wrong argument f o r m a t ! ’
else :
print ’Wrong l o c a t i o n ! ’
else :
print ’Wrong argument f o r m a t ! ’

def callRead ( userArgument ) :

global currentLoc

116
 if len ( userArgument ) > 2:
if ’0 ’ <= userArgument [0] <= ’4 ’:
location = int ( hex ( ord ( userArgument [0]) ) , 16) - 0 x30 # from ascii number
repre sentati on to integer
if userArgument [1] == ’0 ’ and userArgument [2] == ’x ’:
# the program ensures only ascii numbers are entered
error = True
num = []
for i in range (3 , len ( userArgument ) ) :
n = int ( hex ( ord ( userArgument [ i ]) ) , 16)
if (0 x30 <= n <= 0 x39 ) or (0 x41 <= n <= 0 x46 ) or \
(0 x61 <= n <= 0 x66 ) : #0 to 9 , A to F , a to f
error = False # no format error
num . append ( asc2hex ( n ) ) # changes to values
else :
error = True # format error , breaks
break

if error == False : # whether the characters are entered correctly

address = 0
m = len ( userArgument ) - 3
for i in range ( m ) :
address = address | ( num [ i ] << 4*( m -i -1) )
r e a d P r o g r a m M e mo r y ( address , 1) #1 - > prints messages
else :
print ’Wrong a d d r e s s f o r m a t ! ’
else :
print ’Wrong a d d r e s s f o r m a t ! ’
else :
print ’Wrong l o c a t i o n ! ’
else :
print ’Wrong argument f o r m a t ! ’

def callBlinkLed ( userArgument ) :

global currentLoc

if ’0 ’ <= userArgument [0] <= ’4 ’:

if len ( userArgument ) == 1:
currentLoc = int ( hex ( ord ( userArgument [0]) ) , 16) - 0 x30 # from ascii number
repre sentati on to integer
blinkLed ()
else :
print ’No more arguments r e q u i r e d ! ’
else :
print ’Wrong p o r t l o c a t i o n ! ’

#######################################
###### here starts the main program #####
#######################################

if __name__ == " __main__ ":

loadConfig () # reads configuration
# firstly , check number of arguments in the calling

if len ( sys . argv ) == 1: # if there is only 1 argument

os . system (" clear ")

print welcome

input = 1

while 1:

117
 input = raw_input ("\ r \n > > ") # gets keyboard input
# input = input . lower () # changes to lowercase the command typed

[ flagArg , userCommand , userArgument ] = searchCommand ( input ) # separates command

and argument
userCommand = userCommand . lower () # changes to lowercase the command typed

if flagArg == False : # none argument found

if userCommand != ’ ’: # command found ( here only commands without
arguments )
if userCommand == ’ e x i t ’:
print ’ See you ! ! ! \ r \n’
exit (2)

elif userCommand == ’ h e l p ’:
helpMessage (0)

elif userCommand == ’ u s b s c a n ’:
scanUSB ()

elif userCommand == ’ c l e a r ’:
os . system (" clear ")
print welcome
else :
if ( userCommand == ’ run ’ or userCommand == ’ r e s e t ’ or userCommand
== ’ u s b p o r t ’
or userCommand == ’ f i l e a d d r ’ or userCommand == ’ r e a d ’ or
userCommand == ’ e r a s e ’
or userCommand == ’ program ’ or userCommand == ’ t e s t ’) :
print " Missing argument !"
else :
print " Command not found !"

else : # argument found ( here only commands with arguments )

if userCommand == ’ run ’:
callRunProg ( userArgument )

elif userCommand == ’ r e s e t ’:
callResetSys ( userArgument )

elif userCommand == ’ u s b p o r t ’:
callChangeUSB ( userArgument )

elif userCommand == ’ f i l e a d d r ’:
c a l l C h a n g e A d d r e s s ( userArgument )

elif userCommand == ’ r e a d ’:
callRead ( userArgument )

elif userCommand == ’ program ’:

callProgram ( userArgument )

elif userCommand == ’ e r a s e ’:
callErase ( userArgument )

elif userCommand == ’ t e s t ’:
callBlinkLed ( userArgument )

else :
print " Command not found !"

else : # if there is more than 1 argument

argum = sys . argv [1:] # takes the argument list from the second element

118
 option = ’ ’

try :
opts , args = getopt . getopt ( argum , " hus : r : e : p : t : x : d : b :" , [" rstOpt =" , " runOpt
=" , " eraseOpt =" ,
" progOpt =" , " portOpt
=" , " fileOpt =" ,
" readOpt =" , " testOpt
="])
except getopt . GetoptError :
print ’ E x c e p t i o n ! ’
sys . exit (2)

for opt , arg in opts :

if opt == ’−h’:
print ’HELP’ ,
helpMessage (1)

elif opt == ’−u’:

print ’SCAN FOR USB PORTS’ ,
scanUSB ()

elif opt in (" - s " , " - - rstOpt ") :

print ’RESET’ ,
callResetSys ( arg )

elif opt in (" - r " , " - - runOpt ") :

print ’RUN’ ,
callRunProg ( arg )
sys . exit (2)

elif opt in (" - e " , " - - eraseOpt ") :

print ’ERASE’ ,
callErase ( arg )

elif opt in (" - p " , " - - progOpt ") :

print ’PROGRAM’ ,
callProgram ( arg )

elif opt in (" - t " , " - - portOpt ") :

print ’USB PORT’ ,
callChangeUSB ( arg ) # call from console

elif opt in (" - x " , " - - fileOpt ") :

print ’HEX OR ELF FILE ADDRESS’ ,
c a l l C h a n g e A d d r e s s ( arg ) # call from console

elif opt in (" - d " , " - - readOpt ") :

print ’READ’ ,
callRead ( arg )

elif opt in (" - b " , " - - testOpt ") :

print ’TESTING CONNECTION’ ,
callBlinkLed ( arg )

A.1.2 Settings GUI (settings.py)

119
 Listing A.2: Settings-GUI file
#! / u s r / b i n / env python
from g i . r e p o s i t o r y im por t Gtk
im po rt o s
im po rt s e r i a l B o o t

#f i r s t c o n f i g u r a t i o n s initialization
def i n i t F i l e () :

s e l f N a m e = o s . path . basename ( file ) #s c r i p t ’s self location

selfPath = os . path . realpath ( __file__ ) # script ’ s s e l f name

i f s e l f P a t h . e n d s w i t h ( s e l f N a m e ) : #removes s c r i p t ’s name from path

selfPath = selfPath [: - len ( selfName ) ]

os . chdir ( selfPath ) # sets current directory in order to access the config file
# os . system ("./ serialBoot . py -h ")

class firstWindow ( Gtk . Window ) :

def __init__ ( self ) :

Gtk . Window . __init__ ( self , title =" Evidence Flex Board - Bootloader Application -
Version 1.1")
self . s e t _ b o r d e r _ w i d t h (10)

table = Gtk . Table (1 , 1 , True )

self . add ( table )

labelUSB = Gtk . Label ()

labelUSB . set_text (" USB device ")

labelAddr = Gtk . Label ()

labelAddr . set_text ("(*. elf ) File Address ")
labelName1 = Gtk . Label ()
labelName1 . set_text (" Developed by Carlos X . Rosero & Manel Velasco ")
labelName2 = Gtk . Label ()
labelName2 . set_text (" DISTRIBUTED CONTROL SYSTEMS LAB - UPC ")

buttFile = list ()
for i in range (0 , 5) :
chrt = Gtk . Button ( label = " Select file address " + str ( i ) )
chrt . connect (" clicked " , getattr ( self , " on _f il e _c li ck e d " + str ( i ) ) )
buttFile . append ( chrt )

buttUSB = list ()
for i in range (0 , 5) :
chrt = Gtk . Button ( label = " Select USB device " + str ( i ) )
chrt . connect (" clicked " , getattr ( self , " on_ USB_clic ked " + str ( i ) ) )
buttUSB . append ( chrt )

buttTest = list ()
for i in range (0 , 5) :
chrt = Gtk . Button ( label = " Test " + str ( i ) )
chrt . connect (" clicked " , getattr ( self , " on _t es t _c li ck e d " + str ( i ) ) )
buttTest . append ( chrt )

buttApply = Gtk . Button ( label = " Apply changes " , use_underline = True )
buttApply . connect (" clicked " , self . o n _ a p p l y _ c l i ck e d )

buttClose = Gtk . Button (" _Close " , use_underline = True )

buttClose . connect (" clicked " , self . o n _ c l o s e _ c l i ck e d )

120
 buttSearch = Gtk . Button (" Search for attached devices " , use_underline = True )
buttSearch . connect (" clicked " , self . o n _ s e a r c h _ c l i c k e d )

serialBoot . loadConfig () # initializes ram positions in the serialBoot . py file

self . usbPort = list ()

for i in range (0 , 5) :
chrt = Gtk . Entry ()
text = serialBoot . callChangeUSB ( str ( i ) + ’ r ’) # reads stored ports
chrt . set_text ( text )
self . usbPort . append ( chrt )

self . usbAddr = list ()

for i in range (0 , 5) :
chrt = Gtk . Entry ()
text = serialBoot . c a l lC h a n g e A d d r e s s ( str ( i ) + ’ r ’) # reads stored addresses
chrt . set_text ( text )
self . usbAddr . append ( chrt )

self . usbShow = Gtk . TextView ()

self . textBuffer = self . usbShow . get_buffer ()
self . textBuffer . set_text ( ’ ’)

# let ’ s a s s i g n l o c a t i o n i n t o t h e GUI

t a b l e . a t t a c h ( labelUSB , 0 , 1 , 0 , 1 )
t a b l e . attach ( labelAddr , 3 , 6 , 0 , 1)

for i in range (0 , 5) :
t a b l e . a t t a c h ( s e l f . usbPort [ i ] , 0 , 1 , i +1 , i +2)

for i in range (0 , 5) :
t a b l e . a t t a c h ( buttUSB [ i ] , 1 , 2 , i +1 , i +2)

for i in range (0 , 5) :
t a b l e . a t t a c h ( b u t t T e s t [ i ] , 2 , 3 , i +1 , i +2)

for i in range (0 , 5) :
t a b l e . a t t a c h ( s e l f . usbAddr [ i ] , 3 , 6 , i +1 , i +2)

for i in range (0 , 5) :
t a b l e . a t t a c h ( b u t t F i l e [ i ] , 6 , 7 , i +1 , i +2)

t a b l e . a t t a c h ( s e l f . usbShow , 0 , 3 , 6 , 1 2 )
t a b l e . attach ( buttSearch , 3 , 5 , 6 , 8)

t a b l e . a t t a c h ( buttApply , 6 , 7 , 8 , 1 0 )
t a b l e . attach ( buttClose , 6 , 7 , 10 , 12)

t a b l e . a t t a c h ( labelName1 , 4 , 6 , 1 0 , 1 1 )
t a b l e . a t t a c h ( labelName2 , 4 , 6 , 1 1 , 1 2 )

def o n f i l e c l i c k e d 0 ( s e l f , widget ) :
s e l f . o n f i l e c l i c k e d (0)

def o n f i l e c l i c k e d 1 ( s e l f , widget ) :
s e l f . o n f i l e c l i c k e d (1)

def o n f i l e c l i c k e d 2 ( s e l f , widget ) :
s e l f . o n f i l e c l i c k e d (2)

def o n f i l e c l i c k e d 3 ( s e l f , widget ) :
s e l f . o n f i l e c l i c k e d (3)

121
def o n f i l e c l i c k e d 4 ( s e l f , widget ) :
s e l f . o n f i l e c l i c k e d (4)

def on USB clicked0 ( s e l f , widget ) :

s e l f . on USB clicked (0)

def on USB clicked1 ( s e l f , widget ) :

s e l f . on USB clicked (1)

def on USB clicked2 ( s e l f , widget ) :

s e l f . on USB clicked (2)

def on USB clicked3 ( s e l f , widget ) :

s e l f . on USB clicked (3)

def on USB clicked4 ( s e l f , widget ) :

s e l f . on USB clicked (4)

d e f o n c l o s e c l i c k e d ( s e l f , bu tt on ) :
Gtk . m a i n q u i t ( )

def o n t e s t c l i c k e d 0 ( s e l f , widget ) :
s e l f . on test clicked (0)

def o n t e s t c l i c k e d 1 ( s e l f , widget ) :
s e l f . on test clicked (1)

def o n t e s t c l i c k e d 2 ( s e l f , widget ) :
s e l f . on test clicked (2)

def o n t e s t c l i c k e d 3 ( s e l f , widget ) :
s e l f . on test clicked (3)

def o n t e s t c l i c k e d 4 ( s e l f , widget ) :
s e l f . on test clicked (4)

d e f o n a p p l y c l i c k e d ( s e l f , b ut to n ) :

s e r i a l B o o t . l o a d C o n f i g ( ) #i n i t i a l i z e s ram p o s i t i o n s i n t h e s e r i a l B o o t . py f i l e

for i in range (0 , 5) :
t e x t = s e l f . usbPort [ i ] . g e t t e x t ( )
a = s e r i a l B o o t . callChangeUSB ( s t r ( i ) + ’w ’ + t e x t ) #w r i t e s new USB p o r t s
i f a == ’ ’ : #f u n c t i o n r e t u r n s empty s p a c e i f e r r o r
s e l f . usbPort [ i ] . s e t t e x t ( ’ USBportA ddress ’ )

for i in range (0 , 5) :
t e x t = s e l f . usbAddr [ i ] . g e t t e x t ( )
a = s e r i a l B o o t . c a l l C h a n g e A d d r e s s ( s t r ( i ) + ’w ’ + t e x t ) #w r i t e s new e l f file
addresses
i f a == ’ ’ : #f u n c t i o n r e t u r n s empty s p a c e i f e r r o r
s e l f . usbAddr [ i ] . s e t t e x t ( ’ N a m e O f F i l e T o P r o g r a m . elf ’ )

def o n f i l e c l i c k e d ( s e l f , device ) :

d i a l o g F i l e = Gtk . F i l e C h o o s e r D i a l o g ( " Select an *. elf file " , s e l f ,

Gtk . F i l e C h o o s e r A c t i o n .OPEN, ( Gtk .STOCK CANCEL, Gtk . ResponseType .CANCEL,
Gtk . STOCK OPEN, Gtk . ResponseType .OK) )

self . add filtersFile ( dialogFile )

r e s p o n s e = d i a l o g F i l e . run ( )
i f r e s p o n s e == Gtk . ResponseType .OK:
p r i n t ( " File selected : " + d i a l o g F i l e . g e t f i l e n a m e ( ) )

122
 s e l f . usbAddr [ d e v i c e ] . s e t t e x t ( d i a l o g F i l e . g e t f i l e n a m e ( ) )

dialogFile . destroy ()

def on USB clicked ( s e l f , device ) :

dialogUSB = Gtk . F i l e C h o o s e r D i a l o g ( " Select an available port " , s e l f ,

Gtk . F i l e C h o o s e r A c t i o n .OPEN, ( Gtk .STOCK CANCEL, Gtk . ResponseType .CANCEL,
Gtk . STOCK OPEN, Gtk . ResponseType .OK) )

s e l f . a d d f i l t e r s U S B ( dialogUSB )

r e s p o n s e = dialogUSB . run ( )
i f r e s p o n s e == Gtk . ResponseType .OK:
p r i n t ( " File selected : " + dialogUSB . g e t f i l e n a m e ( ) )
s e l f . usbPort [ d e v i c e ] . s e t t e x t ( dialogUSB . g e t f i l e n a m e ( ) )

dialogUSB . d e s t r o y ( )

def o n t e s t c l i c k e d ( s e l f , device ) :

s e r i a l B o o t . l o a d C o n f i g ( ) #i n i t i a l i z e s ram p o s i t i o n s i n t h e s e r i a l B o o t . py f i l e
s e r i a l B o o t . c a l l B l i n k L e d ( s t r ( d e v i c e ) ) #w r i t e s new USB p o r t s

def on search clicked ( s e l f , device ) :

s e r i a l B o o t . l o a d C o n f i g ( ) #i n i t i a l i z e s ram p o s i t i o n s i n t h e s e r i a l B o o t . py f i l e
u s b D e v i c e s = s e r i a l B o o t . scanUSB ( )

i f l e n ( u s b D e v i c e s ) != 0 :
t e x t = ""
for i in range (0 , l e n ( usbDevices ) ) :
t e x t = t e x t + usbDevices [ i ] + "\r\n"
s e l f . textBuffer . set text ( text )
else :
s e l f . t e x t B u f f e r . s e t t e x t ( ’ Has not been found any attached device ! ’ )

def a d d f i l t e r s F i l e ( s e l f , dialogFile ) :

filter elf = Gtk . F i l e F i l t e r ( )

filter elf . s e t n a m e ( " *. elf files " )
filter elf . a d d p a t t e r n ( " *. elf " )
dialogFile . add filter ( f i l t e r e l f )

filter hex = Gtk . F i l e F i l t e r ( )

filter hex . s e t n a m e ( " *. hex files " )
filter hex . a d d p a t t e r n ( " *. hex " )
dialogFile . add filter ( filter hex )

filter any = Gtk . F i l e F i l t e r ( )

filter any . s e t n a m e ( " Any files " )
filter any . add pattern ( "*" )
dialogFile . add filter ( filter any )

d e f a d d f i l t e r s U S B ( s e l f , dialogUSB ) :

f i l t e r a c m = Gtk . F i l e F i l t e r ( )
f i l t e r a c m . s e t n a m e ( " ttyACM " )
for i in range (0 , 256) :
f i l t e r a c m . a d d p a t t e r n ( " ttyACM " + s t r ( i ) )
dialogUSB . a d d f i l t e r ( f i l t e r a c m )

f i l t e r u s b = Gtk . F i l e F i l t e r ( )
f i l t e r u s b . s e t n a m e ( " ttyUSB " )

123
 for i in range (0 , 256) :
f i l t e r u s b . a d d p a t t e r n ( " ttyUSB " + s t r ( i ) )
dialogUSB . a d d f i l t e r ( f i l t e r u s b )

filter any = Gtk . F i l e F i l t e r ( )

filter any . s e t n a m e ( " Any files " )
filter any . add pattern ( "*" )
dialogUSB . add filter ( filter any )

if name == " __main__ " :

i n i t F i l e ()
win = f i r s t W i n d o w ( )
win . c o n n e c t ( " delete - event " , Gtk . m a i n q u i t )
win . s h o w a l l ( )
Gtk . main ( )

A.1.3 Reset GUI (reset.py)

Listing A.3: Reset-GUI file

#! / u s r / b i n / env python
from g i . r e p o s i t o r y im por t Gtk
im po rt o s
im po rt s e r i a l B o o t

#f i r s t c o n f i g u r a t i o n s initialization
def i n i t F i l e () :

s e l f N a m e = o s . path . basename ( file ) #s c r i p t ’s self location

selfPath = os . path . realpath ( __file__ ) # script ’ s s e l f name

i f s e l f P a t h . e n d s w i t h ( s e l f N a m e ) : #removes s c r i p t ’s name from path

selfPath = selfPath [: - len ( selfName ) ]

os . chdir ( selfPath ) # sets current directory in order to access the config file
# os . system ("./ serialBoot . py -h ")

class firstWindow ( Gtk . Window ) :

def __init__ ( self ) :

Gtk . Window . __init__ ( self , title =" Evidence Flex Board - Bootloader Application -
Version 1.1")
self . s e t _ b o r d e r _ w i d t h (20)

grid = Gtk . Grid ()

self . add ( grid )

labelChoose = Gtk . Label ()

labelChoose . set_text (" Choose the device to reset ")

buttReset = list ()
for i in range (0 , 5) :
chrt = Gtk . Button ( label = str ( i ) )

124
 chrt . connect (" clicked " , getattr ( self , " o n _ r e s e t _ c l i ck e d " + str ( i ) ) )
buttReset . append ( chrt )

self . buttClose = Gtk . Button ("\ r \ nClose \ r \ n " , use_underline = True )

self . buttClose . connect (" clicked " , self . o n _ c l o se _ c l i c k e d )

# let ’ s a s s i g n l o c a t i o n i n t o t h e GUI

grid . attach ( labelChoose , 0 , 0 , 5 , 1)

g r i d . attach ( buttReset [ 0 ] , 0 , 1 , 1 , 2)
for i in range (1 , 5) :
g r i d . a t t a c h n e x t t o ( b u t t R e s e t [ i ] , b u t t R e s e t [ i − 1 ] , Gtk . P o s i t i o n T y p e . RIGHT, 1 ,
2)

g r i d . attach ( s e l f . buttClose , 0 , 3 , 5 , 1)

def o n r e s e t c l i c k e d 0 ( s e l f , widget ) :
s e l f . on reset clicked (0)

def o n r e s e t c l i c k e d 1 ( s e l f , widget ) :
s e l f . on reset clicked (1)

def o n r e s e t c l i c k e d 2 ( s e l f , widget ) :
s e l f . on reset clicked (2)

def o n r e s e t c l i c k e d 3 ( s e l f , widget ) :
s e l f . on reset clicked (3)

def o n r e s e t c l i c k e d 4 ( s e l f , widget ) :
s e l f . on reset clicked (4)

d e f o n c l o s e c l i c k e d ( s e l f , b ut ton ) :
Gtk . m a i n q u i t ( )

def o n r e s e t c l i c k e d ( s e l f , device ) :
s e r i a l B o o t . l o a d C o n f i g ( ) #i n i t i a l i z e s ram p o s i t i o n s i n t h e s e r i a l B o o t . py f i l e
serialBoot . callResetSys ( str ( device ) )
s e r i a l B o o t . callRunProg ( s t r ( d e v i c e ) )

if name == " __main__ " :

i n i t F i l e ()
win = f i r s t W i n d o w ( )
win . c o n n e c t ( " delete - event " , Gtk . m a i n q u i t )
win . s h o w a l l ( )
Gtk . main ( )

A.1.4 Erase GUI (erase.py)

Listing A.4: Erase-GUI file

#! / u s r / b i n / env python
from g i . r e p o s i t o r y im por t Gtk
im por t o s
im por t s e r i a l B o o t

125
#f i r s t c o n f i g u r a t i o n s initialization
def i n i t F i l e () :

s e l f N a m e = o s . path . basename ( file ) #s c r i p t ’s self location

selfPath = os . path . realpath ( __file__ ) # script ’ s s e l f name

i f s e l f P a t h . e n d s w i t h ( s e l f N a m e ) : #removes s c r i p t ’s name from path

selfPath = selfPath [: - len ( selfName ) ]

os . chdir ( selfPath ) # sets current directory in order to access the config file
# os . system ("./ serialBoot . py -h ")

class firstWindow ( Gtk . Window ) :

def __init__ ( self ) :

Gtk . Window . __init__ ( self , title =" Evidence Flex Board - Bootloader Application -
Version 1.1")
self . s e t _ b o r d e r _ w i d t h (20)

grid = Gtk . Grid ()

self . add ( grid )

labelChoose = Gtk . Label ()

labelChoose . set_text (" Choose the device in which the memory will be erased ")

buttErase = list ()
for i in range (0 , 5) :
chrt = Gtk . Button ( label = str ( i ) )
chrt . connect (" clicked " , getattr ( self , " o n _ e r a s e _ c l i c k ed " + str ( i ) ) )
buttErase . append ( chrt )

self . buttClose = Gtk . Button ("\ r \ nClose \ r \ n " , use_underline = True )

self . buttClose . connect (" clicked " , self . o n _ c l os e _ c l i c k e d )

# let ’ s a s s i g n l o c a t i o n i n t o t h e GUI

g r i d . attach ( labelChoose , 0 , 0 , 5 , 1)

g r i d . attach ( buttErase [ 0 ] , 0 , 1 , 1 , 2)
for i in range (1 , 5) :
g r i d . a t t a c h n e x t t o ( b u t t E r a s e [ i ] , b u t t E r a s e [ i − 1 ] , Gtk . P o s i t i o n T y p e . RIGHT, 1 ,
2)

g r i d . attach ( s e l f . buttClose , 0 , 3 , 5 , 1)

def o n e r a s e c l i c k e d 0 ( s e l f , widget ) :
s e l f . buttClose . hide ()
s e l f . on erase clicked (0)
Gtk . m a i n q u i t ( )

def o n e r a s e c l i c k e d 1 ( s e l f , widget ) :
s e l f . buttClose . hide ()
s e l f . on erase clicked (1)
Gtk . m a i n q u i t ( )

def o n e r a s e c l i c k e d 2 ( s e l f , widget ) :
s e l f . buttClose . hide ()
s e l f . on erase clicked (2)
Gtk . m a i n q u i t ( )

def o n e r a s e c l i c k e d 3 ( s e l f , widget ) :

126
 s e l f . buttClose . hide ()
s e l f . on erase clicked (3)
Gtk . m a i n q u i t ( )

def o n e r a s e c l i c k e d 4 ( s e l f , widget ) :
s e l f . buttClose . hide ()
s e l f . on erase clicked (4)
Gtk . m a i n q u i t ( )

d e f o n c l o s e c l i c k e d ( s e l f , b ut ton ) :
Gtk . m a i n q u i t ( )

def on erase clicked ( s e l f , device ) :

s e r i a l B o o t . l o a d C o n f i g ( ) #i n i t i a l i z e s ram p o s i t i o n s i n t h e s e r i a l B o o t . py f i l e
serialBoot . callResetSys ( str ( device ) )
s e r i a l B o o t . c a l l E r a s e ( s t r ( d e v i c e ) + ’a ’ ) #e r a s e s t h e whole d e v i c e ’s memory

if __name__ == " __main__ ":

initFile ()
win = firstWindow ()
win . connect (" delete - event " , Gtk . main_quit )
win . show_all ()
Gtk . main ()

A.1.5 Linker file (linker.sh)

Listing A.5: Linker file

#! / b i n / bash
#L i n k e r f i l e , between e c l i p s e IDE and python s c r i p t s t o a c c e s s t h e b o o t l o a d e r s t r u c t u r e
#Developed by C a r l o s X. Rosero & Manel V e l a s c o G.

#i t t a k e s t h e c u r r e n t path o f f i l e s
SOURCE=" $ { BASH_SOURCE [0]} "
while [ −h " $SOURCE " ] ; do # r e s o l v e $SOURCE u n t i l t h e f i l e i s no l o n g e r a s y m l i n k
c u r r e n t D i r=" $ ( cd -P " $ ( dirname " $SOURCE " ) " && pwd ) "
SOURCE=" $ ( readlink "$SOURCE" ) "
[ [ $SOURCE != /∗ ] ] && SOURCE=”$ c u r r e n t D i r /$SOURCE” # i f $SOURCE was a r e l a t i v e s y m l i n k
, we need t o r e s o l v e i t r e l a t i v e t o t h e p a t h where t h e s y m l i n k f i l e was l o c a t e d
done
c u r r e n t D i r=”$ ( cd −P ”$ ( dirname ”$SOURCE” ) ” && pwd ) ”

#”$1 ” i s t h e f i r s t argument t h a t i s comming from t h e c a l l i n g t o t h e f i l e

#i n e c l i p s e
#echo $1
#echo $2 #s ec o nd argument , i s t h e s u p e r u s e r keyword

#i t v e r i f i e s i f t h e argument t h a t i s comming v a l u e s s
echo ” L i n k i n g w i t h p y t ho n s c r i p t s . . . ”

i f [ $1 == s ] #i f s e t t i n g s r e q u i r e d
then
echo ” S e t t i n g t o o l a t work . . . ”
echo ” $2 ” | sudo / u s r / b i n / p y t ho n ” $ c u r r e n t D i r ”/ s e t t i n g s . py

127
 else
i f [ $1 == e ] #i f e r a s i n g r e q u i r e d
then
echo ” E r a s i n g t o o l a t work . . . ”
echo ” $2 ” | sudo / u s r / b i n / p y t ho n ” $ c u r r e n t D i r ”/ e r a s e . py
e l s e #programming r e q u i r e d , v a l u e s b e t w e e n 0 t o 4
i f [ $1 == r ] #i f r e s e t t i n g r e q u i r e d

then
echo ” R e s e t t i n g t o o l a t work . . . ”
echo ” $2 ” | sudo / u s r / b i n / p y t ho n ” $ c u r r e n t D i r ”/ r e s e t . py
else
echo ”Programming t o o l a t work . . . ”
echo ” $2 ” | sudo / u s r / b i n / p y t ho n ” $ c u r r e n t D i r ”/ s e r i a l B o o t . py −s ” $1 ” −p ” $1 ” f
−r ” $1 ”
fi
fi
fi

#echo ”Done ! ”

A.2 Bootloader firmware

A.2.1 DSC main code (main.c)

Listing A.6: DSC main code

//#i n c l u d e ”p33FJ256MC710 . h”
#include " p33Fxxxx . h "
#define COMMANDNACK 0 xff
#define COMMANDACK 0 x01
#define COMMAND READ PM 0 x02
#define COMMAND WRITE PM 0 x03
#define COMMAND WRITE CM 0 x07
#define COMMAND RUN 0 x08
#define COMMAND READ ID 0 x09
#define COMMAND ERASE MEM 0 x0a
#define COMMAND BLINK LED 0 x0b

#define PM ROW SIZE 64∗8

#define CM ROW SIZE 8
#define CONFIG WORD SIZE 1

#define PM ROW ERASE 0 x4042

#define PM ROW WRITE 0 x4001
#define CONFIG WORD WRITE 0X4000
#define yellowLed LATBbits . LATB14

#define FCY 20000000

#define BRGVAL 10 // ( (FCY/BAUDRATE) /16)−1
#define d e l a y B e g i n 2 // r e s e t a f t e r r e b o o t i n s e c o n d s

#include < l i b p i c 3 0 . h>

128
// E x t e r n a l O s c i l l a t o r

FOSCSEL(FNOSC PRIPLL) ;
// C l o c k S w i t c h i n g and F a i l S a f e C l o c k Monitor i s d i s a b l e d
// Primary (XT, HS, EC) O s c i l l a t o r w i t h PLL
FOSC (OSCIOFNC OFF & POSCMD XT) ;
// OSC2 Pin Function : OSC2 i s C l o c k Output
// Primary O s c i l l a t o r Mode : XT C r y s t a l

FWDT(FWDTEN OFF) ; // Watchdog Timer Enabled / d i s a b l e d by u s e r s o f t w a r e

// (LPRC can be d i s a b l e d by c l e a r i n g SWDTEN b i t i n
RCON r e g i s t e r
// FPOR(PWRTEN OFF) ; // Turn o f f t h e power−up t i m e r s .
FGS (GCP OFF) ; // D i s a b l e Code P r o t e c t i o n

typedef short Word16 ;

typedef unsigned short UWord16 ;
typedef long Word32 ;
typedef unsigned long UWord32 ;

typedef union tuReg32

{
UWord32 Val32 ;

struct
{
UWord16 LW;
UWord16 HW;
}Word ;

char Val [ 4 ] ;
} uReg32 ;

extern UWord32 ReadLatch ( UWord16 , UWord16 ) ;

void PutChar ( char ) ;
void GetChar ( char ∗ ) ;
void W r i t e B u f f e r ( char ∗ , i n t ) ;
void ReadPM( char ∗ , uReg32 ) ;
void WritePM ( char ∗ , uReg32 ) ;
void C o n f i g U a r t ( void ) ;
void ConfigTimer ( void ) ;

char B u f f e r [ PM ROW SIZE∗3 + 1 ] ;

i n t main ( void )
{
// C o n f i g u r e O s c i l l a t o r t o o p e r a t e t h e d e v i c e a t 40Mhz= 20MIPS
// Fosc= Fin ∗M/(N1∗N2) , Fcy=Fosc /2
// Fosc= 4M∗ 4 0 ( 2 ∗ 2 ) =40Mhz f o r 4M i n p u t c l o c k

PLLFBD = 4 0 ; // M=40
CLKDIVbits . PLLPOST = 0 ; // N1=2
CLKDIVbits . PLLPRE = 0 ; // N2=2
OSCTUN=0; // Tune FRC o s c i l l a t o r , i f FRC i s used

RCONbits .SWDTEN = 0 ; /∗ D i s a b l e Watch Dog Timer ∗/

while ( OSCCONbits .LOCK != 1 ) { } ; /∗ Wait f o r PLL t o l o c k ∗/

ConfigTimer ( ) ;
ConfigUart ( ) ;

TRISBbits . TRISB14 = 0 ; // c o n f i g u r e p i n as o u t p u t
y e l l o w L e d = 1 ; // t u r n s on y e l l o w l e d

129
while ( 1 )
{
char Command ;

GetChar(&Command) ;

switch (Command)
{
case COMMAND READ PM:
{
uReg32 SourceAddr ;

GetChar (&( SourceAddr . Val [ 0 ] ) ) ;

GetChar (&( SourceAddr . Val [ 1 ] ) ) ;
GetChar (&( SourceAddr . Val [ 2 ] ) ) ;
SourceAddr . Val [ 3 ] = 0 ;
ReadPM( B u f f e r , SourceAddr ) ;
W r i t e B u f f e r ( B u f f e r , PM ROW SIZE∗ 3 ) ;
break ;
}

case COMMAND ERASE MEM:

{
uReg32 SourceAddr ;
i n t addr ;
char temp ;

GetChar (&( SourceAddr . Val [ 0 ] ) ) ;

GetChar (&( SourceAddr . Val [ 1 ] ) ) ;
GetChar (&( SourceAddr . Val [ 2 ] ) ) ;
SourceAddr . Val [ 3 ] = 0 ;

i f ( ( SourceAddr . Word .HW == 0 && SourceAddr . Word .LW == 0 x400 ) | |

( SourceAddr . Word .HW == 0 && SourceAddr . Word .LW == 0 x800 ) )
{
/∗ I t i s n o t a l l o w e d t o e r a s e p a g e s from 0 x400 and from 0 x800
( b o o t l o a d e r ’ s p l a c e ) ∗/
PutChar (COMMANDNACK) ; /∗ Send e r r o r f l a g ∗/
}
else
{
i f ( SourceAddr . Word .HW == 0 && SourceAddr . Word .LW == 0 )
{
/∗When t r y i n g t o d e l e t e page 0 , b o o t l o a d e r ’ s r e s e t v e c t o r
o v e r l i e s , t h e r e s t o f memory i s e r a s e d ∗/
ReadPM( B u f f e r , SourceAddr ) ; // r e a d s program memory a t page 0

/∗ c h a n g e s o r d e r ( o r d e r i n d a t a b u f f e r i s i n v e r t e d than a c t u a l ) ∗/
temp = B u f f e r [ 0 ] ;
Buffer [ 0 ] = Buffer [ 2 ] ;
B u f f e r [ 2 ] = temp ;

temp = B u f f e r [ 3 ] ;
Buffer [ 3 ] = Buffer [ 5 ] ;
B u f f e r [ 5 ] = temp ;

/∗ p r e s e r v e s b o o t l o a d e r ’ s r e s e t v e c t o r ( f i r s t 6 l o c a t i o n s ) ∗/

f o r ( addr = 6 ; addr < PM ROW SIZE∗ 3 ; addr++)

{
B u f f e r [ addr ] = 0 x f f ;
}

130
 E r a s e ( SourceAddr . Word .HW, SourceAddr . Word .LW, PM ROW ERASE) ;
WritePM ( B u f f e r , SourceAddr ) ; /∗ reprogram page 0 ∗/
}
else
{
E r a s e ( SourceAddr . Word .HW, SourceAddr . Word .LW, PM ROW ERASE) ;
}
PutChar (COMMANDACK) ; /∗ s e n d s Acknowledgment ∗/
}

break ;
}

case COMMAND WRITE PM: /∗ t e s t e d ∗/

{
uReg32 SourceAddr ;
i n t addr ;
char B u f f e r 1 [ PM ROW SIZE∗3 + 1 ] ;

GetChar (&( SourceAddr . Val [ 0 ] ) ) ;

GetChar (&( SourceAddr . Val [ 1 ] ) ) ;
GetChar (&( SourceAddr . Val [ 2 ] ) ) ;
SourceAddr . Val [ 3 ] = 0 ;

f o r ( addr = 0 ; addr < PM ROW SIZE∗ 3 ; addr++)

{
GetChar (&( B u f f e r [ addr ] ) ) ; /∗ t a k e s d a t a from u a r t b u f f e r ∗/
}

i f ( ( SourceAddr . Word .HW == 0 && SourceAddr . Word .LW == 0 x400 ) | |

( SourceAddr . Word .HW == 0 && SourceAddr . Word .LW == 0 x800 ) )
{
/∗ I t i s n o t a l l o w e d t o w r i t e t o p a g e s from 0 x400 and from 0 x800
( b o o t l o a d e r ’ s p l a c e ) ∗/
PutChar (COMMANDNACK) ; /∗ s e n d s e r r o r f l a g ∗/
}
else
{
/∗ here , g o e s i n s i d e o n l y i f i t t r i e s t o a c c e s s t h e page 0 ∗/
i f ( SourceAddr . Word .HW == 0 && SourceAddr . Word .LW == 0 )
{
ReadPM( B u f f e r 1 , SourceAddr ) ; // r e a d s program memory a t page 0

/∗ p r e s e r v e s b o o t l o a d e r ’ s r e s e t v e c t o r ( f i r s t 6 l o c a t i o n s ) ∗/
/∗ c h a n g e s o r d e r ( o r d e r i n d a t a b u f f e r i s i n v e r t e d than a c t u a l ) ∗/
Buffer [ 0 ] = Buffer1 [ 2 ] ;
Buffer [ 1 ] = Buffer1 [ 1 ] ;
Buffer [ 2 ] = Buffer1 [ 0 ] ;
Buffer [ 3 ] = Buffer1 [ 5 ] ;
Buffer [ 4 ] = Buffer1 [ 4 ] ;
Buffer [ 5 ] = Buffer1 [ 3 ] ;
}

E r a s e ( SourceAddr . Word .HW, SourceAddr . Word .LW, PM ROW ERASE) ;

WritePM ( B u f f e r , SourceAddr ) ; /∗ program page ∗/
PutChar (COMMANDACK) ; /∗ Send Acknowledgment ∗/
}
break ;
}

case COMMAND READ ID: /∗ t e s t e d ∗/

{
uReg32 SourceAddr ;
uReg32 Temp ;

131
 SourceAddr . Val32 = 0 xFF0000 ;
Temp . Val32 = ReadLatch ( SourceAddr . Word .HW, SourceAddr . Word .LW) ;
W r i t e B u f f e r (&(Temp . Val [ 0 ] ) , 4 ) ;
SourceAddr . Val32 = 0 xFF0002 ;
Temp . Val32 = ReadLatch ( SourceAddr . Word .HW, SourceAddr . Word .LW) ;
W r i t e B u f f e r (&(Temp . Val [ 0 ] ) , 4 ) ;
break ;
}

case COMMAND WRITE CM: /∗ This f u n c t i o n has n o t been implemented ∗/

{
int S i z e ;
f o r ( S i z e = 0 ; S i z e < CM ROW SIZE ∗ 3 ; )
{
GetChar (&( B u f f e r [ S i z e ++]) ) ;
GetChar (&( B u f f e r [ S i z e ++]) ) ;
GetChar (&( B u f f e r [ S i z e ++]) ) ;
PutChar (COMMANDACK) ; /∗ Send Acknowledgment ∗/
}
break ;
}

/∗ c a s e COMMAND RESET:
{
uReg32 SourceAddr ;
int Size ;
uReg32 Temp ;

f o r ( S i z e = 0 , SourceAddr . Val32 = 0 xF80000 ; S i z e < CM ROW SIZE∗ 3 ;

S i z e +=3, SourceAddr . Val32 += 2)
{
i f ( B u f f e r [ S i z e ] == 0)
{
Temp . Val [ 0 ] = B u f f e r [ S i z e + 1 ] ;
Temp . Val [ 1 ] = B u f f e r [ S i z e + 2 ] ;
WriteLatch ( SourceAddr . Word .HW, SourceAddr . Word .LW,
Temp . Word .HW, Temp . Word .LW) ;
WriteMem (CONFIG WORD WRITE) ;
}
}

RunProg ( ) ;
break ;
} ∗/

case COMMAND RUN:

{
yellowLed = 0 ;
PutChar (COMMANDACK) ;
RunProg ( ) ;
break ;
}

case COMMAND BLINK LED:

{
unsigned char i , j ;
PutChar (COMMANDACK) ; //command r e c e i v e d

yellowLed = 0 ;
delay ms (200) ;

f o r ( j = 0 ; j < 2 ; j ++)
{

132
 f o r ( i = 0 ; i < 6 ; i ++)
{
yellowLed = ˜ yellowLed ;
delay ms (100) ;
}

f o r ( i = 0 ; i < 6 ; i ++)
{
yellowLed = ˜ yellowLed ;
delay ms (300) ;
}
}
delay ms (200) ;
yellowLed = 1 ;

break ;
}

default :
PutChar (COMMANDNACK) ;
break ;
}
}
}

void GetChar ( char ∗ ptrChar )

{
while ( 1 )
{
/∗ i f t i m e r e x p i r e d , s i g n a l t o a p p l i c a t i o n t o jump t o u s e r code ∗/
i f ( I F S 0 b i t s . T3IF == 1 )
{
∗ ptrChar = COMMAND RUN;
break ;
}

/∗ c h e c k f o r r e c e i v e e r r o r s ∗/
i f ( U1STAbits .FERR == 1 )
{
continue ;
}

/∗ must c l e a r t h e o v e r r u n e r r o r t o k e e p u a r t r e c e i v i n g ∗/
i f ( U1STAbits .OERR == 1 )
{
U1STAbits .OERR = 0 ;
continue ;
}

/∗ g e t t h e d a t a ∗/
i f ( U1STAbits .URXDA == 1 )
{
T2CONbits .TON = 0 ; /∗ D i s a b l e t i m e r countdown ∗/
∗ ptrChar = U1RXREG;
break ;
}
}
}

void ReadPM( char ∗ ptrData , uReg32 SourceAddr )

{
int Size ;
uReg32 Temp ;

133
 f o r ( S i z e = 0 ; S i z e < PM ROW SIZE ; S i z e ++)
{
Temp . Val32 = ReadLatch ( SourceAddr . Word .HW, SourceAddr . Word .LW) ;

ptrData [ 0 ] = Temp . Val [ 2 ] ; ;

ptrData [ 1 ] = Temp . Val [ 1 ] ; ;
ptrData [ 2 ] = Temp . Val [ 0 ] ; ;

ptrData = ptrData + 3 ;

SourceAddr . Val32 = SourceAddr . Val32 + 2 ;

}
}

void W r i t e B u f f e r ( char ∗ ptrData , i n t S i z e )

{
i n t DataCount ;

f o r ( DataCount = 0 ; DataCount < S i z e ; DataCount++)

{
PutChar ( ptrData [ DataCount ] ) ;
}
}

void PutChar ( char Char )

{
while ( ! U1STAbits .TRMT) ;
U1TXREG = Char ;
}

void WritePM ( char ∗ ptrData , uReg32 SourceAddr )

{
int Size , Size1 ;
uReg32 Temp ;
uReg32 TempAddr ;
uReg32 TempData ;

f o r ( S i z e = 0 , S i z e 1 =0; S i z e < PM ROW SIZE ; S i z e ++)

{
Temp . Val [ 0 ] = ptrData [ S i z e 1 + 0 ] ;
Temp . Val [ 1 ] = ptrData [ S i z e 1 + 1 ] ;
Temp . Val [ 2 ] = ptrData [ S i z e 1 + 2 ] ;
Temp . Val [ 3 ] = 0 ;
S i z e 1 += 3 ;

WriteLatch ( SourceAddr . Word .HW, SourceAddr . Word .LW, Temp . Word .HW, Temp . Word .LW) ;

/∗ D e v i c e ID e r r a t a workaround : Save d a t a a t any a d d r e s s t h a t has LSB 0 x18 ∗/

i f ( ( SourceAddr . Val32 & 0 x0000001F ) == 0 x18 )
{
TempAddr . Val32 = SourceAddr . Val32 ;
TempData . Val32 = Temp . Val32 ;
}

i f ( ( S i z e !=0) && ( ( ( S i z e + 1 ) % 6 4 ) == 0 ) )
{
/∗ D e v i c e ID e r r a t a workaround : Reload d a t a a t a d d r e s s w i t h LSB o f 0 x18 ∗/
WriteLatch ( TempAddr . Word .HW, TempAddr . Word .LW, TempData . Word .HW, TempData .
Word .LW) ;
WriteMem (PM ROW WRITE) ;
}
SourceAddr . Val32 = SourceAddr . Val32 + 2 ;
}
}

134
void C o n f i g U a r t ( void )
{
U1MODEbits . STSEL = 0 ; //1− s t o p b i t
U1MODEbits . PDSEL = 0 ; //No P a r i t y , 8−d a t a b i t s
U1MODEbits .ABAUD = 0 ; // Autobaud D i s a b l e d

U1MODEbits .BRGH = 0 ; // 1 = BRG g e n e r a t e s 4 c l o c k s p e r b i t p e r i o d (4 x baud c l o c k ,

// High−Speed mode )
// 0 = BRG g e n e r a t e s 16 c l o c k s p e r b i t p e r i o d (16 x baud c l o c k ,
// Standard mode )

U1BRG = BRGVAL; //BRGVAL=((FCY/BITRATE1) /16)−1 // With BRGH=0−−>Slow B i t r a t e

//BRGVAL=((FCY/BITRATE1) /4)−1 // With BRGH=1−−>F a s t B i t r a t e
// Enable UART Rx and Tx
U1MODEbits .UARTEN = 1 ; // Enable UART
U1STAbits .UTXEN = 1 ; // Enable UART Tx
}

void ConfigTimer ( void )

{
uReg32 d e l a y ;
T2CONbits . T32 = 1 ; /∗ t o i n c r e m e n t e v e r y i n s t r u c t i o n c y c l e ∗/
I F S 0 b i t s . T3IF = 0 ; /∗ C l e a r t h e Timer3 I n t e r r u p t F l a g ∗/
I E C 0 b i t s . T3IE = 0 ; /∗ D i s a b l e Timer3 I n t e r r u p t S e r v i c e Routine ∗/

/∗ Convert s e c o n d s i n t o t i m e r c o u n t v a l u e ∗/
d e l a y . Val32 = ( ( UWord32 ) (FCY) ) ∗ ( ( UWord32 ) ( d e l a y B e g i n ) ) ;
PR3 = d e l a y . Word .HW;
PR2 = d e l a y . Word .LW;

/∗ Enable Timer ∗/
T2CONbits .TON = 1 ;
}

A.2.2 DSC atomic subroutines (memory.s)

Listing A.7: DSC atomic subroutines

; . i n c l u d e " p33FJ256MC710 . inc "
. i n c l u d e " p33Fxxxx . inc "

. global LoadAddr , WriteMem , WriteLatch , ReadLatch , RunProg , E r a s e ; C c a l l e d

LoadAddr : ;W0=NVMADRU,W1=NVMADR − no return v a l u e s

mov W0,TBLPAG
mov W1,W1

return

;∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
WriteMem : ;W0=NVMCON − no return v a l u e s
mov W0,NVMCON
mov #0x55 ,W0 ; Unlock s e q u e n c e − i n t e r r u p t s need t o be o f f
mov W0,NVMKEY
mov #0xAA,W0

135
 mov W0,NVMKEY
b s e t NVMCON,#WR
nop ; Required
nop
1: b t s c NVMCON,#WR ; Wait f o r w r i t e end
bra 1b

return

;∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
W r i t e L a t c h : ;W0=TBLPAG,W1=Wn,W2=WordHi ,W3=WordLo − no return v a l u e s
mov W0,TBLPAG
t b l w t l W3, [ W1]
t b l w t h W2, [ W1]

return

;∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
ReadLatch : ;W0=TBLPAG,W1=Wn − data i n W1:W0
mov W0,TBLPAG
t b l r d l [W1] ,W0
t b l r d h [W1] ,W1

return

;∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
RunProg :
goto 0xC00
return

;∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
Erase :
push TBLPAG
mov W2,NVMCON

mov w0 ,TBLPAG ; I n i t P o i n t e r t o page t o be e r a s e d

tblwtl w1 , [ w1 ] ; Dummy w r i t e t o s e l e c t t h e row

mov #0x55 ,W0 ; Unlock s e q u e n c e − i n t e r r u p t s need t o be

off
mov W0,NVMKEY
mov #0xAA,W0
mov W0,NVMKEY
b s e t NVMCON,#WR
nop ; Required
nop

erase wait :
b t s c NVMCON,#WR ; Wait f o r w r i t e end
bra e r a s e w a i t

pop TBLPAG
return

;∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
. end

136
A.3 USB bridge firmware
A.3.1 PIC18F code (base.c)

Listing A.8: PIC18F code

#include <.\ base complemento . h> // s p e c i a l and i n t e r r u p t r o u t i n e s i n s i d e
// //////////////////////////////////////////////
void main ( )
{
int1 flag = 0;

p o r t a = 0 ; // a l l p o r t s a r e z e r o
portb = 0 ;
portc = 0;

s e t u p a d c p o r t s ( n o a n a l o g s | v s s v d d ) ; // d i g i t a l f u n c t i o n s s e l e c t e d
s e t u p a d c ( a d c o f f ) ; // i n t e r n a l r c o s c i l l a t o r d i s a b l e d f o r adc
s e t u p w d t ( w d t o f f ) ; // watch dog t i m e r d i s a b l e d
s e t u p t i m e r 0 ( r t c c o f f ) ; // a l l t i m e r s d i s a b l e d
setup timer 1 ( t1 disabled ) ;
setup timer 2 ( t2 disabled ,0 ,1) ;
setup timer 3 ( t3 disabled | t3 div by 1 ) ;
s e t u p c o m p a r a t o r ( n c n c n c n c ) ; // c o m pa r a t or s d i s a b l e d
s e t u p v r e f ( f a l s e ) ; // no r e f e r e n c e v o l t a g e i n ra2
s e t u p c c p 1 ( c c p o f f ) ; // d i s a b l e ccp1
s e t u p c c p 2 ( c c p o f f ) ; // d i s a b l e ccp2
e n a b l e i n t e r r u p t s ( i n t r d a ) ; // u a r t r x i n t e r r u p t i o n e n a b l e d
e n a b l e i n t e r r u p t s ( g l o b a l ) ; // g l o b a l i n t e r r u p t i o n s e n a b l e d
usb cdc init () ;
u s b i n i t ( ) ; // i n i t i a l i z e hardware u s b and w a i t f o r PC c o n e c t i o n

s e t t r i s a ( 0 b00111111 ) ;
s e t t r i s b ( 0 b11111011 ) ; // r b 2 o u t p u t mclr d s p i c

port b pullups ( false ) ;

s e t t r i s c ( 0 b10111111 ) ;

s t a te D s p i c = running ;
counterReset = 0;
d e l a y m s ( 5 0 0 ) ; // w a i t f o r s u p p l y s t a b i l i z a t i o n

while ( t r u e )
{
usb task () ;
manage conection ( ) ;

i f ( usb cdc kbhit () )

{
d a t a r x u s b=u s b c d c g e t c ( ) ; // r e a d b u f f e r and s a v e i n d a t a r x
p r i n t f ( " % c " , d a t a r x u s b ) ; // send t h r o u g h u a r t

i f ( d a t a r x u s b == rstKeyword [ 0 ] )
{
i f ( c o u n t e r R e s e t == 0 )
c o u n t e r R e s e t ++;
}
e l s e i f ( d a t a r x u s b == rstKeyword [ 1 ] )
{
i f ( c o u n t e r R e s e t == 1 )

137
 c o u n t e r R e s e t ++;
else
counterReset = 0;
}
e l s e i f ( d a t a r x u s b == rstKeyword [ 2 ] )
{
i f ( c o u n t e r R e s e t == 2 )
c o u n t e r R e s e t ++;
else
counterReset = 0;
}
e l s e i f ( d a t a r x u s b == rstKeyword [ 3 ] )
{
i f ( c o u n t e r R e s e t == 3 )
c o u n t e r R e s e t ++;
else
counterReset = 0;
}
e l s e i f ( d a t a r x u s b == rstKeyword [ 4 ] && c o u n t e r R e s e t == 4 ) // here , a l l
r e q u i r e m e n t s were met
{
counterReset = 0;
f l a g = 0 ; // r e s e t f l a g
f o r ( i = 0 ; i < 1 0 0 0 0 ; i ++) // w a i t f o r t h e n e x t b y t e
{
i f ( u s b c d c k b h i t ( ) ) // i f a new b y t e i s r e c e i v e d
{
d a t a r x u s b = u s b c d c g e t c ( ) ; // r e a d b u f f e r and s a v e i n d a t a r x
p r i n t f ( " % c " , d a t a r x u s b ) ; // send t h r o u g h u a r t
flag = 0;
break ;
}
flag = 1;
}
i f ( f l a g == 1 ) // a p p l y r e s e t when no c h a r a c t e r s were r e c e i v e d
{
stateDspic = stop ;
delay ms (50) ;
s t a te D s p i c = running ;
}
}
else
counterReset = 0;

}
}
}

A.3.2 PIC18F complementary code (base complemento.h)

complemento.h

Listing A.9: PIC18F complementary code

complemento.h
#include <18F2550 . h>

138
void main ( ) {

} complemento.h

A.4 Pendulum control software

A.4.1 Pendulum GUI (pendulum.py)

Listing A.10: Pendulum-GUI file

im por t Tkinter as tk
im por t tkMessageBox
im por t time
im por t numpy
im por t rtMonitoring
im por t modeling
im por t serialBoot
im por t os

im por t m a t p l o t l i b . p y p l o t a s p l t
from m a t p l o t l i b . backends . b a c k e n d t k a g g i mpo rt FigureCanvasTkAgg , NavigationToolbar2TkAgg
from m a t p l o t l i b . backends . b a c k e n d p d f i mpo rt PdfPages

g a i n s C t r l 0 = [ ’’ , ’’ , ’’ , ’’ , ’’ ]
g a i n s C t r l 1 = [ ’’ , ’’ , ’’ , ’’ , ’’ ]
g a i n s C t r l 2 = [ ’’ , ’’ , ’’ , ’’ , ’’ ]
g a i n s C t r l 3 = [ ’’ , ’’ , ’’ , ’’ , ’’ ]
samplingCtrl0 = ’’
samplingCtrl1 = ’’
samplingCtrl2 = ’’
samplingCtrl3 = ’’

def i n i t F i l e () :

s e l f N a m e = o s . path . basename ( file ) #s c r i p t ’s self location

selfPath = os . path . realpath ( __file__ ) # script ’ s s e l f name

i f s e l f P a t h . e n d s w i t h ( s e l f N a m e ) : #removes s c r i p t ’s name from path

selfPath = selfPath [: - len ( selfName ) ]

os . chdir ( selfPath ) # sets current directory in order to access the config file

global gainsCtrl0 , gainsCtrl1 , gainsCtrl2 , gainsCtrl3

os . chdir ( selfPath ) # sets current directory in order to access the config file
defaultGains = open ( ’ c o n f i g G a i n s C o n t r o l . c o n f i g ’ , ’ r ’)

for i in range (0 , 5) :
gainsCtrl0 [ i ] = defaultGains . readline () [: -1] # reads line and removes last
character
for i in range (0 , 5) :

139
 gainsCtrl1 [ i ] = defaultGains . readline () [: -1] # reads line and removes last
character
for i in range (0 , 5) :
gainsCtrl2 [ i ] = defaultGains . readline () [: -1] # reads line and removes last
character
for i in range (0 , 5) :
gainsCtrl3 [ i ] = defaultGains . readline () [: -1] # reads line and removes last
character

defaultGains . close ()

global samplingCtrl0 , samplingCtrl1 , samplingCtrl2 , samplingCtrl3

os . chdir ( selfPath ) # sets current directory in order to access the config file
d ef au lt S am pl in g = open ( ’ c o n f i g S a m p l i n g . c o n f i g ’ , ’ r ’)

samplingCtrl0 = de f au lt S am pl in g . readline () [: -1] # reads line and removes last

character
samplingCtrl1 = de f au lt S am pl in g . readline () [: -1] # reads line and removes last
character
samplingCtrl2 = de f au lt S am pl in g . readline () [: -1] # reads line and removes last
character
samplingCtrl3 = de f au lt S am pl in g . readline () [: -1] # reads line and removes last
character

d ef au lt S am pl in g . close ()

class gui () :
def __init__ ( self , master ) :

global gainsCtrl0 , gainsCtrl1 , gainsCtrl2 , gainsCtrl3

global samplingCtrl0 , samplingCtrl1 , samplingCtrl2 , samplingCtrl3

self . samples = 200

self . portWasOpened = 0
self . stateOfCalib = 0
self . state1 = float (0)
self . state2 = float (0)
self . control = float (0)
self . state3 = float (0)
self . state4 = float (0)
self . state5 = float (0)
self . calibState = int (0)
self . s e n d G a i n s R e q u e s t = 1
self . s e n d S a m p l i n g R e q u e s t = 1

self . typeProcess = 0 #0 crank control , 1 inverted control , 2 other control

#3 another control , 4 large calibration , 5 short calibration
self . typ eProces sOld = self . typeProcess

self . gainText = list ()

for i in range (0 , 5) :
self . gainText . append ( tk . StringVar () )

self . samplingText = tk . StringVar ()

self . gainTx = list ([0.0 , 0.0 , 0.0 , 0.0 , 0.0]) # gains to be sent

self . samplingTx = float (0) # sampling times to be sent

self . referValue = tk . DoubleVar ()

self . ref erValue Prev = self . referValue . get ()

self . us bP o rt Va ri a bl e = tk . StringVar ()

140
 self . currentPort = tk . StringVar ()
self . stateCrank = tk . IntVar ()
self . stateInverted = tk . IntVar ()
self . stateOther = tk . IntVar ()
self . stateAnother = tk . IntVar ()
self . master = master
self . initInterface ()
self . initStart ()

def initInterface ( self ) :

# frames
self . myframe = tk . Frame ( self . master ) # inside of this frame will be placed the
most of the widgets
# except canvas
# self . myframe1 = tk . Frame ( self . master )

# buttons
self . startButton = tk . Button ( self . myframe , text = ’ S t a r t ’ , width =8 , command = self .
startPlotting )
self . stopButton = tk . Button ( self . myframe , text = ’ Stop ’ , height =5 , width =8 , command
= self . stopPlotting )
self . closeButton = tk . Button ( self . myframe , text = ’ C l o s e ’ , width =8 , command = self .
quitGui )
self . clearButton = tk . Button ( self . myframe , text = ’ C l e a r ’ , width =8 , command = self .
clearAll )
self . updateUsb = tk . Button ( self . myframe , text =" Update USB Device " ,
height =1 , width =13 , command = self . updatePorts )
self . re setUsbBu tton = tk . Button ( self . myframe , text =" Reset USB Device " ,
height =1 , width =13 , command = self . resetUsb )
self . l a r g e C a l i b B u t t o n = tk . Button ( self . myframe , text =" Large Calibration " ,
height =1 , width =14 , command = self . l a r g e C a l i b P e n d u l u m )
self . s h o r t C a l i b B u t t o n = tk . Button ( self . myframe , text =" Short Calibration " ,
height =1 , width =14 , command = self . s h o r t C a l i b P e n d u l u m )
self . u p d a t e G a i n s B u t t o n = tk . Button ( self . myframe , text =" Save Gains " ,
height =1 , width =11 , command = self . updateGains )
self . u p d a t e S a m p l i n g B u t t o n = tk . Button ( self . myframe , text =" Save Sampling " ,
height =1 , width =11 , command = self . update Samplin g )

# checkbuttons
self . cr an k Pe nd Bu t to n = tk . Checkbutton ( self . myframe , text =" Crank control " ,
variable = self . stateCrank ,
onvalue =1 , offvalue =0 , command = self .
crankPend )
self . invPendButton = tk . Checkbutton ( self . myframe , text =" Inverted control " ,
variable = self . stateInverted ,
onvalue =1 , offvalue =0 , command = self . invPend )
self . ot he r Pe nd Bu t to n = tk . Checkbutton ( self . myframe , text =" Other control " ,
variable = self . stateOther ,
onvalue =1 , offvalue =0 , command = self .
otherPend )
self . a n o t h e r P e n d B u t t o n = tk . Checkbutton ( self . myframe , text =" Another control " ,
variable = self . stateAnother ,
onvalue =1 , offvalue =0 , command = self .
anotherPend )
self . cr an k Pe nd Bu t to n . select () # by default crank control

# labels
self . calibLabel = tk . Label ( self . myframe , text ="" , fg =" red " ,
justify = tk . LEFT , font =(" Helvetica " , 14) )

self . gainLabel = tk . Label ( self . myframe , text =" GAINS :" , fg =" green " ,
justify = tk . CENTER , font =(" Helvetica " , 11) )

141
self . samplingLabel = tk . Label ( self . myframe , text =" SAMPLING TIME :" , fg =" red " ,
justify = tk . CENTER , font =(" Helvetica " , 11) )
self . msLabel = tk . Label ( self . myframe , text ="( ms ) " , fg =" red " ,
justify = tk . CENTER , font =(" Helvetica " , 11) )
self . kLabel = list ()

labels = list ([ ’x ’ , ’xDot ’ , ’ t h e t a ’ , ’ t h e t a D o t ’ , ’ i n t ’ ])

for i in range (0 , 5) :
label = tk . Label ( self . myframe , text = ’K’+ labels [ i ] , fg =" green " ,
justify = tk . CENTER , font =(" Helvetica " , 10) )
self . kLabel . append ( label )

# menus
self . usbMenu = tk . OptionMenu ( self . myframe , self . usbPortVariable , () )
self . usbMenu . configure ( height =1 , width =11)
self . updatePorts ()

# entries
self . entryGain = list ()

entry0 = tk . Entry ( self . myframe , width =8 , textvariable = self . gainText [0])

entry0 . bind ( ’<KeyPress>’ , self . k eyValida tion0 )
entry0 . focus ()
self . entryGain . append ( entry0 )

entry1 = tk . Entry ( self . myframe , width =8 , textvariable = self . gainText [1])

entry1 . bind ( ’<KeyPress>’ , self . k eyValida tion1 )
entry1 . focus ()
self . entryGain . append ( entry1 )

entry2 = tk . Entry ( self . myframe , width =8 , textvariable = self . gainText [2])

entry2 . bind ( ’<KeyPress>’ , self . k eyValida tion2 )
entry2 . focus ()
self . entryGain . append ( entry2 )

entry3 = tk . Entry ( self . myframe , width =8 , textvariable = self . gainText [3])

entry3 . bind ( ’<KeyPress>’ , self . k eyValida tion3 )
entry3 . focus ()
self . entryGain . append ( entry3 )

entry4 = tk . Entry ( self . myframe , width =8 , textvariable = self . gainText [4])

entry4 . bind ( ’<KeyPress>’ , self . k eyValida tion4 )
entry4 . focus ()
self . entryGain . append ( entry4 )

self . entrySampling = tk . Entry ( self . myframe , width =8 , textvariable = self .

samplingText )
self . entrySampling . bind ( ’<KeyPress>’ , self . k e y V a l i d a t i o n S a m p l i n g )
self . entrySampling . focus ()

# menu bar
self . menubar = tk . Menu ( self . master )
menu = tk . Menu ( self . menubar , tearoff =0)
self . menubar . add_cascade ( label =" File " , menu = menu )
menu . add_command ( label =" Close " , command = self . quitGui )

menu = tk . Menu ( self . menubar , tearoff =0)

self . menubar . add_cascade ( label =" View " , menu = menu )
menu . add_command ( label =" Snapshot PDF " , command = self . snapShotPdf )
menu . add_command ( label =" Snapshot EPS " , command = self . snapShotEps )

menu = tk . Menu ( self . menubar , tearoff =0)

self . menubar . add_cascade ( label =" Help " , menu = menu )
menu . add_command ( label =" About " , command = self . credits )

142
try :
self . master . config ( menu = self . menubar )
except Attri buteErro r :
# master is a toplevel window ( Python 1.4/ Tkinter 1.63)
self . master . tk . call ( self . master , " config " , " - menu " , self . menubar )

self . canvasMenu = tk . Canvas ( self . master , bg =" white " , width =400 , height =400 ,
h i g h l i g h t t h i c k n e s s =0)

# figures
self . figure = plt . figure ()
plt . rc ( ’ x t i c k ’ , labelsize =9)
plt . rc ( ’ y t i c k ’ , labelsize =9)

self . ax1 = plt . subplot (321)

self . ax11 = plt . subplot (321) # reference line
self . ax1 . grid ( True )
self . ax11 . grid ( True )
self . line1 , = self . ax1 . plot ([] , [] , color =(1 , 0 , 0) )
self . line11 , = self . ax1 . plot ([] , [] , color =(0 , 0 , 0) )

plt . xlim (0 , self . samples )

plt . ylim ( -1 , 1)
plt . ylabel ( ’CART POSITION [m] ’ , fontsize =10)

self . ax2 = plt . subplot (323)

self . ax2 . grid ( True )
self . line2 , = self . ax2 . plot ([] , [] , color =(0 , 1 , 0) )
plt . xlim (0 , self . samples )
plt . ylim ( - numpy . pi -0.5 , numpy . pi +0.5) # -1.5* numpy . pi , 1.5* numpy . pi )
plt . ylabel ( ’ROD POSITION [ rad ] ’ , fontsize =10)

self . ax3 = plt . subplot (325)

self . ax3 . grid ( True )
self . line3 , = self . ax3 . plot ([] , [] , color =(0 , 0 , 1) )
plt . xlim (0 , self . samples )
plt . ylim ( -12.4 , 12.4)
# plt . xlabel ( ’SAMPLES’)
plt . ylabel ( ’CONTROL ACTION [V] ’ , fontsize =10)

self . ax4 = plt . subplot (322)

self . ax4 . grid ( True )
self . line4 , = self . ax4 . plot ([] , [] , color =(1 , 0 , 0) )
plt . xlim (0 , self . samples )
plt . ylim ( -1.5 , 1.5)
plt . ylabel ( ’CART VELOCITY [m/ s ] ’ , fontsize =10)

self . ax5 = plt . subplot (324)

self . ax5 . grid ( True )
self . line5 , = self . ax5 . plot ([] , [] , color =(0 , 1 , 0) )
plt . xlim (0 , self . samples )
plt . ylim ( -2* numpy . pi , 2* numpy . pi )
plt . ylabel ( ’ROD VELOCITY [ rad / s ] ’ , fontsize =10)

self . ax6 = plt . subplot (326)

self . ax6 . grid ( True )
self . line6 , = self . ax6 . plot ([] , [] , color =(0 , 0 , 1) )
plt . xlim (0 , self . samples )
plt . ylim ( -50 , 50)
plt . ylabel ( ’ACCUMULATED ERROR [m] ’ , fontsize =10)

self . canvas = F i g u r e C a n v a s T k A g g ( self . figure , master = self . master )

143
 # slides
self . referSlide = tk . Scale ( master = root , label =" Reference [ m ]" , variable = self .
referValue ,
from_ = -0.25 , to =0.25 , sliderlength =50 , length =650 ,
resolution =0.01 ,
orient = tk . HORIZONTAL )
# locations on the layer
# self . canvas . get_tk_widget () . pack_forget ()
# self . referSlide . pack_forget ()
# self . myframe . pack_forget ()

self . canvas . get_tk_widget () . pack ( fill = tk . BOTH , expand = tk . YES )

self . referSlide . pack ( fill = tk . BOTH , expand = tk . YES )
self . myframe . pack ( side = tk . BOTTOM , fill = tk . BOTH , expand = tk . YES )

self . gainLabel . grid ( row =0 , column =0)

self . samplingLabel . grid ( row =2 , column =0)
self . msLabel . grid ( row =2 , column =1)
self . u p d a t e G a i n s B u t t o n . grid ( row =1 , column =0 , rowspan =1)
self . u p d a t e S a m p l i n g B u t t o n . grid ( row =3 , column =0 , rowspan =1)
self . entrySampling . grid ( row =3 , column =1)

for i in range (0 , 5) :
self . entryGain [ i ]. grid ( row =1 , column = i +1)

for i in range (0 , 5) :
self . kLabel [ i ]. grid ( row =0 , column = i +1)

self . updateUsb . grid ( row =1 , column =7)

self . usbMenu . grid ( row =2 , column =7)
self . res etUsbBu tton . grid ( row =3 , column =7)
self . s h o r t C a l i b B u t t o n . grid ( row =1 , column =9)
self . l a r g e C a l i b B u t t o n . grid ( row =1 , column =10)
self . calibLabel . grid ( row =2 , column =9 , columnspan =2)

self . cr an k Pe nd Bu t to n . grid ( row =2 , column =2 , columnspan =2)

self . invPendButton . grid ( row =2 , column =4 , columnspan =2)
self . ot he r Pe nd Bu t to n . grid ( row =3 , column =2 , columnspan =2)
self . a n o t h e r P e n d B u t t o n . grid ( row =3 , column =4 , columnspan =2)

self . startButton . grid ( row =1 , column =11)

self . clearButton . grid ( row =2 , column =11)
self . closeButton . grid ( row =3 , column =11)
self . stopButton . grid ( row =1 , column =12 , rowspan =3)

def initStart ( self ) :

self . stopButton [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . clearButton [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . typeProcess = 0
self . typ eProces sOld = self . typeProcess
self . cr an k Pe nd Bu t to n . select () # by default crank control
self . invPendButton . deselect ()
self . ot he r Pe nd Bu t to n . deselect ()
self . a n o t h e r P e n d B u t t o n . deselect ()
self . showGains ()
self . showSampling ()
self . isPlotting = False
self . valueX = 0 # reinitialize samples counter
self . referValue . set (0) # reference bar = 0

def clearAll ( self ) :

self . canvas . get_tk_widget () . pack_forget ()
self . referSlide . pack_forget ()
self . myframe . pack_forget ()

144
 self . initInterface ()
self . initStart ()

def _ r e s e t O p t i o n M e n u ( self , options , index = None ) :

’ ’ ’ r e s e t t h e v a l u e s i n t h e o p t i o n menu
i f i n d e x i s g i v e n , s e t t h e v a l u e o f t h e menu t o
the option at the given index
’’’
menu = self . usbMenu [" menu "]
menu . delete (0 , " end ")
for string in options :
menu . add_command ( label = string ,
command = lambda value = string :
self . us bP o rt Va ri a bl e . set ( value ) )
if index is not None :
self . us bP o rt Va ri a bl e . set ( options [ index ])

def updatePorts ( self ) :

’ ’ ’ S wi tc h t h e o p t i o n menu t o d i s p l a y c o l o r s ’ ’ ’
# self . _ r e s e t Op t i o n M e n u (["1" ,"2" ,"3" ,"4"] , 0)

serialBoot . loadConfig () # initializes ram positions in the serialBoot . py file

usbDevices = serialBoot . scanUSB ()

if len ( usbDevices ) == 0:
usbDevices = [ ’None ’]

self . _ r e s e t O p t i o n M e n u ( usbDevices , 0)

def resetUsb ( self ) :

pass

def startPlotting ( self ) :

self . currentPort = self . u sbP or tV a ri ab le . get () # takes selected port from usb
option menu

if self . currentPort != ’None ’:

self . rxData = rtMonitoring . serialCx ( self . currentPort , 33 , self . samples , ’cXrC
’ , 115200 ,
self . typeProcess )
self . startButton [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . stopButton [ ’ s t a t e ’] = ’ normal ’
self . clearButton [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . closeButton [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . s h o r t C a l i b B u t t o n [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . l a r g e C a l i b B u t t o n [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . updateUsb [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . res etUsbBu tton [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . usbMenu [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . cr an k Pe nd Bu t to n [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . invPendButton [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . ot he r Pe nd Bu t to n [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . a n o t h e r P e n d B u t t o n [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . rxData . sendReference ( self . referValue . get () ) # sends the reference
self . ref erValue Prev = self . referValue . get () # updates the previous value of
the reference
self . isPlotting = True
self . updateGains ()
self . upd ateSamp ling () # with this command , mi c ro co nt r ol le r starts the whole
process
self . _plotting ()
else :

145
 tkMessageBox . showinfo (" Error " , " A USB compatible device , has not been
selected !")

def _plotting ( self ) :

while self . isPlotting :

if self . referValue . get () != self . re ferValu ePrev : # ensures sending only if the
reference changes
self . rxData . sendReference ( self . referValue . get () ) # sends the reference
self . re ferValue Prev = self . referValue . get () # updates the previous value of
the reference

if self . s e n d G a i n s R e q u e s t == 1:
self . rxData . sendGains ( self . gainTx [0] , self . gainTx [1] , self . gainTx [2] ,
self . gainTx [3] , self . gainTx [4])
self . s e n d G a i n s R e q u e s t = 0

if self . s e n d S a m p l i n g R e q u e s t == 1:
self . rxData . sendSampling ( self . samplingTx )
self . s e n d S a m p l i n g R e q u e s t = 0

[ self . state1 , self . state2 , self . control , self . state3 , self . state4 , self .
state5 , self . calibState ] = \
self . rxData . update DataOnly ()

# print self . state1 ,

# print self . state2 ,
# print self . state3 ,
# print self . state4

## update plot
# Update data ( with the new _and_ the old points )
self . line1 . set_xdata ( numpy . append ( self . line1 . get_xdata () , self . valueX ) )
self . line1 . set_ydata ( numpy . append ( self . line1 . get_ydata () , self . state1 ) )
self . line11 . set_xdata ( numpy . append ( self . line11 . get_xdata () , self . valueX ) )
self . line11 . set_ydata ( numpy . append ( self . line11 . get_ydata () , self . referValue .
get () ) )

self . line2 . set_xdata ( numpy . append ( self . line2 . get_xdata () , self . valueX ) )
self . line2 . set_ydata ( numpy . append ( self . line2 . get_ydata () , self . state2 ) )

self . line3 . set_xdata ( numpy . append ( self . line3 . get_xdata () , self . valueX ) )
self . line3 . set_ydata ( numpy . append ( self . line3 . get_ydata () , self . control ) )

self . line4 . set_xdata ( numpy . append ( self . line4 . get_xdata () , self . valueX ) )
self . line4 . set_ydata ( numpy . append ( self . line4 . get_ydata () , self . state3 ) )

self . line5 . set_xdata ( numpy . append ( self . line5 . get_xdata () , self . valueX ) )
self . line5 . set_ydata ( numpy . append ( self . line5 . get_ydata () , self . state4 ) )

self . line6 . set_xdata ( numpy . append ( self . line6 . get_xdata () , self . valueX ) )
self . line6 . set_ydata ( numpy . append ( self . line6 . get_ydata () , self . state5 ) )

self . valueX = self . valueX + 1

# change scale
if self . valueX == 180:
self . ax1 . axis ([20 , self . samples +20 , -1 , 1])
self . ax2 . axis ([20 , self . samples +20 , - numpy . pi -0.5 , numpy . pi +0.5])
self . ax3 . axis ([20 , self . samples +20 , -12.4 , 12.4])
self . ax4 . axis ([20 , self . samples +20 , -1 , 1])
self . ax5 . axis ([20 , self . samples +20 , -2* numpy . pi , 2* numpy . pi ])
self . ax6 . axis ([20 , self . samples +20 , -50 , 50])

146
 elif self . valueX >= self . samples :
if self . valueX %20 == 0:
self . ax1 . axis ([ self . valueX -( self . samples -20) , self . valueX +20 , -1 , 1])
self . ax2 . axis ([ self . valueX -( self . samples -20) , self . valueX +20 , - numpy .
pi -0.5 , numpy . pi +0.5])
self . ax3 . axis ([ self . valueX -( self . samples -20) , self . valueX +20 , -12.4 ,
12.4])
self . ax4 . axis ([ self . valueX -( self . samples -20) , self . valueX +20 , -1 , 1])
self . ax5 . axis ([ self . valueX -( self . samples -20) , self . valueX +20 , -2*
numpy . pi , 2* numpy . pi ])
self . ax6 . axis ([ self . valueX -( self . samples -20) , self . valueX +20 , -50 ,
50])

# We need to draw * and * flush

self . figure . canvas . draw ()
self . figure . canvas . flush_events ()

if self . calibState == 0:
self . calibLabel . config ( text =" Device has not been calibrated " , fg =" red ")
elif self . calibState == 1:
self . calibLabel . config ( text =" Positioning cart at the track start " , fg ="
orange ")
elif self . calibState == 2:
self . calibLabel . config ( text =" Waiting until rod gets stop " , fg =" orange ")
elif self . calibState == 3:
self . calibLabel . config ( text =" Resetting the cart sensor " , fg =" orange ")
elif self . calibState == 4:
self . calibLabel . config ( text =" Resetting the rod sensor " , fg =" orange ")
elif self . calibState == 5:
self . calibLabel . config ( text =" Positioning cart at the track center " , fg ="
orange ")
elif self . calibState == 6:
self . calibLabel . config ( text =" Device is already calibrated " , fg =" green ")
if self . typeProcess == 4 or self . typeProcess == 5: # only in calibration
processes
self . stopPlotting ()
self . typeProcess = self . typeP rocessO ld
self . showGains ()
self . showSampling ()

def stopPlotting ( self ) :

self . startButton [ ’ s t a t e ’] = ’ normal ’
self . stopButton [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . clearButton [ ’ s t a t e ’] = ’ normal ’
self . closeButton [ ’ s t a t e ’] = ’ normal ’
self . s h o r t C a l i b B u t t o n [ ’ s t a t e ’] = ’ normal ’
self . l a r g e C a l i b B u t t o n [ ’ s t a t e ’] = ’ normal ’
self . updateUsb [ ’ s t a t e ’] = ’ normal ’
self . res etUsbBu tton [ ’ s t a t e ’] = ’ normal ’
self . usbMenu [ ’ s t a t e ’] = ’ normal ’
self . cr an k Pe nd Bu t to n [ ’ s t a t e ’] = ’ normal ’
self . invPendButton [ ’ s t a t e ’] = ’ normal ’
self . ot he r Pe nd Bu t to n [ ’ s t a t e ’] = ’ normal ’
self . a n o t h e r P e n d B u t t o n [ ’ s t a t e ’] = ’ normal ’
self . u p d a t e G a i n s B u t t o n [ ’ s t a t e ’] = ’ normal ’
self . u p d a t e S a m p l i n g B u t t o n [ ’ s t a t e ’] = ’ normal ’
for i in range (0 , 5) :
self . entryGain [ i ][ ’ s t a t e ’] = ’ normal ’
self . entrySampling [ ’ s t a t e ’] = ’ normal ’

self . isPlotting = False

rtMonitoring . serialCx . close ( self . rxData ) # close port
self . portWasOpened = 0

147
 self . typeProcess = self . typeP rocessOl d
self . showGains ()
self . showSampling ()

def showGains ( self ) :

for i in range (0 , 5) :
self . entryGain [ i ]. delete (0 , tk . END )

if self . typeProcess == 0:
for i in range (0 , 5) :
self . entryGain [ i ]. insert (0 , gainsCtrl0 [ i ])

elif self . typeProcess == 1:

for i in range (0 , 5) :
self . entryGain [ i ]. insert (0 , gainsCtrl1 [ i ])

elif self . typeProcess == 2:

for i in range (0 , 5) :
self . entryGain [ i ]. insert (0 , gainsCtrl2 [ i ])

elif self . typeProcess == 3:

for i in range (0 , 5) :
self . entryGain [ i ]. insert (0 , gainsCtrl3 [ i ])

def showSampling ( self ) :

self . entrySampling . delete (0 , tk . END )

if self . typeProcess == 0:
self . entrySampling . insert (0 , samplingCtrl0 )

elif self . typeProcess == 1:

self . entrySampling . insert (0 , samplingCtrl1 )

elif self . typeProcess == 2:

self . entrySampling . insert (0 , samplingCtrl2 )

elif self . typeProcess == 3:

self . entrySampling . insert (0 , samplingCtrl3 )

def l a r g e C a l i b P e n d u l u m ( self ) :
self . stateOfCalib = 0
self . currentPort = self . u s bP or tV a ri ab le . get () # takes selected port from usb
option menu
self . typ eProces sOld = self . typeProcess # saves type of control set before
calibration
self . typeProcess = 4 # large calibration
self . showGains ()
# self . showSampling ()
self . u p d a t e G a i n s B u t t o n [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . u p d a t e S a m p l i n g B u t t o n [ ’ s t a t e ’] = ’ d i s a b l e d ’
for i in range (0 , 5) :
self . entryGain [ i ][ ’ s t a t e ’] = ’ d i s a b l e d ’
self . entrySampling [ ’ s t a t e ’] = ’ d i s a b l e d ’

self . startPlotting ()

def s h o r t C a l i b P e n d u l u m ( self ) :
self . stateOfCalib = 0
self . currentPort = self . u s bP or tV a ri ab le . get () # takes selected port from usb
option menu

148
 self . ty peProces sOld = self . typeProcess # saves type of control set before
calibration
self . typeProcess = 5 # short calibration
self . showGains ()
# self . showSampling ()
self . u p d a t e G a i n s B u t t o n [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . u p d a t e S a m p l i n g B u t t o n [ ’ s t a t e ’] = ’ d i s a b l e d ’
for i in range (0 , 5) :
self . entryGain [ i ][ ’ s t a t e ’] = ’ d i s a b l e d ’
self . entrySampling [ ’ s t a t e ’] = ’ d i s a b l e d ’
self . startPlotting ()

def credits ( self ) :

tkMessageBox . showinfo (" About " , " Performed by \ r \ nCarlos Xavier Rosero & Manel
Velasco ")

def keyVali dation0 ( self , event ) :

if event . char in ’ 0123456789 − ’: # allows only numbers

return True
elif event . char == ’ . ’: # allows only 1 decimal point
if ’ . ’ in self . gainText [0]. get () :
return ’break ’
else :
return True
elif event . keysym == ’ BackSpace ’: # allows backspace to delete
return True
elif event . keysym == ’Tab’:
return True
elif event . keysym == ’ D e l e t e ’:
return True
else :
return ’break ’

def keyVali dation1 ( self , event ) :

if event . char in ’ 0123456789 − ’: # allows only numbers

return True
elif event . char == ’ . ’: # allows only 1 decimal point
if ’ . ’ in self . gainText [1]. get () :
return ’break ’
else :
return True
elif event . keysym == ’ BackSpace ’: # allows backspace to delete
return True
elif event . keysym == ’Tab’:
return True
elif event . keysym == ’ D e l e t e ’:
return True
else :
return ’break ’

def keyVali dation2 ( self , event ) :

if event . char in ’ 0123456789 − ’: # allows only numbers

return True
elif event . char == ’ . ’: # allows only 1 decimal point
if ’ . ’ in self . gainText [2]. get () :
return ’break ’
else :
return True
elif event . keysym == ’ BackSpace ’: # allows backspace to delete
return True
elif event . keysym == ’Tab’:

149
 return True
elif event . keysym == ’ D e l e t e ’:
return True
else :
return ’break ’

def keyVali dation3 ( self , event ) :

if event . char in ’ 0123456789 − ’: # allows only numbers

return True
elif event . char == ’ . ’: # allows only 1 decimal point
if ’ . ’ in self . gainText [3]. get () :
return ’break ’
else :
return True
elif event . keysym == ’ BackSpace ’: # allows backspace to delete
return True
elif event . keysym == ’Tab’:
return True
elif event . keysym == ’ D e l e t e ’:
return True
else :
return ’break ’

def keyVali dation4 ( self , event ) :

if event . char in ’ 0123456789 − ’: # allows only numbers

return True
elif event . char == ’ . ’: # allows only 1 decimal point
if ’ . ’ in self . gainText [4]. get () :
return ’break ’
else :
return True
elif event . keysym == ’ BackSpace ’: # allows backspace to delete
return True
elif event . keysym == ’Tab’:
return True
elif event . keysym == ’ D e l e t e ’:
return True
else :
return ’break ’

def k e y V a l i d a t i o n S a m p l i n g ( self , event ) :

if event . char in ’ 0123456789 ’: # allows only numbers

return True
elif event . keysym == ’ BackSpace ’: # allows backspace to delete
return True
elif event . keysym == ’Tab’:
return True
elif event . keysym == ’ D e l e t e ’:
return True
else :
return ’break ’

def text2float ( self , text , type ) :

try :
a = float ( text )
print ’New’ , type , ’ : ’ , a
return a

except ValueError :
print ’ E r r o r when v a l i d a t i n g g a i n ! ’
return float (0)

150
def snapShotPdf ( self ) :
fileName = " experiment_ " + time . strftime ("% Y % m % d ") + time . strftime (" _ % I % M % S ")
pp = PdfPages ( fileName +". pdf ")
pp . savefig ()
pp . close ()

def crankPend ( self ) :

self . typeProcess = 0 # crank control
self . ty peProces sOld = self . typeProcess
self . showGains ()
self . showSampling ()
if self . stateCrank . get () :
self . invPendButton . deselect ()
self . ot he r Pe nd Bu t to n . deselect ()
self . a n o t h e r P e n d B u t t o n . deselect ()
else :
self . cr an k Pe nd Bu t to n . select ()

def invPend ( self ) :

self . typeProcess = 1 # inverted control
self . ty peProces sOld = self . typeProcess
self . showGains ()
self . showSampling ()
if self . stateInverted . get () :
self . cr an k Pe nd Bu t to n . deselect ()
self . ot he r Pe nd Bu t to n . deselect ()
self . a n o t h e r P e n d B u t t o n . deselect ()
else :
self . i n v P e n d B u t t o n P e n d B u t t o n . select ()

def otherPend ( self ) :

self . typeProcess = 2 # other control
self . ty peProces sOld = self . typeProcess
self . showGains ()
self . showSampling ()
if self . stateOther . get () :
self . cr an k Pe nd Bu t to n . deselect ()
self . invPendButton . deselect ()
self . a n o t h e r P e n d B u t t o n . deselect ()
else :
self . ot he r Pe nd Bu t to n . select ()

def anotherPend ( self ) :

self . typeProcess = 3 # another control
self . ty peProces sOld = self . typeProcess
self . showGains ()
self . showSampling ()
if self . stateAnother . get () :
self . cr an k Pe nd Bu t to n . deselect ()
self . invPendButton . deselect ()
self . ot he r Pe nd Bu t to n . deselect ()
else :
self . a n o t h e r P e n d B u t t o n . select ()

def updateGains ( self ) :

if self . typeProcess != 4 and self . typeProcess != 5:

for i in range (0 , 5) :
self . gainTx [ i ] = ( self . text2float ( self . gainText [ i ]. get () , ’ g a i n ’) )

self . s e n d G a i n s R e q u e s t = 1 # flag to 1 , for tx purposes

151
 defaultGains = open ( ’ c o n f i g G a i n s C o n t r o l . c o n f i g ’ , ’w’) # it is going to replace
existing file content

if self . typeProcess == 0:
for i in range (0 , 5) :
value = ’%.3 f ’ % self . gainTx [ i ]
defaultGains . write ( value + ’\n’) # writes gains
gainsCtrl0 [ i ] = value
for i in range (0 , 5) :
defaultGains . write ( gainsCtrl1 [ i ] + ’\n’)
for i in range (0 , 5) :
defaultGains . write ( gainsCtrl2 [ i ] + ’\n’)
for i in range (0 , 5) :
defaultGains . write ( gainsCtrl3 [ i ] + ’\n’)

elif self . typeProcess == 1:

for i in range (0 , 5) :
defaultGains . write ( gainsCtrl0 [ i ] + ’\n’)
for i in range (0 , 5) :
value = ’%.3 f ’ % self . gainTx [ i ]
defaultGains . write ( value + ’\n’) # writes gains
gainsCtrl1 [ i ] = value
for i in range (0 , 5) :
defaultGains . write ( gainsCtrl2 [ i ] + ’\n’)
for i in range (0 , 5) :
defaultGains . write ( gainsCtrl3 [ i ] + ’\n’)

elif self . typeProcess == 2:

for i in range (0 , 5) :
defaultGains . write ( gainsCtrl0 [ i ] + ’\n’)
for i in range (0 , 5) :
defaultGains . write ( gainsCtrl1 [ i ] + ’\n’)
for i in range (0 , 5) :
value = ’%.3 f ’ % self . gainTx [ i ]
defaultGains . write ( value + ’\n’) # writes gains
gainsCtrl2 [ i ] = value
for i in range (0 , 5) :
defaultGains . write ( gainsCtrl3 [ i ] + ’\n’)

elif self . typeProcess == 3:

for i in range (0 , 5) :
defaultGains . write ( gainsCtrl0 [ i ] + ’\n’)
for i in range (0 , 5) :
defaultGains . write ( gainsCtrl1 [ i ] + ’\n’)
for i in range (0 , 5) :
defaultGains . write ( gainsCtrl2 [ i ] + ’\n’)
for i in range (0 , 5) :
value = ’%.3 f ’ % self . gainTx [ i ]
defaultGains . write ( value + ’\n’) # writes gains
gainsCtrl3 [ i ] = value

defaultGains . close ()

def updateS ampling ( self ) :

global samplingCtrl0 , samplingCtrl1 , samplingCtrl2 , samplingCtrl3

self . samplingTx = ( self . text2float ( self . samplingText . get () , ’ s a m p l i n g time ’) )

self . s e n d S a m p l i n g R e q u e s t = 1 # flag to 1 , for tx purposes

if self . typeProcess != 4 and self . typeProcess != 5:

d ef au lt S am pl in g = open ( ’ c o n f i g S a m p l i n g . c o n f i g ’ , ’w’) # it is going to replace
existing file content
if self . typeProcess == 0:

152
 value = ’%.0 f ’ % self . samplingTx
d ef au lt S am pl in g . write ( value + ’\n’) # writes sampling time
samplingCtrl0 = value
d ef au lt S am pl in g . write ( samplingCtrl1 + ’\n’)
d ef au lt S am pl in g . write ( samplingCtrl2 + ’\n’)
d ef au lt S am pl in g . write ( samplingCtrl3 + ’\n’)

elif self . typeProcess == 1:

d ef au lt S am pl in g . write ( samplingCtrl0 + ’\n’)
value = ’%.0 f ’ % self . samplingTx
d ef au lt S am pl in g . write ( value + ’\n’) # writes sampling time
samplingCtrl1 = value
d ef au lt S am pl in g . write ( samplingCtrl2 + ’\n’)
d ef au lt S am pl in g . write ( samplingCtrl3 + ’\n’)

elif self . typeProcess == 2:

d ef au lt S am pl in g . write ( samplingCtrl0 + ’\n’)
d ef au lt S am pl in g . write ( samplingCtrl1 + ’\n’)
value = ’%.0 f ’ % self . samplingTx
d ef au lt S am pl in g . write ( value + ’\n’) # writes sampling time
samplingCtrl2 = value
d ef au lt S am pl in g . write ( samplingCtrl3 + ’\n’)

elif self . typeProcess == 3:

d ef au lt S am pl in g . write ( samplingCtrl0 + ’\n’)
d ef au lt S am pl in g . write ( samplingCtrl1 + ’\n’)
d ef au lt S am pl in g . write ( samplingCtrl2 + ’\n’)
value = ’%.0 f ’ % self . samplingTx
d ef au lt S am pl in g . write ( value + ’\n’) # writes sampling time
samplingCtrl3 = value

d ef au lt S am pl in g . close ()

def snapShotEps ( self ) :

fileName = " experiment_ " + time . strftime ("% Y % m % d ") + time . strftime (" _ % I % M % S ")
plt . savefig ( fileName +". eps " , format = ’ e p s ’ , dpi =1000)

def quitGui ( self ) :

if self . portWasOpened == 1:
# rtMonitoring . serialCx . close ( self . rxData ) # close port
self . rxData . close ( self . rxData ) # close port
self . master . destroy ()
self . master . quit ()

########################################
###### here starts the main program ######
########################################

if __name__ == " __main__ ":

initFile ()

root = tk . Tk ()
root . wm_title (" Pendulum User Interface ")

app = gui ( root )

root . protocol ( ’WM DELETE WINDOW’ , app . quitGui )
root . mainloop ()

153
A.4.2 Real-time communication file (rtMonitoring.py)

Listing A.11: Real-time communication file

#! / u s r / b i n / env python
# −∗− c o d i n g : u t f −8 −∗−
"""
@authors : carlos xavier rosero
manel velasco garcia

best performance tested with python 2.7.3 , GCC 2.6.3 , pyserial 2.5
"""
from future im po rt d i v i s i o n
im po rt numpy
im po rt s e r i a l , struct
from c o l l e c t i o n s imp or t deque
im po rt time

im po rt m a t p l o t l i b . p y p l o t a s p l t
im po rt m a t p l o t l i b . a n i m a t i o n a s a n i m a t i o n

c l a s s command :
startCrankControl = 0 xf0
startInvertedControl = 0 xf1
startOtherControl = 0 xf2
startAnotherControl = 0 xf3
largeCalibration = 0 xf4
shortCalibration = 0 xf5
stopControl = 0 xfc
sendReference = 0 xfd
sendGainCrank = 0 x10
sendGainInv = 0 x11
sendGainOther = 0 x12
sendGainAnother = 0 x13
sendSampling = 0 x14

#u s e t h i s p a r t o n l y with o l d v e r s i o n s o f python and l i b r a r i e s

#t e s t e d s p e c i f i c a l l y with python 2 . 6 . 5 , GCC 4 . 4 . 3 , s e r i a l 1 . 3 . 5
c l a s s adapt ( s e r i a l . S e r i a l ) :
d e f w r i t e ( s e l f , data ) :
super ( s e l f . class , s e l f ) . w r i t e ( s t r ( data ) )

class serialCx :

def init ( s e l f , usbPort , frameLen , maxLen , EOL, bauds , t y p e P r o c e s s ) :

# open s e r i a l p o r t

s e l f . s e r = adapt ( p o r t=usbPort , b a u d r a t e=bauds , t i m e o u t =1 , p a r i t y= s e r i a l .

PARITY NONE,
s t o p b i t s= s e r i a l . STOPBITS ONE)

s e l f . s e r . open
s e l f . s e r . isOpen

time . s l e e p ( 0 . 2 5 )
i n p u t S = [ ] #i n i t i a l i z e s b u f f e r
while s e l f . s e r . i n W a i t i n g ( ) > 0 : #n e g l e c t s t h e t r a s h code r e c e i v e d
i n p u t S . append ( s e l f . s e r . r e a d ( 1 ) ) #appends a new v a l u e i n t o t h e b u f f e r

s e l f . typeProcess = typeProcess

154
 if s e l f . t y p e P r o c e s s == 0 :
v a r = command . s t a r t C r a n k C o n t r o l
e l i f s e l f . t y p e P r o c e s s == 1 :
v a r = command . s t a r t I n v e r t e d C o n t r o l
e l i f s e l f . t y p e P r o c e s s == 2 :
v a r = command . s t a r t O t h e r C o n t r o l
e l i f s e l f . t y p e P r o c e s s == 3 :
v a r = command . s t a r t A n o t h e r C o n t r o l
e l i f s e l f . t y p e P r o c e s s == 4 :
v a r = command . l a r g e C a l i b r a t i o n
e l i f s e l f . t y p e P r o c e s s == 5 :
v a r = command . s h o r t C a l i b r a t i o n

b u f f e r = b y t e a r r a y ( [ var , 0 x01 , 0 x02 , 0 x03 , 0 x04 , 0 x05 , 0 x06 ,

0 x07 , 0 x08 , 0 x09 , 0 x0a , 0x0b , 0 x0c ,
0x0d , 0 x0e , 0 x0f , 0 x10 , 0 x11 , 0 x12 , 0 x13 , 0 x14 ] )
s e l f . s e r . w r i t e ( b u f f e r ) #s e n d s t h e command t o s t a r t t h e r e c e p t i o n

self . ay1 = deque ( [ 0 . 0 ] ∗ maxLen )

self . ay2 = deque ( [ 0 . 0 ] ∗ maxLen )
self . ay3 = deque ( [ 0 . 0 ] ∗ maxLen )
self . ay4 = deque ( [ 0 . 0 ] ∗ maxLen )
self . ay5 = deque ( [ 0 . 0 ] ∗ maxLen )

s e l f . maxLen = maxLen

s e l f . e o l = EOL #end o f l i n e
s e l f . l e n E o l = l e n ( s e l f . e o l ) #l e n g t h o f t h e end o f l i n e

s e l f . frameLen = frameLen − s e l f . l e n E o l

s e l f . factorFCY = 1/40 e6 #1/Fcy

# s e l f . factorROD = 2 . 0 ∗ numpy . p i / 4 0 9 6 . 0 #(2 p i /1023 h o l e s )
# r a d i u s = 0 . 0 3 7 5 #cm
# s e l f . factorCAR = 2 . 0 ∗ numpy . p i ∗ r a d i u s / 4 0 9 2 . 0

self . state1 = float (0)

self . state2 = float (0)
self . control = float (0)
self . state3 = float (0)
self . state4 = float (0)
self . state5 = float (0)
self . c a l i b S t a t e = int ( 0 )

# add t o b u f f e r
d e f addToBuf ( s e l f , buf , v a l ) :
i f l e n ( b u f ) < s e l f . maxLen :
b u f . append ( v a l )
else :
b u f . pop ( )
buf . a p p e n d l e f t ( val )

# update p l o t
d e f update ( s e l f , frameNum , a1 , a2 , a3 , a4 , a5 ) :
try :
l i n e = bytearray ()
while True :
c = s e l f . s e r . read (1)
if c :
l i n e . append ( c )
i f l i n e [− s e l f . l e n E o l : ] == s e l f . e o l :
l i n e = l i n e [ 0 : − s e l f . l e n E o l ] #removes EOF
break

155
 else :
break

i f l e n ( l i n e ) == s e l f . frameLen :
# data = [ i n t ( v a l ) f o r v a l i n l i n e [ 0 : − 4 ] ] #e x c e p t t h e l a s t 4 e l e m e n t s

# time = ( data [0] < <24 | data [ 1 ] << 16 | data [ 2 ] << 8 | data [ 3 ] ) ∗ s e l f .
factorFCY

s e l f . s t a t e 1 = struct . unpack ( ’f ’ , l i n e [ 4 : 8 ] )
s e l f . s t a t e 1 = s e l f . s t a t e 1 [ 0 ] #t a k e s out t h e o n l y one e l e m e n t from t h e
tuple

s e l f . s t a t e 2 = struct . unpack ( ’f ’ , l i n e [ 8 : 1 2 ] )
s e l f . s t a t e 2 = s e l f . s t a t e 2 [ 0 ] #t a k e s out t h e o n l y one e l e m e n t from t h e
tuple

s e l f . c o n t r o l = struct . unpack ( ’f ’ , l i n e [ 1 2 : 1 6 ] ) #l a s t 4 e l e m e n t s become

float
s e l f . c o n t r o l = s e l f . c o n t r o l [ 0 ] #t a k e s out t h e o n l y one e l e m e n t from t h e
tuple

s e l f . s t a t e 3 = struct . unpack ( ’f ’ , l i n e [ 1 6 : 2 0 ] )
s e l f . s t a t e 3 = s e l f . s t a t e 3 [ 0 ] #t a k e s out t h e o n l y one e l e m e n t from t h e
tuple

s e l f . s t a t e 4 = struct . unpack ( ’f ’ , l i n e [ 2 0 : 2 4 ] )
s e l f . s t a t e 4 = s e l f . s t a t e 4 [ 0 ] #t a k e s out t h e o n l y one e l e m e n t from t h e
tuple

s e l f . s t a t e 5 = struct . unpack ( ’f ’ , l i n e [ 2 4 : 2 8 ] )
s e l f . s t a t e 5 = s e l f . s t a t e 5 [ 0 ] #t a k e s out t h e o n l y one e l e m e n t from t h e
tuple

self . calibState = line [28]

# add data t o b u f f e r
s e l f . addToBuf ( s e l f . ay1 , self . state1 )
s e l f . addToBuf ( s e l f . ay2 , self . state2 )
s e l f . addToBuf ( s e l f . ay3 , self . control )
s e l f . addToBuf ( s e l f . ay4 , self . state3 )
s e l f . addToBuf ( s e l f . ay5 , self . state4 )
s e l f . addToBuf ( s e l f . ay5 , self . state5 )

a1 . set d a t a ( range ( self . maxLen ) , self . ay1 )

a2 . set d a t a ( range ( self . maxLen ) , self . ay2 )
a3 . set d a t a ( range ( self . maxLen ) , self . ay3 )
a4 . set d a t a ( range ( self . maxLen ) , self . ay4 )
a5 . set d a t a ( range ( self . maxLen ) , self . ay5 )

else :
p r i n t ’ Incorrect frame length : ’ ,
print len ( line )
s e l f . s e r . f l u s h I n p u t ( ) #g e t s empty t h e s e r i a l p o r t b u f f e r

except KeyboardInterrupt :
p r i n t ( ’ Exiting ... ’ )

# update data o n l y
d e f updateDataOnly ( s e l f ) :

try :
l i n e = bytearray ()
while True :

156
 c = s e l f . s e r . read (1)
if c :
l i n e . append ( c )
i f l i n e [− s e l f . l e n E o l : ] == s e l f . e o l :
l i n e = l i n e [ 0 : − s e l f . l e n E o l ] #removes EOF
break
else :
break

if s e l f . frameLen == l e n ( l i n e ) :
data = [ i n t ( v a l ) f o r v a l i n l i n e [ 0 : − 4 ] ] #e x c e p t t h e l a s t 4 e l e m e n t s

time = ( data [ 0 ] << 24 | data [ 1 ] << 16 | data [ 2 ] << 8 | data [ 3 ] ) ∗ s e l f .

factorFCY

s e l f . s t a t e 1 = struct . unpack ( ’f ’ , l i n e [ 4 : 8 ] )
s e l f . s t a t e 1 = s e l f . s t a t e 1 [ 0 ] #t a k e s out t h e o n l y one e l e m e n t from t h e
tuple

s e l f . s t a t e 2 = struct . unpack ( ’f ’ , l i n e [ 8 : 1 2 ] )
s e l f . s t a t e 2 = s e l f . s t a t e 2 [ 0 ] #t a k e s out t h e o n l y one e l e m e n t from t h e
tuple

s e l f . c o n t r o l = struct . unpack ( ’f ’ , l i n e [ 1 2 : 1 6 ] ) #4 e l e m e n t s become f l o a t

s e l f . c o n t r o l = s e l f . c o n t r o l [ 0 ] #t a k e s out t h e o n l y one e l e m e n t from t h e
tuple

s e l f . s t a t e 3 = struct . unpack ( ’f ’ , l i n e [ 1 6 : 2 0 ] )
s e l f . s t a t e 3 = s e l f . s t a t e 3 [ 0 ] #t a k e s out t h e o n l y one e l e m e n t from t h e
tuple

s e l f . s t a t e 4 = struct . unpack ( ’f ’ , l i n e [ 2 0 : 2 4 ] )
s e l f . s t a t e 4 = s e l f . s t a t e 4 [ 0 ] #t a k e s out t h e o n l y one e l e m e n t from t h e
tuple

s e l f . s t a t e 5 = struct . unpack ( ’f ’ , l i n e [ 2 4 : 2 8 ] )
s e l f . s t a t e 5 = s e l f . s t a t e 5 [ 0 ] #t a k e s out t h e o n l y one e l e m e n t from t h e
tuple

self . calibState = line [28]

return [ s e l f . s t a t e 1 , s e l f . s t a t e 2 , s e l f . control ,
s e l f . state3 , s e l f . state4 , s e l f . s t a t e 5 , s e l f . c a l i b S t a t e ] #s e n d s
current states

else :
p r i n t ’ Incorrect frame length !!! ’
s e l f . s e r . f l u s h I n p u t ( ) #g e t s empty t h e s e r i a l p o r t b u f f e r
return [ s e l f . s t a t e 1 , s e l f . s t a t e 2 , s e l f . c o n t r o l ,
s e l f . s t a t e 3 , s e l f . s t a t e 4 , s e l f . s t a t e 5 , s e l f . c a l i b S t a t e ] #s e n d s
previous states

except KeyboardInterrupt :
p r i n t ( ’ Exiting ... ’ )

def sendReference ( s e l f , r e f e r e n c e ) :

v a l u e = struct . pack ( ’% sf ’ % 1 , r e f e r e n c e ) #s p l i t s t h e f l o a t v a l u e i n t o 4 s t r i n g s
b u f f e r = b y t e a r r a y ( [ command . s e n d R e f e r e n c e , ord ( v a l u e [ 0 ] ) , ord ( v a l u e [ 1 ] ) , ord (
v a l u e [ 2 ] ) , ord ( v a l u e [ 3 ] ) ,
0 x05 , 0 x06 , 0 x07 , 0 x08 , 0 x09 , 0 x0a , 0x0b , 0 x0c ,
0x0d , 0 x0e , 0 x0f , 0 x10 , 0 x11 , 0 x12 , 0 x13 , 0 x14 ] )
s e l f . s e r . w r i t e ( b u f f e r ) #s e n d s t h e r e f e r e n c e

157
 d e f s en d Ga i ns ( s e l f , g a i n 0 , g a i n 1 , g a i n 2 , g a i n 3 , g a i n 4 ) :

if s e l f . t y p e P r o c e s s == 0 :
v a r = command . sendGainCrank
e l i f s e l f . t y p e P r o c e s s == 1 :
v a r = command . sendGainInv
e l i f s e l f . t y p e P r o c e s s == 2 :
v a r = command . sendGainOther
e l i f s e l f . t y p e P r o c e s s == 3 :
v a r = command . sendGainAnother
else :
v a r = command . sendGainCrank

value0 = struct . pack ( ’% sf ’ % 1, gain0 ) #s p l i t s the float value into 4 strings

value1 = struct . pack ( ’% sf ’ % 1, gain1 ) #s p l i t s the float value into 4 strings
value2 = struct . pack ( ’% sf ’ % 1, gain2 ) #s p l i t s the float value into 4 strings
value3 = struct . pack ( ’% sf ’ % 1, gain3 ) #s p l i t s the float value into 4 strings
value4 = struct . pack ( ’% sf ’ % 1, gain4 ) #s p l i t s the float value into 4 strings

b u f f e r = b y t e a r r a y ( [ var ,
ord ( v a l u e 0 [ 0 ] ) , ord ( v a l u e 0 [ 1 ] ) , ord ( v a l u e 0 [ 2 ] ) , ord ( v a l u e 0
[3]) ,
ord ( v a l u e 1 [ 0 ] ) , ord ( v a l u e 1 [ 1 ] ) , ord ( v a l u e 1 [ 2 ] ) , ord ( v a l u e 1
[3]) ,
ord ( v a l u e 2 [ 0 ] ) , ord ( v a l u e 2 [ 1 ] ) , ord ( v a l u e 2 [ 2 ] ) , ord ( v a l u e 2
[3]) ,
ord ( v a l u e 3 [ 0 ] ) , ord ( v a l u e 3 [ 1 ] ) , ord ( v a l u e 3 [ 2 ] ) , ord ( v a l u e 3
[3]) ,
ord ( v a l u e 4 [ 0 ] ) , ord ( v a l u e 4 [ 1 ] ) , ord ( v a l u e 4 [ 2 ] ) , ord ( v a l u e 4
[3]) ])

s e l f . s e r . w r i t e ( b u f f e r ) #s e n d s t h e g a i n s

d e f sendSampling ( s e l f , samplingTime ) :

v a l u e = struct . pack ( ’% sf ’ % 1 , samplingTime ) #s p l i t s t h e f l o a t v a l u e i n t o 4

strings
b u f f e r = b y t e a r r a y ( [ command . sendSampling , ord ( v a l u e [ 0 ] ) , ord ( v a l u e [ 1 ] ) , ord ( v a l u e
[ 2 ] ) , ord ( v a l u e [ 3 ] ) ,
0 x05 , 0 x06 , 0 x07 , 0 x08 , 0 x09 , 0 x0a , 0x0b , 0 x0c ,
0x0d , 0 x0e , 0 x0f , 0 x10 , 0 x11 , 0 x12 , 0 x13 , 0 x14 ] )
s e l f . s e r . w r i t e ( b u f f e r ) #s e n d s t h e s a m p l i n g time

# c l e a n up
def close ( s e l f ) :
# close serial
b u f f e r = b y t e a r r a y ( [ command . s t o p C o n t r o l , 0 x01 , 0 x02 , 0 x03 , 0 x04 , 0 x05 , 0 x06 ,
0 x07 , 0 x08 , 0 x09 , 0 x0a , 0x0b , 0 x0c ,
0x0d , 0 x0e , 0 x0f , 0 x10 , 0 x11 , 0 x12 , 0 x13 , 0 x14 ] )
s e l f . s e r . w r i t e ( b u f f e r ) #s e n d s t h e command t o s t a r t t h e r e c e p t i o n
s e l f . ser . flushInput ()
s e l f . ser . close ()

d e f p l o t t i n g ( givenData ) :

fig = plt . figure ()

ax1 = p l t . s u b p l o t ( 3 2 1 )
ax1 . g r i d ( True )
a1 , = ax1 . p l o t ( [ 0 ] , [ 0 ] , c o l o r =(0 , 1 , 0 ) )
p l t . xlim ( 0 , 200)
p l t . y l i m ( −1 , 1 )
p l t . y l a b e l ( ’ DISTANCE [ m ] ’ )
p l t . t i t l e ( ’ CART POSITION ’ , f o n t s i z e =10)

158
 ax4 = p l t . s u b p l o t ( 3 2 2 )
ax4 . g r i d ( True )
a4 , = ax4 . p l o t ( [ 0 ] , [ 0 ] , c o l o r =(0 , 1 , 0 ) )
p l t . xlim ( 0 , 200)
p l t . y l i m ( −1 , 1 )
p l t . y l a b e l ( ’ VELOCITY [ m / s ] ’ )
p l t . t i t l e ( ’ CART VELOCITY ’ , f o n t s i z e =10)

ax2 = p l t . s u b p l o t ( 3 2 3 )
ax2 . g r i d ( True )
a2 , = ax2 . p l o t ( [ ] , [ ] , c o l o r =(1 , 0 , 0 ) )
p l t . xlim ( 0 , 200)
p l t . y l i m (−numpy . pi , numpy . p i )

p l t . t i t l e ( ’ ROD POSITION ’ , f o n t s i z e =10)

p l t . y l a b e l ( ’ ANGLE [ rad ] ’ )

ax5 = p l t . s u b p l o t ( 3 2 4 )
ax5 . g r i d ( True )
a5 , = ax5 . p l o t ( [ ] , [ ] , c o l o r =(1 , 0 , 0 ) )
p l t . xlim ( 0 , 200)
p l t . y l i m (−numpy . pi , numpy . p i )
p l t . t i t l e ( ’ ROD ANGULAR VELOCITY ’ , f o n t s i z e =10)
p l t . y l a b e l ( ’ ANGULAR VELOCITY [ rad / s ] ’ )

ax3 = p l t . s u b p l o t ( 3 2 5 )
ax3 . g r i d ( True )
a3 , = ax3 . p l o t ( [ ] , [ ] , c o l o r =(0 , 0 , 1 ) )
p l t . xlim ( 0 , 200)
p l t . ylim ( −12.2 , 1 2 . 2 )
p l t . x l a b e l ( ’ TIME [ sec ] ’ )
p l t . y l a b e l ( ’ VOLTAGE [ V ] ’ )
p l t . t i t l e ( ’ CONTROL ACTION ’ , f o n t s i z e =10)

anim = a n i m a t i o n . FuncAnimation ( f i g , g i v e n D a t a . update , f a r g s =(a1 , a2 , a3 , a4 , a5 ) ,

i n t e r v a l =50)

p l t . show ( ) # show p l o t

#######################################
######h e r e s t a r t s t h e main program#####
#######################################
if name == " __main__ " :

t e s t C a r l o s = s e r i a l C x ( ’/ dev / ttyACM0 ’ , 3 3 , 2 0 0 , ’ cXrC ’ , 1 1 5 2 0 0 , 0 )

plotting ( testCarlos )
serialCx . close ( testCarlos )

A.5 Pendulum control firmware

A.5.1 Controller-node main code (controller.c)

Listing A.12: Controller-node main code

159
// INVERTED PENDULUM − CONTROLLER NODE
// PERFORMED BY CARLOS XAVIER ROSERO & MANEL VELASCO
// BASED ON LIBRARIES FROM ANTONIO CAMACHO

#include " ee . h "

#include " cpu / pic30 / inc / ee_irqstub . h "
#include " setupCON . h "
#include " uart_dmaCON . h "
#include " e_can1CON . h "

FOSCSEL(FNOSC PRIPLL) ; // Primary (XT, HS, EC) O s c i l l a t o r w i t h PLL

FOSC (OSCIOFNC ON & POSCMD XT) ; // OSC2 Pin Function : OSC2 i s C l o c k Output
// Primary O s c i l l a t o r
Mode : XT C r y s t a l
FWDT(FWDTEN OFF) ; // Watchdog Timer Enabled / d i s a b l e d by
user software
FGS (GCP OFF) ; // D i s a b l e Code P r o t e c t i o n

//DEFINITIONS
#define LED LATBbits . LATB14
#define ON 1
#define OFF 0

//commands t h r o u g h CAN, f o r s e n s o r − a c t u a t o r c o n f i g u r a t i o n

#define cmdStop 0 xf0

#define cmdStart 0 xf1
#define cmdAskSettings 0 xf2
#define cmdInitSensor 0 xf3

#define actuator 0 x00

#define sensorROD 0 x01
#define sensorCART 0 x02
#define controller 0 x03
#define checkingTop 10
#define left 0
#define right 1
#define vTop 12.0
#define pwmTop 0x7fff
#define scaleTop 0.45

#define xDotLimit 0 . 1 // l i m i t s t o know i f t h e s ys t em i s s t a t i c ( i n

c a l i b r a t i o n process )
#define t h e t a D o t L i m i t 0.1
#define p i 3.14159265359

#define downControl 0 // pendulum down ( used f o r cra nk c o n t r o l )

#define u p C o n t r o l 1 // pendulum up ( used f o r i n v e r t e d c o n t r o l )

// l i s t o f commands from UART

#define s t a r t C r a n k C o n t r o l 0 xf0
#define s t a r t I n v e r t e d C o n t r o l 0 xf1
#define s t a r t O t h e r C o n t r o l 0 xf2
#define s t a r t A n o t h e r C o n t r o l 0 xf3
#define l a r g e C a l i b r a t i o n 0 xf4
#define s h o r t C a l i b r a t i o n 0 xf5
#define s t a r t M o d e l i n g 0 xf6
#define s t o p C o n t r o l 0 xfc
#define r e f e r e n c e I n c o m i n g 0 xfd
#define c r a n k G a i n s I n c o m i n g 0 x10
#define i n v e r t e d G a i n s I n c o m i n g 0 x11
#define o t h e r G a i n s I n c o m i n g 0 x12
#define a n o t h e r G a i n s I n c o m i n g 0 x13

160
#define s a m p l i n g I n c o m i n g 0 x14

// l i s t o f d i s c r e t e s t a t e s
#define s t o p p e d 0 x0c
#define c r a n k C o n t r o l 1
#define i n v e r t e d C o n t r o l 2
#define o t h e r C o n t r o l 3
#define a n o t h e r C o n t r o l 4
#define m o d e l i n g 5

#define s t a r t 1
#define s t o p 0

// t y p e s o f messages t h r o u g h CAN
unsigned long configToSensorROD = 0 x00 ;
unsigned long configToSensorCART = 0 x01 ;
unsigned long c o n f i g T o A c t u a t o r = 0 x02 ;
unsigned long synchroToSensorCART = 0 x03 ;
unsigned long b o t h S t a t e s = 0 x04 ;
unsigned long changePWM = 0 x05 ;
unsigned long configFromSensorROD = 0 x06 ;
unsigned long configFromSensorCART = 0 x07 ;
unsigned long c o n f i g F r o m A c t u a t o r = 0 x08 ;

unsigned i n t m s S u p e r v i s i o n = 1 0 0 ; // s u p e r v i s i o n time i n ms

unsigned i n t msSamplingSystem = 1 0 ; // d e f a u l t v a l u e o f rod−s e n s o r ’ s s a m p l i n g time i n ms

unsigned char f l a g A c t u a t o r R x = 0 ;
unsigned char flagSensorRxROD = 0 ;
unsigned char flagSensorRxCAR = 0 ;

unsigned char f l a g T y p e C o n f i g = s t o p ;

unsigned char checkedActuatorRx = 0 ;

unsigned char checkedSensorRxROD = 0 ;
unsigned char checkedSensorRxCAR = 0 ;

unsigned char c h e c k i n g I t e r = 0 ;
unsigned char c h e c k i n g T i m e s = 0 ;

unsigned int valueCarPosition = 0;

unsigned int valueRodPosition = 0 ;
unsigned char dirRod = 0 ;
unsigned char d i r C a r = 0 ;
unsigned int c o n t C a l i b S t a t e = 0 ;
unsigned int c o n t C a l i b T r a n s i t i o n = 0 ;

unsigned char c t r l S t a t e = stopped ;

unsigned char calibrationState = 0;
unsigned char calibrationType = 0;
unsigned char contModeling = 0 ;

//#d e f i n e BIT( x , n ) ( ( ( x ) >> ( n ) ) & 1) // t o a c c e s s a t b i t level

// D e f i n e ECAN Message B u f f e r s
ECAN1MSGBUF ecan1msgBuf attribute ( ( s p a c e (dma) , a l i g n e d (ECAN1 MSG BUF LENGTH∗ 1 6 ) ) ) ;

// CAN Messages i n RAM

mID messageConfigSensorROD ; // message r e c e i v e d t h r o u g h CAN b u s
mID messageConfigSensorCART ; // message r e c e i v e d t h r o u g h CAN b u s
mID m e s s a g e C o n f i g A c t u a t o r ; // message r e c e i v e d t h r o u g h CAN b u s
mID m e s s a g e B o t h S t a t e s ;

161
//mID r x e c a n 1 m e s s a g e 2 ; / /RX C o n t r o l l e r t o a c t u a t o r message
//mID r x e c a n 1 m e s s a g e 3 ; / /RX Controller updates reference ( supervision )
//mID r x e c a n 1 m e s s a g e 4 ; / /RX Not used
mID canTxMessage ; // message t o send t h r o u g h CAN b u s

float volatile t S a m p l i n g = 0 ; // s a m p l i n g as f l o a t v a l u e
float volatile x = 0 , xDot = 0 , t h e t a = 0 , t h e t a D o t = 0 , e r r o r = 0 ;
float volatile integralError = 0;
float volatile x old = 0 , theta old = 0 , error old = 0;
float volatile u = 0;
float volatile r = 0;
float volatile u O f f s e t = 0 . 0 ; // v o l t a g e compensation b e c a u s e o f n o n l i n e a r b e h a v i o r o f
motor

float Kcrank [ 5 ] = { 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ;
float Kinv [ 5 ] = { 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ;
float Kother [ 5 ] = { 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ;
float Kanother [ 5 ] = { 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ;

// r e g i s t e r s used f o r t h e o b s e r v a t i o n
f l o a t v o l a t i l e x h a t = 0 ; // e s t i m a t e d s t a t e s
f l o a t v o l a t i l e xDot hat = 0 ;
float volatile theta hat = 0;
float volatile thetaDot hat = 0;

f l o a t v o l a t i l e A [ 4 ] [ 4 ] = {{ 1 . 0 , 0.0292 , −0.0004 , 0 },
{ 0, 0.9489 , −0.0284 , 0.0003} ,
{ 0, 0.0012 , 1.0078 , 0.0299} ,
{ 0, 0.0817 , 0.5160 ,
0.9943}};

f l o a t v o l a t i l e B [ 4 ] = { 0 . 0 0 0 7 , 0 . 0 4 6 4 , −0.0011 , − 0 . 0 7 4 3 } ; // m a t r i x B ( t r a n s p o s e d )

float volatile L [ 4 ] [ 4 ] = {{0.0313 , −0.0290 , −0.0004 , 0.0000} ,

{0.0582 , −0.0198 , −0.0284 ,
0.0003} ,
{0 , 0.0012 , 0.8015 ,
−0.8890} ,
{0 , 0.0817 , 1.4349 ,
0.7880}};

void sendMessageCAN ( unsigned char l e n g t h , unsigned char id , unsigned char b u f f e r ) ;

void c o n f i g N o d e ( unsigned char node ) ;
void r e a d S e n s o r s ( unsigned char t y p e C o n t r o l ) ;

void c r a n k C o n t r o l R o u t i n e ( )
{
u = −Kcrank [ 0 ] ∗ x − Kcrank [ 1 ] ∗ xDot − Kcrank [ 2 ] ∗ t h e t a − Kcrank [ 3 ] ∗ t h e t a D o t + Kcrank
[4]∗ error ;

i f (u < 0)
u −= u O f f s e t ;
else i f (u > 0)
u += u O f f s e t ;

void i n v e r t e d C o n t r o l R o u t i n e ( )
{
u = −Kinv [ 0 ] ∗ x − Kinv [ 1 ] ∗ xDot − Kinv [ 2 ] ∗ t h e t a − Kinv [ 3 ] ∗ t h e t a D o t + Kinv [ 4 ] ∗ e r r o r ;

i f (u < 0)
u −= u O f f s e t ;

162
 else i f (u > 0)
u += u O f f s e t ;
}

void o t h e r C o n t r o l R o u t i n e ( )
{
u = −Kother [ 0 ] ∗ x − Kother [ 1 ] ∗ xDot − Kother [ 2 ] ∗ t h e t a − Kother [ 3 ] ∗ t h e t a D o t − Kother
[ 4 ] ∗ u ; // e x t e n d e d c o n t r o l l e r

i f (u < 0)
u −= u O f f s e t ;
else i f (u > 0)
u += u O f f s e t ;

void a n o t h e r C o n t r o l R o u t i n e ( )
{
f l o a t v o l a t i l e x hat new , xDot hat new , t h e t a h a t n e w , t h e t a D o t h a t n e w ;

// c o n t r o l a c t i o n
u = −Kanother [ 0 ] ∗ x h a t − Kanother [ 1 ] ∗ xDot hat − Kanother [ 2 ] ∗ t h e t a h a t − Kanother [ 3 ] ∗
thetaDot hat ;

// o b s e r v e r
x h a t n e w = A [ 0 ] [ 0 ] ∗ ( x h a t ) + A [ 0 ] [ 1 ] ∗ xDot hat + A [ 0 ] [ 2 ] ∗ t h e t a h a t + A [ 0 ] [ 3 ] ∗
thetaDot hat
+ B [ 0 ] ∗ u + L [ 0 ] [ 0 ] ∗ ( x−x h a t ) + L [ 0 ] [ 1 ] ∗ ( xDot−xDot hat ) + L [ 0 ] [ 2 ] ∗ ( t h e t a
−t h e t a h a t )
+ L [ 0 ] [ 3 ] ∗ ( thetaDot−t h e t a D o t h a t ) ;

xDot hat new = A [ 1 ] [ 0 ] ∗ ( x h a t ) + A [ 1 ] [ 1 ] ∗ xDot hat + A [ 1 ] [ 2 ] ∗ t h e t a h a t + A [ 1 ] [ 3 ] ∗

thetaDot hat
+ B [ 1 ] ∗ u + L [ 1 ] [ 0 ] ∗ ( x−x h a t ) + L [ 1 ] [ 1 ] ∗ ( xDot−xDot hat ) + L [ 1 ] [ 2 ] ∗ (
t h e t a −t h e t a h a t )
+ L [ 1 ] [ 3 ] ∗ ( thetaDot−t h e t a D o t h a t ) ;

t h e t a h a t n e w = A [ 2 ] [ 0 ] ∗ ( x h a t ) + A [ 2 ] [ 1 ] ∗ xDot hat + A [ 2 ] [ 2 ] ∗ t h e t a h a t + A [ 2 ] [ 3 ] ∗
thetaDot hat
+ B [ 2 ] ∗ u + L [ 2 ] [ 0 ] ∗ ( x−x h a t ) + L [ 2 ] [ 1 ] ∗ ( xDot−xDot hat ) + L [ 2 ] [ 2 ] ∗ (
t h e t a −t h e t a h a t )
+ L [ 2 ] [ 3 ] ∗ ( thetaDot−t h e t a D o t h a t ) ;

t h e t a D o t h a t n e w = A [ 3 ] [ 0 ] ∗ ( x h a t ) + A [ 3 ] [ 1 ] ∗ xDot hat + A [ 3 ] [ 2 ] ∗ t h e t a h a t + A
[ 3 ] [ 3 ] ∗ thetaDot hat
+ B [ 3 ] ∗ u + L [ 3 ] [ 0 ] ∗ ( x−x h a t ) + L [ 3 ] [ 1 ] ∗ ( xDot−xDot hat ) + L
[ 3 ] [ 2 ] ∗ ( t h e t a −t h e t a h a t )
+ L [ 3 ] [ 3 ] ∗ ( thetaDot−t h e t a D o t h a t ) ;

x hat = x hat new ;

xDot hat = xDot hat new ;
theta hat = theta hat new ;
thetaDot hat = thetaDot hat new ;

//u = −Kanother [ 0 ] ∗ x h a t − Kanother [ 1 ] ∗ x D o t h a t − Kanother [ 2 ] ∗ t h e t a h a t −

Kanother [ 3 ] ∗ t h e t a D o t h a t ;

/∗ i f ( u<0)
u −= u O f f s e t ;
e l s e i f ( u>0)
u += u O f f s e t ; ∗/
}

void m o d e l i n g R o u t i n e ( )

163
{
c o n t M o d e l i n g++;

i f ( c o n t M o d e l i n g >= 2 0 )
{
contModeling = 0 ;
i f ( u >= 0 )
u = −6;
else
u = 6;
}
}

void sendMessageCAN ( unsigned char l e n g t h , unsigned char id , unsigned char b u f f e r )

{
C1CTRL1bits .ABAT = 1 ;

while ( C1TR01CONbits .TXREQ0) { } ;

canTxMessage . b u f f e r = b u f f e r ; // B u f f e r number
canTxMessage . f r a m e t y p e = 1 ; //0−>S t d Id , 1−>Ext I d
canTxMessage . i d = i d ; // I d e n t i f i e r ;
canTxMessage . m e s s a g e t y p e = 0 ; //0−>Normal , 1−>Remote Transmit

i f ( length > 8) length = 8 ;

canTxMessage . d a t a l e n g t h = l e n g t h ; // Length o f d a t a (0 t o 8 b y t e s )

ecan1SendMessage (&canTxMessage ) ;
while ( C1TR01CONbits .TXREQ0) { } ;
}

void c o n f i g N o d e ( unsigned char node )

{
unsigned char i = 0 ;

i f ( node == a c t u a t o r )
{
// c o n f i g u r e s t h e a c t u a t o r node
i f ( f l a g T y p e C o n f i g == s t a r t )
canTxMessage . data [ 0 ] = cmdStart ;
e l s e i f ( f l a g T y p e C o n f i g == s t o p )
canTxMessage . data [ 0 ] = cmdStop ;

f o r ( i =1; i <8; i ++)

canTxMessage . data [ i ] = 0 x00 ; // f i l l s the r e s t with n u l l

sendMessageCAN ( 1 , c o n f i g T o A c t u a t o r , 0 ) ; // s e n d s message t h r o u g h CAN

}
e l s e i f ( node == sensorROD )
{
// c o n f i g u r e s t h e ROD s e n s o r node
i f ( f l a g T y p e C o n f i g == s t a r t )
canTxMessage . data [ 0 ] = cmdStart ;
e l s e i f ( f l a g T y p e C o n f i g == s t o p )
canTxMessage . data [ 0 ] = cmdStop ;
canTxMessage . data [ 1 ] = ( unsigned char ) ( msSamplingSystem >>8) ;
canTxMessage . data [ 2 ] = ( unsigned char ) ( msSamplingSystem ) ;

f o r ( i =3; i <8; i ++)

canTxMessage . data [ i ] = 0 x00 ; // f i l l s the r e s t with n u l l

sendMessageCAN ( 3 , configToSensorROD , 0 ) ; // s e n d s message t h r o u g h CAN

164
 }
e l s e i f ( node == sensorCART )
{
// c o n f i g u r e s t h e CART s e n s o r node
i f ( f l a g T y p e C o n f i g == s t a r t )
canTxMessage . data [ 0 ] = cmdStart ;
e l s e i f ( f l a g T y p e C o n f i g == s t o p )
canTxMessage . data [ 0 ] = cmdStop ;

f o r ( i =1; i <8; i ++)

canTxMessage . data [ i ] = 0 x00 ; // f i l l s the r e s t with n u l l

sendMessageCAN ( 1 , configToSensorCART , 0 ) ; // s e n d s message t h r o u g h CAN

}
}

void r e s e t S e n s o r N o d e ( unsigned char node )

{
unsigned char i = 0 ;

i f ( node == sensorROD )
{
// re− i n i t i a l i z e t h e ROD p o s i t i o n s e n s o r

canTxMessage . data [ 0 ] = c m d I n i t S e n s o r ;

f o r ( i =1; i <8; i ++)

canTxMessage . data [ i ] = 0 x00 ; // f i l l s the r e s t with n u l l

sendMessageCAN ( 1 , configToSensorROD , 0 ) ; // s e n d s message t h r o u g h CAN

}
e l s e i f ( node == sensorCART )
{
// re− i n i t i a l i z e s t h e CAR p o s i t i o n s e n s o r
canTxMessage . data [ 0 ] = c m d I n i t S e n s o r ;

i f ( c a l i b r a t i o n T y p e == 1 ) // s h o r t c a l i b r a t i o n
{
canTxMessage . data [ 1 ] = 0 x f 1 ;
}
else
{
canTxMessage . data [ 1 ] = 0 x f 0 ;
}

f o r ( i =2; i <8; i ++)

canTxMessage . data [ i ] = 0 x00 ; // f i l l s the r e s t with n u l l

sendMessageCAN ( 2 , configToSensorCART , 0 ) ; // s e n d s message t h r o u g h CAN

}
}

void u p d a t e S t a t e s ( )
{
v a l u e C a r P o s i t i o n = ( unsigned i n t ) ( m e s s a g e B o t h S t a t e s . data [3] < <8) | ( unsigned i n t ) (
m e s s a g e B o t h S t a t e s . data [ 4 ] ) ;
d i r C a r = m e s s a g e B o t h S t a t e s . data [ 5 ] ;

v a l u e R o d P o s i t i o n = ( unsigned i n t ) ( m e s s a g e B o t h S t a t e s . data [0] < <8) | ( unsigned i n t ) (

m e s s a g e B o t h S t a t e s . data [ 1 ] ) ;
dirRod = m e s s a g e B o t h S t a t e s . data [ 2 ] ;
}

165
void r e a d S e n s o r s ( unsigned char t y p e S e n s o r )
{
// c a l c u l a t e s t h e r e a l v a l u e o f s t a t e s
x = ( float ) ( valueCarPosition ) ∗2.0∗ pi ∗0.0375/4092.0 − 0 . 5 5 ;

i f ( t y p e S e n s o r == downControl )
{
t h e t a = −( f l o a t ) ( v a l u e R o d P o s i t i o n ) ∗ 2 . 0 ∗ p i / 4 0 9 6 . 0 + 1 . 5 ∗ p i ; // 270 d e g r e e s =
1.5∗ pi
}
e l s e i f ( t y p e S e n s o r == u p C o n t r o l ) // i f i n v e r t e d c o n t r o l , t h e t a i s i n t e r p r e t e d i n
d i f f e r e n t way
{
t h e t a = −( f l o a t ) ( v a l u e R o d P o s i t i o n ) ∗ 2 . 0 ∗ p i / 4 0 9 6 . 0 + 0 . 5 ∗ p i ; // 90 d e g r e e s =
0.5∗ pi
}

// c a l c u l a t e s t h e d e r i v a t i v e o f s t a t e s
xDot= ( x − x o l d ) / t S a m p l i n g ;
thetaDot = ( theta − t h e t a o l d ) / tSampling ;
t h e t a o l d = t h e t a ; // o l d v a l u e o f s t a t e s
x old = x ;

// c a l c u l a t e s t h e e r r o r and i n t e g r a l o f e r r o r
error = ( r − x) + error old ;
i n t e g r a l E r r o r = e r r o r ∗ tSampling ;
error old = error ;

// /////////// c o n t r o l s u b r o u t i n e //////////////
void c o n t r o l ( void )
{
unsigned char i = 0 ;
unsigned char s i g n = 0 ;
unsigned i n t v a l u e = 0 ;

//LED = ˜LED;
LED = 1 ;
// i f ( ( v a l u e+=0xF ) > 0 x 7 f f f )
// v a l u e = 0 ;

i f ( c a l i b r a t i o n S t a t e > 0 && c a l i b r a t i o n S t a t e < 6 ) // h e r e c a l i b r a t i o n

{
r e a d S e n s o r s ( downControl ) ;
switch ( c a l i b r a t i o n S t a t e )
{
case 1 : // moving s l o w l y t o l e f t hand
u = −1.9;

// t e s t i n g i f dCar/ d t = 0

i f ( c o n t C a l i b S t a t e++ >= 5 0 )
{
i f ( xDot >= −xDotLimit && xDot <= xDotLimit )
{
i f ( c o n t C a l i b T r a n s i t i o n ++>=50)
{
calibrationState = 2;
contCalibState = 0;
contCalibTransition = 0;
}
}
}

166
 break ;

case 2 : // s t o p p i n g and w a i t i n g u n t i l t h e rod s t o p s o s c i l l a t i n g &

// s e n d i n g c a l i b r a t i o n message t o c a r t
u = 0;

// t e s t i n g i f dRod/ d t = 0
// t h i s method ( i n c r e a s i n g and d e c r e a s i n g c o u n t e r i s
a p p l i e d , b e c a u s e each time t h e rod
// c h a n g e s t h e d i r e c t i o n , dRod/ d t = 0 . We must t o have a t
l e a s t 50 c o n s e c u t i v e d e r i v a t i v e s e q u a l
// t o z e r o , t o jump t o t h e n e x t d i s c r e t e s t a t e
i f ( c a l i b r a t i o n T y p e == 0 ) // l a r g e c a l i b r a t i o n
{
i f ( t h e t a D o t >= −t h e t a D o t L i m i t && t h e t a D o t <=
thetaDotLimit )
{
i f ( c o n t C a l i b T r a n s i t i o n++ >= 5 0 )
{
calibrationState = 3;
flagSensorRxCAR = 0 ; // c l e a r
c a l i b r a t i o n acknowledgment
flag
contCalibTransition = 0;
r e s e t S e n s o r N o d e ( sensorCART ) ; //
s e n d s message
}
}
else
{
i f ( c o n t C a l i b T r a n s i t i o n >0)
c o n t C a l i b T r a n s i t i o n −−;
}
}
e l s e i f ( c a l i b r a t i o n T y p e == 1 ) // s h o r t c a l i b r a t i o n
{
calibrationState = 3;
flagSensorRxCAR = 0 ; // c l e a r c a l i b r a t i o n
acknowledgment f l a g
contCalibTransition = 0;
r e s e t S e n s o r N o d e ( sensorCART ) ; // s e n d s message
}
break ;

case 3 : // w a i t i n g f o r acknowledgment from c a r t s e n s o r &

// s e n d i n g c a l i b r a t i o n message t o rod
i f ( flagSensorRxCAR == 1 )
{
flagSensorRxCAR = 0 ;
calibrationState = 4;
flagSensorRxROD = 0 ;
r e s e t S e n s o r N o d e ( sensorROD ) ; // s e n d s message
}
break ;

case 4 : // w a i t i n g f o r acknowledgment from rod s e n s o r

i f ( flagSensorRxROD == 1 )
{
flagSensorRxROD = 0 ;
flagSensorRxROD = 0 ;

i f ( c a l i b r a t i o n T y p e == 1 ) // s h o r t c a l i b r a t i o n
{

167
 c a l i b r a t i o n S t a t e = 6 ; // d i r e c t l y t o
successful calibration
}
e l s e i f ( c a l i b r a t i o n T y p e == 0 ) // l a r g e
calibration
{
c a l i b r a t i o n S t a t e = 5 ; // n e x t s t a t e
}
}
break ;

case 5 : // moving s l o w l y t o t h e r i g h t u n t i l t h e c e n t e r o f t h e
t r a c k ( more or l e s s ) −−> 0 . 0

u = 1.9;

i f ( c o n t C a l i b S t a t e++ >= 5 0 )
{
i f ( x >= −0.03) // a b o u t p o s i t i o n 0
{
u = 0;
i f ( c o n t C a l i b T r a n s i t i o n ++>= 5 0 ) // w a i t s
in order to v e r i f y the p o s i t i o n
t h r o u g h t h e GUI
{
// o n l y f o r
v i s u a l i z a t i o n purposes
calibrationState = 6;
contCalibState = 0;
contCalibTransition = 0;
}
}
}
break ;

default :

break ;
}
}
e l s e i f ( c a l i b r a t i o n S t a t e == 6 ) // h e r e r e a l c o n t r o l , o n l y i f d e v i c e i s a l r e a d y
calibrated
{
switch ( c t r l S t a t e )
{
case s t o p p e d :
u = 0;
break ;

case c r a n k C o n t r o l : // cran k c o n t r o l
r e a d S e n s o r s ( downControl ) ;
i f (− s c a l e T o p <= x && x <= s c a l e T o p )
{
crankControlRoutine () ;
}
e l s e i f ( x<−s c a l e T o p )
{
u = vTop ;
}
e l s e i f ( x>s c a l e T o p )
{
u = −vTop ;
}
break ;

168
case i n v e r t e d C o n t r o l : // i n v e r t e d c o n t r o l
readSensors ( upControl ) ;
i f (− s c a l e T o p <= x && x <= s c a l e T o p )
{
invertedControlRoutine () ;
}
e l s e i f ( x<−s c a l e T o p )
{
u = vTop ;
}
e l s e i f ( x>s c a l e T o p )
{
u = −vTop ;
}
break ;

case o t h e r C o n t r o l : // o t h e r c o n t r o l
readSensors ( upControl ) ;
i f (− s c a l e T o p <= x && x <= s c a l e T o p )
{
otherControlRoutine () ;
}
e l s e i f ( x<−s c a l e T o p )
{
u = vTop ;
}
e l s e i f ( x>s c a l e T o p )
{
u = −vTop ;
}
break ;

case a n o t h e r C o n t r o l : // a n o t h e r c o n t r o l
readSensors ( upControl ) ;
i f (− s c a l e T o p <= x && x <= s c a l e T o p )
{
anotherControlRoutine () ;
}
e l s e i f ( x<−s c a l e T o p )
{
u = vTop ;
}
e l s e i f ( x>s c a l e T o p )
{
u = −vTop ;
}
break ;

case m o d e l i n g : // a p p l i e s s i g n a l t o t a k e d a t a
r e a d S e n s o r s ( downControl ) ;
i f (− s c a l e T o p <= x && x <= s c a l e T o p )
{
modelingRoutine ( ) ;
}
e l s e i f ( x<−s c a l e T o p )
{
u = vTop ;
}
e l s e i f ( x>s c a l e T o p )
{
u = −vTop ;
}
break ;

169
 }
}
else
{
u = 0;
}

i f ( c a l i b r a t i o n S t a t e != 3 && c a l i b r a t i o n S t a t e != 4 )
{
// t r a n s l a t i n g u v a l u e i n t o pwm v a l u e w i t h s i g n

i f (u > 0)
{
i f ( u > vTop ) // hardware v o l t a g e c o n s t r a i n t
u = vTop ;

sign = right ;
v a l u e = ( unsigned i n t ) ( ( u/vTop ) ∗pwmTop) ; // c o n t r o l ∗ (pwmTop) /vTop
}
else
{
i f ( u < −vTop ) // hardware v o l t a g e c o n s t r a i n t
u = −vTop ;

sign = l e f t ;
v a l u e = ( unsigned i n t ) ( −(u/vTop ) ∗pwmTop) ; // c o n t r o l ∗ (pwmTop) /vTop
}

// s e n d i n g pwm and d i r e c t i o n t h r o u g h CAN

canTxMessage . data [ 0 ] = ( unsigned char ) ( v a l u e ) ;
canTxMessage . data [ 1 ] = ( unsigned char ) ( v a l u e >> 8 ) ;
canTxMessage . data [ 2 ] = s i g n ;

f o r ( i =3; i <8; i ++)

canTxMessage . data [ i ] = 0 x00 ; // f i l l s the r e s t with n u l l

sendMessageCAN ( 3 , changePWM , 0 ) ; // s e n d s message t h r o u g h CAN

}
LED = 0 ;
}

TASK( T a s k C o n t r o l l e r )
{
control () ;
//LED = ˜LED;
/∗
LATBbits . LATB10 = 1 ; //To g e t time w i t h t h e o s c i l l o s c o p e
LATBbits . LATB10 = 0 ; //To g e t time w i t h t h e o s c i l l o s c o p e
∗/
}

TASK( T a s k S u p e r v i s i o n )
{
s t a t i c unsigned long sysTime =0;
s t a t i c unsigned char ∗ p o i n t e r u = ( unsigned char ∗ )&u ;
s t a t i c unsigned char ∗ p o i n t e r x = ( unsigned char ∗ )&x ;
s t a t i c unsigned char ∗ p o i n t e r t h e t a = ( unsigned char ∗ )&t h e t a ;
s t a t i c unsigned char ∗ p o i n t e r x D o t = ( unsigned char ∗ )&xDot ;
s t a t i c unsigned char ∗ p o i n t e r t h e t a D o t = ( unsigned char ∗ )&t h e t a D o t ;
s t a t i c unsigned char ∗ p o i n t e r e r r o r = ( unsigned char ∗ )&e r r o r ;
s t a t i c unsigned char ∗ p o i n t e r x h a t = ( unsigned char ∗ )&x h a t ;
s t a t i c unsigned char ∗ p o i n t e r x D o t h a t = ( unsigned char ∗ )&xDot hat ;
s t a t i c unsigned char ∗ p o i n t e r t h e t a h a t = ( unsigned char ∗ )&t h e t a h a t ;
s t a t i c unsigned char ∗ p o i n t e r t h e t a D o t h a t = ( unsigned char ∗ )&t h e t a D o t h a t ;

170
LATBbits . LATB10 = 1 ; //To g e t time w i t h t h e o s c i l l o s c o p e

sysTime=GetTime ( ) ; // Get s ys t em time (EDF S c h e d u l e r )

OutBuffer [ 0 ] = ( unsigned char ) ( sysTime >>24) ; // 4 t h byte of unsigned long

OutBuffer [ 1 ] = ( unsigned char ) ( sysTime >>16) ; // 3 rd byte of unsigned long
OutBuffer [ 2 ] = ( unsigned char ) ( sysTime >>8) ; // 2nd byte of unsigned long
OutBuffer [ 3 ] = ( unsigned char ) sysTime ; // 1 s t byte of unsigned long

i f ( c t r l S t a t e == a n o t h e r C o n t r o l )
{
OutBuffer [ 4 ] = ∗ p o i n t e r x h a t ; // 4 t h b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 5 ] = ∗ ( p o i n t e r x h a t +1) ; // 3 rd b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 6 ] = ∗ ( p o i n t e r x h a t +2) ; // 2nd b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 7 ] = ∗ ( p o i n t e r x h a t +3) ; // 1 s t b y t e o f f l o a t (32 b i t s )
OutBuffer [ 8 ] = ∗ p o i n t e r t h e t a h a t ; // 4 t h b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 9 ] = ∗ ( p o i n t e r t h e t a h a t +1) ; // 3 rd b y t e o f f l o a t (32 b i t s
)
O u t B u f f e r [ 1 0 ] = ∗ ( p o i n t e r t h e t a h a t +2) ; // 2nd b y t e o f f l o a t (32
bits )
O u t B u f f e r [ 1 1 ] = ∗ ( p o i n t e r t h e t a h a t +3) ; // 1 s t b y t e o f f l o a t (32
bits )
OutBuffer [ 1 6 ] = ∗ p o i n t e r x D o t h a t ; // 4 t h b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 1 7 ] = ∗ ( p o i n t e r x D o t h a t +1) ; // 3 rd b y t e o f f l o a t (32 b i t s
)
O u t B u f f e r [ 1 8 ] = ∗ ( p o i n t e r x D o t h a t +2) ; // 2nd b y t e o f f l o a t (32 b i t s
)
O u t B u f f e r [ 1 9 ] = ∗ ( p o i n t e r x D o t h a t +3) ; // 1 s t b y t e o f f l o a t (32 b i t s
)
OutBuffer [ 2 0 ] = ∗ p o i n t e r t h e t a D o t h a t ; // 4 t h b y t e o f f l o a t (32
bits )
O u t B u f f e r [ 2 1 ] = ∗ ( p o i n t e r t h e t a D o t h a t +1) ; // 3 rd b y t e o f f l o a t (32
bits )
O u t B u f f e r [ 2 2 ] = ∗ ( p o i n t e r t h e t a D o t h a t +2) ; // 2nd b y t e o f f l o a t (32
bits )
O u t B u f f e r [ 2 3 ] = ∗ ( p o i n t e r t h e t a D o t h a t +3) ; // 1 s t b y t e o f f l o a t (32
bits )

}
else
{
OutBuffer [ 4 ] = ∗ p o i n t e r x ; // 4 t h b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 5 ] = ∗ ( p o i n t e r x +1) ; // 3 rd b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 6 ] = ∗ ( p o i n t e r x +2) ; // 2nd b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 7 ] = ∗ ( p o i n t e r x +3) ; // 1 s t b y t e o f f l o a t (32 b i t s )
OutBuffer [ 8 ] = ∗ p o i n t e r t h e t a ; // 4 t h b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 9 ] = ∗ ( p o i n t e r t h e t a +1) ; // 3 rd b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 1 0 ] = ∗ ( p o i n t e r t h e t a +2) ; // 2nd b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 1 1 ] = ∗ ( p o i n t e r t h e t a +3) ; // 1 s t b y t e o f f l o a t (32 b i t s )
OutBuffer [ 1 6 ] = ∗ pointer xDot ; // 4 t h b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 1 7 ] = ∗ ( p o i n t e r x D o t +1) ; // 3 rd b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 1 8 ] = ∗ ( p o i n t e r x D o t +2) ; // 2nd b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 1 9 ] = ∗ ( p o i n t e r x D o t +3) ; // 1 s t b y t e o f f l o a t (32 b i t s )
OutBuffer [ 2 0 ] = ∗ p o i n t e r t h e t a D o t ; // 4 t h b y t e o f f l o a t (32 b i t s )
O u t B u f f e r [ 2 1 ] = ∗ ( p o i n t e r t h e t a D o t +1) ; // 3 rd b y t e o f f l o a t (32 b i t s
)
O u t B u f f e r [ 2 2 ] = ∗ ( p o i n t e r t h e t a D o t +2) ; // 2nd b y t e o f f l o a t (32 b i t s
)
O u t B u f f e r [ 2 3 ] = ∗ ( p o i n t e r t h e t a D o t +3) ; // 1 s t b y t e o f f l o a t (32 b i t s
)
}

OutBuffer [ 1 2 ] = ∗ p o i n t e r u ; // 4 t h b y t e o f f l o a t (32 b i t s )

171
 OutBuffer [ 1 3 ] = ∗ ( p o i n t e r u +1) ; // 3 rd b y t e o f f l o a t (32 b i t s )
OutBuffer [ 1 4 ] = ∗ ( p o i n t e r u +2) ; // 2nd b y t e o f f l o a t (32 b i t s )
OutBuffer [ 1 5 ] = ∗ ( p o i n t e r u +3) ; // 1 s t b y t e o f f l o a t (32 b i t s )
OutBuffer [ 2 4 ] = ∗ pointer error ; // 4 t h b y t e o f f l o a t (32 b i t s )
OutBuffer [ 2 5 ] = ∗ ( p o i n t e r e r r o r +1) ; // 3 rd b y t e o f f l o a t (32 b i t s )
OutBuffer [ 2 6 ] = ∗ ( p o i n t e r e r r o r +2) ; // 2nd b y t e o f f l o a t (32 b i t s )
OutBuffer [ 2 7 ] = ∗ ( p o i n t e r e r r o r +3) ; // 1 s t b y t e o f f l o a t (32 b i t s )
OutBuffer [ 2 8 ] = c a l i b r a t i o n S t a t e ; //
OutBuffer [ 2 9 ] = ’c ’ ;
OutBuffer [ 3 0 ] = ’X ’ ;
OutBuffer [ 3 1 ] = ’r ’ ;
OutBuffer [ 3 2 ] = ’C ’ ;

// Force s e n d i n g d a t a
DMA4CONbits .CHEN = 1 ; // Re−e n a b l e DMA4 Channel
DMA4REQbits .FORCE = 1 ; // Manual mode : Kick−s t a r t t h e
f i r s t transfer

i f ( c a l i b r a t i o n S t a t e == 6 && c t r l S t a t e == s t o p p e d )
{
CancelAlarm ( A l a r m S u p e r v i s i o n ) ; // k i l l s i t s e l f
f l a g T y p e C o n f i g = s t o p ; // remote s e t t i n g s w i l l s t o p nodes
A c t i v a t e T a s k ( TaskRemoteConfig ) ; // t r i g g e r t a s k t o change remote s e t t i n g s
}
LATBbits . LATB10 = 0 ; //To g e t time w i t h t h e o s c i l l o s c o p e
}

TASK( TaskRemoteConfig ) // change remote s e t t i n g s

{
f l a g A c t u a t o r R x = 0 ; // c l e a r f l a g s
flagSensorRxROD = 0 ;
flagSensorRxCAR = 0 ;
c h e c k i n g I t e r = 0 ; // c l e a r c o u n t e r s
checkingTimes = 0 ;

// s t a t u s F l a g b i t s . c o n f A c t u a t o r R x = 0 ;
i f ( checkedSensorRxROD == 0 )
{
c o n f i g N o d e ( sensorROD ) ;
}

i f ( checkedSensorRxCAR == 0 )
{
c o n f i g N o d e ( sensorCART ) ;
}

i f ( checkedActuatorRx == 0 )
{
configNode ( actuator ) ;
}

SetRelAlarm ( TaskCheckConfig , 1 0 0 0 , 5 0 0 ) ;
}

TASK( TaskCheckConfig ) // v e r i f i e s remote s e t t i n g s

{
// c h e c k messages
i f ( flagSensorRxROD == 1 )
{
flagSensorRxROD = 0 ; // c l e a r f l a g
checkedSensorRxROD = 1 ;
}

172
 i f ( flagSensorRxCAR == 1 )
{
flagSensorRxCAR = 0 ;
checkedSensorRxCAR = 1 ;
}

i f ( f l a g A c t u a t o r R x == 1 )
{
flagActuatorRx = 0;
checkedActuatorRx = 1 ;
}

i f ( checkedSensorRxROD == 1 && checkedSensorRxCAR == 1 && checkedActuatorRx == 1 )

{
checkedSensorRxROD = 0 ;
checkedSensorRxCAR = 0 ;
checkedActuatorRx = 0 ;

// k i l l s i t s e l f
CancelAlarm ( TaskCheckConfig ) ;
}
e l s e i f (++ c h e c k i n g I t e r >= checkingTop )
{
c h e c k i n g I t e r = 0 ; // c l e a r s c o u n t e r
// t r i g g e r t a s k t o change remote s e t t i n g s
A c t i v a t e T a s k ( TaskRemoteConfig ) ;
}
}

/∗ CAN b u s 1 I n t e r r u p t , ISR2 t y p e ∗/
ISR2 ( C 1 I n t e r r u p t )
{
I F S 2 b i t s . C1IF = 0 ; // c l e a r i n t e r r u p t f l a g

/∗ Transmission i n t e r r u p t ( n o t h i n g t o be done b u t c l e a r f l a g ) ∗/
i f ( C1INTFbits . TBIF )
{
C1INTFbits . TBIF = 0 ;
}

/∗ R e c e p t i o n i n t e r r u p t , d i f f e r e n t code f o r d i f f e r e n t f i l t e r e d i d ’ s ∗/
i f ( C1INTFbits . RBIF )
{
// F i l t e r 1 : configFromSensorROD
i f ( C1RXFUL1bits . RXFUL1 == 1 )
{
// T e l l s rxECAN1 t h e b u f f e r t o p a s s from DMA t o RAM
messageConfigSensorROD . b u f f e r = 1 ;
C1RXFUL1bits . RXFUL1 = 0 ;
rxECAN1(&messageConfigSensorROD ) ;
flagSensorRxROD = 1 ;
}

// F i l t e r 2 : configFromSensorCART
i f ( C1RXFUL1bits . RXFUL2 == 1 )
{
// T e l l s rxECAN1 t h e b u f f e r t o p a s s from DMA t o RAM
messageConfigSensorCART . b u f f e r = 2 ;
C1RXFUL1bits . RXFUL2 = 0 ;
rxECAN1(&messageConfigSensorCART ) ;
flagSensorRxCAR = 1 ;
}

// F i l t e r 3 : c o n f i g F r o m A c t u a t o r

173
 i f ( C1RXFUL1bits . RXFUL3 == 1 )
{
// T e l l s rxECAN1 t h e b u f f e r t o p a s s from DMA t o RAM
messageConfigActuator . b u f f e r = 3 ;
C1RXFUL1bits . RXFUL3 = 0 ;
rxECAN1(& m e s s a g e C o n f i g A c t u a t o r ) ;
flagActuatorRx = 1;
}

// F i l t e r 4 : b o t h S t a t e s
i f ( C1RXFUL1bits . RXFUL4 == 1 )
{
// T e l l s rxECAN1 t h e b u f f e r t o p a s s from DMA t o RAM
messageBothStates . b u f f e r = 4 ;
C1RXFUL1bits . RXFUL4 = 0 ; // c l e a r f l a g
rxECAN1(& m e s s a g e B o t h S t a t e s ) ;
updateStates () ;
ActivateTask ( TaskController ) ;
}
C1INTFbits . RBIF = 0 ; // c l e a r f l a g
}
}

union h e x 2 f l o a t // used t o j o i n 4 b y t e s i n t o 1 f l o a t . I t i s a l s o used o t h e r method h e r e t o

do
{ // t h e same ( w i t h p o i n t e r s ) , b u t l e t ’ s t r y . . .
unsigned char data [ 4 ] ;
f l o a t floatNumber ;
} hex2float ;

ISR2 ( DMA5Interrupt ) //UART RX i n t e r r u p t i o n

{
unsigned char i , j ;

switch ( I n B u f f e r A [ 0 ] )
{
case s t a r t C r a n k C o n t r o l : // s t a r t s cr ank c o n t r o l
flagTypeConfig = s t a r t ;
SetRelAlarm ( A l a r m S u p e r v i s i o n , 1 0 0 0 , m s S u p e r v i s i o n ) ; // S u p e r v i s i o n
a c t i v a t e s e v e r y 10∗ msSampling
c t r l S t a t e = crankControl ;
error = 0;
error old = 0;
i f ( c a l i b r a t i o n S t a t e != 0 && c a l i b r a t i o n S t a t e != 6 )
c a l i b r a t i o n S t a t e = 0 ; // c l e a r t h e f l a g , p e r h a p s l e f t i n
different state ,
break ; // i f a c a n c e l e d
c a l i b r a t i o n c o u l d happen

case s t a r t I n v e r t e d C o n t r o l : // s t a r t s i n v e r t e d c o n t r o l
SetRelAlarm ( A l a r m S u p e r v i s i o n , 1 0 0 0 , m s S u p e r v i s i o n ) ; // S u p e r v i s i o n
a c t i v a t e s e v e r y 10∗ msSampling
ctrlState = invertedControl ;
error = 0;
error old = 0;
i f ( c a l i b r a t i o n S t a t e != 0 && c a l i b r a t i o n S t a t e != 6 )
c a l i b r a t i o n S t a t e = 0 ; // c l e a r t h e f l a g , p e r h a p s l e f t i n
different state ,
break ; // i f a c a n c e l e d
c a l i b r a t i o n c o u l d happen

case s t a r t O t h e r C o n t r o l : // s t a r t s o t h e r c o n t r o l
SetRelAlarm ( A l a r m S u p e r v i s i o n , 1 0 0 0 , m s S u p e r v i s i o n ) ; // S u p e r v i s i o n
a c t i v a t e s e v e r y 10∗ msSampling

174
 c t r l S t a t e = otherControl ;
error = 0;
error old = 0;
i f ( c a l i b r a t i o n S t a t e != 0 && c a l i b r a t i o n S t a t e != 6 )
c a l i b r a t i o n S t a t e = 0 ; // c l e a r t h e f l a g , p e r h a p s l e f t i n
different state ,
break ; // i f a c a n c e l e d
c a l i b r a t i o n c o u l d happen

case s t a r t A n o t h e r C o n t r o l : // s t a r t s a n o t h e r c o n t r o l
SetRelAlarm ( A l a r m S u p e r v i s i o n , 1 0 0 0 , m s S u p e r v i s i o n ) ; // S u p e r v i s i o n
a c t i v a t e s e v e r y 10∗ msSampling
c t r l S t a t e = anotherControl ;
error = 0;
error old = 0;
r e a d S e n s o r s ( u p C o n t r o l ) ; // i n i t i a l i z e s e s t i m a t i o n s t a t e s w i t h
v a l u e s from s e n s o r s
x hat = x ;
xDot hat = xDot ;
theta hat = theta ;
thetaDot hat = thetaDot ;

i f ( c a l i b r a t i o n S t a t e != 0 && c a l i b r a t i o n S t a t e != 6 )
c a l i b r a t i o n S t a t e = 0 ; // c l e a r t h e f l a g , p e r h a p s l e f t i n
different state ,
break ; // i f a c a n c e l e d
c a l i b r a t i o n c o u l d happen

case s t a r t M o d e l i n g : // s t a r t s m o d e l i n g
SetRelAlarm ( A l a r m S u p e r v i s i o n , 1 0 0 0 , m s S u p e r v i s i o n ) ; // S u p e r v i s i o n
a c t i v a t e s e v e r y 10∗ msSampling
contModeling = 0 ;
c t r l S t a t e = modeling ;
r = 0;
error = 0;
error old = 0;
i f ( c a l i b r a t i o n S t a t e != 0 && c a l i b r a t i o n S t a t e != 6 )
c a l i b r a t i o n S t a t e = 0 ; // c l e a r t h e f l a g , p e r h a p s l e f t i n
different state ,
break ; // i f a c a n c e l e d
c a l i b r a t i o n c o u l d happen

case l a r g e C a l i b r a t i o n : // s t a r t s l a r g e c a l i b r a t i o n
SetRelAlarm ( A l a r m S u p e r v i s i o n , 1 0 0 0 , m s S u p e r v i s i o n ) ; // S u p e r v i s i o n
a c t i v a t e s e v e r y 10∗ msSampling
c t r l S t a t e = stopped ;
calibrationState = 1;
calibrationType = 0;
c o n t C a l i b S t a t e = 0 ; // i n i t i a l i z e s c o u n t e r t o g i v e a d e l a y b e f o r e
s t a r t t e s t i n g the d e r i v a t i v e s
c o n t C a l i b T r a n s i t i o n = 0 ; // i n i t i a l i z e s c o u n t e r t o remain t h e
c o n d i t i o n b e f o r e jumping t o t h e n e x t d i s c r e t e s t a t e
break ;

case s h o r t C a l i b r a t i o n : // s t a r t s s h o r t c a l i b r a t i o n
SetRelAlarm ( A l a r m S u p e r v i s i o n , 1 0 0 0 , m s S u p e r v i s i o n ) ; // S u p e r v i s i o n
a c t i v a t e s e v e r y 10∗ msSampling
c t r l S t a t e = stopped ;
calibrationState = 2;
calibrationType = 1;
c o n t C a l i b S t a t e = 0 ; // i n i t i a l i z e s c o u n t e r t o g i v e a d e l a y b e f o r e
s t a r t t e s t i n g the d e r i v a t i v e s
c o n t C a l i b T r a n s i t i o n = 0 ; // i n i t i a l i z e s c o u n t e r t o remain t h e
c o n d i t i o n b e f o r e jumping t o t h e n e x t d i s c r e t e s t a t e

175
 break ;

case r e f e r e n c e I n c o m i n g : // r e f e r e n c e incoming
f o r ( i =0; i <=3; i ++)
h e x 2 f l o a t . data [ i ] = I n B u f f e r A [ i + 1 ] ;
r = h e x 2 f l o a t . floatNumber ;
i f ( r < −s c a l e T o p )
r = −s c a l e T o p ;
else i f ( r > scaleTop )
r = scaleTop ;
break ;

case c r a n k G a i n s I n c o m i n g : // g a i n s incoming
f o r ( j =0; j <=4; j ++)
{
f o r ( i =0; i <=3; i ++)
h e x 2 f l o a t . data [ i ] = I n B u f f e r A [ j ∗4+ i + 1 ] ;
Kcrank [ j ] = h e x 2 f l o a t . floatNumber ;
}
break ;

case i n v e r t e d G a i n s I n c o m i n g : // g a i n s incoming
f o r ( j =0; j <=4; j ++)
{
f o r ( i =0; i <=3; i ++)
h e x 2 f l o a t . data [ i ] = I n B u f f e r A [ j ∗4+ i + 1 ] ;
Kinv [ j ] = h e x 2 f l o a t . floatNumber ;
}
break ;

case o t h e r G a i n s I n c o m i n g : // g a i n s incoming
f o r ( j =0; j <=4; j ++)
{
f o r ( i =0; i <=3; i ++)
h e x 2 f l o a t . data [ i ] = I n B u f f e r A [ j ∗4+ i + 1 ] ;
Kother [ j ] = h e x 2 f l o a t . floatNumber ;
}

break ;

case a n o t h e r G a i n s I n c o m i n g : // g a i n s incoming
f o r ( j =0; j <=4; j ++)
{
f o r ( i =0; i <=3; i ++)
h e x 2 f l o a t . data [ i ] = I n B u f f e r A [ j ∗4+ i + 1 ] ;
Kanother [ j ] = h e x 2 f l o a t . floatNumber ;
}
break ;

case s a m p l i n g I n c o m i n g : // s a m p l i n g time incoming

f o r ( i =0; i <=3; i ++)
h e x 2 f l o a t . data [ i ] = I n B u f f e r A [ i + 1 ] ;
t S a m p l i n g = h e x 2 f l o a t . floatNumber ∗ 0 . 0 0 1 ; // a c t u a l v a l u e i n ms f o r
c a l c u l a t i o n purposes
msSamplingSystem = ( unsigned i n t ) ( h e x 2 f l o a t . floatNumber ) ; // v a l u e
for tx purposes

f l a g T y p e C o n f i g = s t a r t ; // remote s e t t i n g s w i l l s t a r t up nodes
A c t i v a t e T a s k ( TaskRemoteConfig ) ; // t r i g g e r t a s k t o change remote
settings

break ;

case s t o p C o n t r o l : // s t a r t s s h o r t i n v e r t e d c a l i b r a t i o n

176
 CancelAlarm ( A l a r m S u p e r v i s i o n ) ; // k i l l s s u p e r v i s i o n
f l a g T y p e C o n f i g = s t o p ; // remote s e t t i n g s w i l l s t o p nodes
A c t i v a t e T a s k ( TaskRemoteConfig ) ; // t r i g g e r t a s k t o change remote
settings
c t r l S t a t e = stopped ;

break ;
}

I F S 3 b i t s . DMA5IF = 0 ; // C l e a r t h e DMA1 I n t e r r u p t F l a g
}

/∗ main f u n c t i o n , o n l y t o i n i t i a l i z e s o f t w a r e and hardware ,

f i r e alarms , and implement b a c k g r o u n d a c t i v i t i e s ∗/
i n t main ( void )
{

S y s i n i t ( ) ; // I n i t i a l i z e c l o c k , d e v i c e s and p e r i p h e r a l s

t S a m p l i n g = ( f l o a t ) ( msSamplingSystem ) ∗ 0 . 0 0 1 ; // s a m p l i n g as f l o a t v a l u e
/∗
flagTypeConfig = s t a r t ;
A c t i v a t e T a s k ( TaskRemoteConfig ) ; // t r i g g e r t a s k t o change remote s e t t i n g s
∗/

/∗ F o r e v e r l o o p : b a c k g r o u n d a c t i v i t i e s ( i f any ) s h o u l d go h e r e ∗/
for ( ; ; ) ;
}

A.5.2 Actuator-node main code (actuator.c)

Listing A.13: Actuator-node main code

// INVERTED PENDULUM − MOTOR ACTUATOR NODE
// PERFORMED BY CARLOS XAVIER ROSERO & MANEL VELASCO
// BASED ON LIBRARIES FROM ANTONIO CAMACHO

#include " ee . h "

#include " cpu / pic30 / inc / ee_irqstub . h "
#include " setupACT . h "
#include " uart_dmaACT . h "
#include " e_can1ACT . h "

FOSCSEL(FNOSC PRIPLL) ; // Primary (XT, HS, EC) O s c i l l a t o r w i t h PLL

FOSC (OSCIOFNC ON & POSCMD XT) ; // OSC2 Pin Function : OSC2 i s C l o c k Output
// Primary O s c i l l a t o r
Mode : XT C r y s t a l
FWDT(FWDTEN OFF) ; // Watchdog Timer Enabled / d i s a b l e d by
user software
FGS (GCP OFF) ; // D i s a b l e Code P r o t e c t i o n

//DEFINITIONS
#define LED LATBbits . LATB14
#define d i r O u t p u t LATEbits . LATE2
#define ON 1
#define OFF 0

177
#define l e f t 0
#define r i g h t 1

#define standby 0
#define working 1
#define s e t t i n g 2

//commands t h r o u g h CAN, f o r c o n f i g u r a t i o n
#define cmdStop 0 xf0
#define cmdStart 0 xf1
#define cmdAskSettings 0 xf2
#define c m d I n i t S e n s o r 0 xf3

/∗ Variables ∗/

// t y p e s o f messages t h r o u g h CAN
unsigned long configToSensorROD = 0 x00 ;
unsigned long configToSensorCART = 0 x01 ;
unsigned long c o n f i g T o A c t u a t o r = 0 x02 ;
unsigned long synchroToSensorCART = 0 x03 ;
unsigned long b o t h S t a t e s = 0 x04 ;
unsigned long changePWM = 0 x05 ;
unsigned long configFromSensorROD = 0 x06 ;
unsigned long configFromSensorCART = 0 x07 ;
unsigned long c o n f i g F r o m A c t u a t o r = 0 x08 ;

unsigned char cntTaskLed = 0 ;

unsigned char workStateLed = standby ;

unsigned i n t m s S u p e r v i s i o n A c t = 5 0 0 ; // d e f a u l t v a l u e o f sy s te m ’ s s u p e r v i s i o n time : 500ms

// D e f i n e ECAN Message B u f f e r s
ECAN1MSGBUF ecan1msgBuf a t t r i b u t e ( ( s p a c e (dma) , a l i g n e d (ECAN1 MSG BUF LENGTH∗ 1 6 ) ) ) ;
mID messageChangePWM ; //RX C o n t r o l l e r t o a c t u a t o r message
mID m e s s a g e C o n f i g A c t u a t o r ; //RX C o n t r o l l e r t o a c t u a t o r message

mID canTxMessage ; // message t o send t h r o u g h CAN b u s

void sendMessageCAN ( unsigned char l e n g t h , unsigned char id , unsigned char b u f f e r ) ;

void sendACK ( )
{
unsigned char i = 0 ;

f o r ( i =0; i <8; i ++)

canTxMessage . data [ i ] = 0 x00 ; // f i l l s the r e s t with n u l l

sendMessageCAN ( 0 , c o n f i g F r o m A c t u a t o r , 0 ) ; // s e n d s message t h r o u g h CAN

}

void s e n d S e t t i n g s ( )
{
unsigned char i = 0 ;

canTxMessage . data [ 0 ] = cmdStart ;

f o r ( i =1; i <8; i ++)

canTxMessage . data [ i ] = 0 x00 ; // f i l l s the r e s t with n u l l

sendMessageCAN ( 1 , c o n f i g F r o m A c t u a t o r , 0 ) ; // s e n d s message t h r o u g h CAN

}

/∗ t a s k s ∗/

178
unsigned i n t PWMvalue = 0 ;

void u p d a t e A c t u a t o r ( void )
{
//LED = ˜LED;

LED = 1 ;
PWMvalue = ( messageChangePWM . data [1] < <8) | messageChangePWM . data [ 0 ] ;
PDC1 = PWMvalue ;

i f ( messageChangePWM . data [ 2 ] == 1 ) // c h e c k s i g n
{
dirOutput = l e f t ;
}
e l s e i f ( messageChangePWM . data [ 2 ] == 0 )
{
dirOutput = r i g h t ;
}
LED = 0 ;
}

void sendMessageCAN ( unsigned char l e n g t h , unsigned char id , unsigned char b u f f e r )

{
C1CTRL1bits .ABAT = 1 ;

while ( C1TR01CONbits .TXREQ0) { } ;

canTxMessage . b u f f e r = b u f f e r ; // B u f f e r number
canTxMessage . f r a m e t y p e = 1 ; //0−>S t d Id , 1−>Ext I d
canTxMessage . i d = i d ; // I d e n t i f i e r ;
canTxMessage . m e s s a g e t y p e = 0 ; //0−>Normal , 1−>Remote Transmit

i f ( length > 8) length = 8 ;

canTxMessage . d a t a l e n g t h = l e n g t h ; // Length o f d a t a (0 t o 8 b y t e s )

ecan1SendMessage (&canTxMessage ) ;
while ( C1TR01CONbits .TXREQ0) { } ;
}

// ///////////////////////////////////////////////////////////////////
// /////////////////////////////TASKS/////////////////////////////////
// ///////////////////////////////////////////////////////////////////

TASK( TaskActuator )
{
updateActuator ( ) ;
}

TASK( TaskConfig )
{
//LED = ˜LED;
switch ( m e s s a g e C o n f i g A c t u a t o r . data [ 0 ] )
{
case cmdStart : // s t a r t
workStateLed = working ;
sendACK ( ) ;
break ;

case cmdStop : // s t o p
workStateLed = standby ;
cntTaskLed = 0 ;
PDC1 = 0 ; // s t o p s a c t u a t o r
sendACK ( ) ;

179
 break ;

case cmdAskSettings : // s e n d s s e t t i n g s
sendSettings () ;
break ;

default :
break ;
}

sendSettings () ;
}

/∗ Send d a t a u s i n g t h e UART p o r t 1 v i a RS−232 t o t h e PC

∗ Note : This Task t a k e s l e s s than 0 . 0 0 0 2 s e c o n d s ∗/

/∗ CAN b u s 1 I n t e r r u p t , ISR2 t y p e ∗/
ISR2 ( C 1 I n t e r r u p t )
{
I F S 2 b i t s . C1IF = 0 ; // c l e a r i n t e r r u p t f l a g

/∗ Transmission i n t e r r u p t ( n o t h i n g t o be done b u t c l e a r f l a g ) ∗/
i f ( C1INTFbits . TBIF )
{
C1INTFbits . TBIF = 0 ;
}

/∗ R e c e p t i o n i n t e r r u p t , d i f f e r e n t code f o r d i f f e r e n t f i l t e r e d i d ’ s ∗/
i f ( C1INTFbits . RBIF )
{
// F i l t e r 0 : changePWM
i f ( C1RXFUL1bits . RXFUL1 == 1 )
{
/∗ T e l l s rxECAN1 t h e b u f f e r t o p a s s from DMA t o RAM ∗/
messageChangePWM . b u f f e r = 1 ;
C1RXFUL1bits . RXFUL1 = 0 ; // c l e a r f l a g
rxECAN1(&messageChangePWM ) ;

i f ( workStateLed == working ) // moves a c t u a t o r i f o n l y t h e s ys t em

i s ” working ”
// u p d a t e A c t u a t o r ( ) ;
A c t i v a t e T a s k ( TaskActuator ) ;
else
PDC1 = 0 ; // s t o p s a c t u a t o r
}

// F i l t e r 1 : c o n f i g T o A c t u a t o r
i f ( C1RXFUL1bits . RXFUL2 == 1 )
{
/∗ T e l l s rxECAN1 t h e b u f f e r t o p a s s from DMA t o RAM ∗/
messageConfigActuator . b u f f e r = 2 ;
C1RXFUL1bits . RXFUL2 = 0 ; // c l e a r f l a g
rxECAN1(& m e s s a g e C o n f i g A c t u a t o r ) ;
A c t i v a t e T a s k ( TaskConfig ) ;
}

C1INTFbits . RBIF = 0 ; // c l e a r f l a g
}
}

/∗ main f u n c t i o n , o n l y t o i n i t i a l i z e s o f t w a r e and hardware ,

∗ f i r e alarms , and implement b a c k g r o u n d a c t i v i t i e s ∗/
i n t main ( void )
{

180
 S y s i n i t ( ) ; // I n i t i a l i z e c l o c k , d e v i c e s and p e r i p h e r a l s

/∗ F o r e v e r l o o p : b a c k g r o u n d a c t i v i t i e s ( i f any ) s h o u l d go h e r e ∗/
for ( ; ; )
{
i f ( workStateLed == standby )
{
//LED = 0 ;
}
e l s e i f ( workStateLed == working )
{
//LED = ˜LED; // Orange l e d t o g g l e s
}
}
return 0 ;
}

A.5.3 Rod-sensor-node main code (sensorROD.c)

Listing A.14: Rod-sensor-node main code

// INVERTED PENDULUM − ROD POSITION SENSOR
// PERFORMED BY CARLOS XAVIER ROSERO & MANEL VELASCO
// BASED ON PREVIOUS WORK FROM ANTONIO CAMACHO

#include " ee . h "

#include " cpu / pic30 / inc / ee_irqstub . h "
#include " setupSENROD . h "
#include " ua rt_dmaSE NROD . h "
#include " e_can1SENROD . h "

FOSCSEL(FNOSC PRIPLL) ; // Primary (XT, HS, EC) O s c i l l a t o r w i t h PLL

FOSC (OSCIOFNC ON & POSCMD XT) ; // OSC2 Pin Function : OSC2 i s C l o c k Output
// Primary O s c i l l a t o r
Mode : XT C r y s t a l
FWDT(FWDTEN OFF) ; // Watchdog Timer Enabled / d i s a b l e d by
user software
FGS (GCP OFF) ; // D i s a b l e Code P r o t e c t i o n

//DEFINITIONS
#define LED LATBbits . LATB14
#define ON 1
#define OFF 0

#define standby 0
#define working 1
#define s e t t i n g 2

//commands t h r o u g h CAN, f o r c o n f i g u r a t i o n
#define cmdStop 0 xf0
#define cmdStart 0 xf1
#define cmdAskSettings 0 xf2
#define c m d I n i t S e n s o r 0 xf3

/∗ Variables ∗/

181
// t y p e s o f messages t h r o u g h CAN
unsigned long configToSensorROD = 0 x00 ;
unsigned long configToSensorCART = 0 x01 ;
unsigned long c o n f i g T o A c t u a t o r = 0 x02 ;
unsigned long synchroToSensorCART = 0 x03 ;
unsigned long b o t h S t a t e s = 0 x04 ;
unsigned long changePWM = 0 x05 ;
unsigned long configFromSensorROD = 0 x06 ;
unsigned long configFromSensorCART = 0 x07 ;
unsigned long c o n f i g F r o m A c t u a t o r = 0 x08 ;

unsigned char cntTaskLed = 0 ;

unsigned char workStateLed = standby ;

unsigned i n t msSamplingSen = 1 0 ;

// D e f i n e ECAN Message B u f f e r s
ECAN1MSGBUF ecan1msgBuf a t t r i b u t e ( ( s p a c e (dma) , a l i g n e d (ECAN1 MSG BUF LENGTH∗ 1 6 ) ) ) ;
//ECAN2MSGBUF ecan2msgBuf attribute ( ( s p a c e (dma) , a l i g n e d (ECAN2 MSG BUF LENGTH∗16) ) ) ;
// CAN Messages i n RAM

mID m e s s a g e C o n f i g S e n s o r ;

//mID r x e c a n 1 m e s s a g e 3 ; / /RX C o n t r o l l e r u p d a t e s r e f e r e n c e ( s u p e r v i s i o n )
//mID r x e c a n 1 m e s s a g e 4 ; / /RX Not used
mID canTxMessage ; // message t o send t h r o u g h CAN b u s

void sendMessageCAN ( unsigned char l e n g t h , unsigned char id , unsigned char b u f f e r ) ;

void sendACK ( )
{
unsigned char i = 0 ;

f o r ( i =0; i <8; i ++)

canTxMessage . data [ i ] = 0 x00 ; // f i l l s the r e s t with n u l l

sendMessageCAN ( 0 , configFromSensorROD , 0 ) ; // s e n d s message t h r o u g h CAN

}

void s e n d S e t t i n g s ( )
{
unsigned char i = 0 ;

canTxMessage . data [ 0 ] = cmdStart ;

canTxMessage . data [ 1 ] = ( unsigned char ) ( msSamplingSen >>8) ;
canTxMessage . data [ 2 ] = ( unsigned char ) ( msSamplingSen ) ;

f o r ( i =3; i <8; i ++)

canTxMessage . data [ i ] = 0 x00 ; // f i l l s the r e s t with n u l l

sendMessageCAN ( 3 , configFromSensorROD , 0 ) ; // s e n d s message t h r o u g h CAN

// s t a t i c u n s i g n e d c h a r ∗ p d a t a = NULL;

void sendMessageCAN ( unsigned char l e n g t h , unsigned char id , unsigned char b u f f e r )

{
C1CTRL1bits .ABAT = 1 ;

while ( C1TR01CONbits .TXREQ0) { } ;

canTxMessage . b u f f e r = b u f f e r ; // B u f f e r number
canTxMessage . f r a m e t y p e = 1 ; //0−>S t d Id , 1−>Ext I d

182
 canTxMessage . i d = i d ; // I d e n t i f i e r ;
canTxMessage . m e s s a g e t y p e = 0 ; //0−>Normal , 1−>Remote Transmit

i f ( length > 8) length = 8 ;

canTxMessage . d a t a l e n g t h = l e n g t h ; // Length o f d a t a (0 t o 8 b y t e s )

ecan1SendMessage (&canTxMessage ) ;
while ( C1TR01CONbits .TXREQ0) { } ;
}

s t a t i c unsigned i n t p o s i t i o n = 0 ;
s t a t i c unsigned char d i r e c t i o n = 0 ;

void r e a d S t a t e ( )
{
p o s i t i o n = POSCNT;

i f (QEICON & 0 x0800 )

d i r e c t i o n = 0 x01 ;
else
d i r e c t i o n = 0 x00 ;
}

/∗ t a s k s ∗/

// r e a d s and s e n d s rod p o s i t i o n

TASK( TaskSensor )
{
unsigned char i ;

//LED = ˜LED;
LED = 1 ;
readState () ;

canTxMessage . data [ 0 ] = ( unsigned char ) ( p o s i t i o n >>8) ;

canTxMessage . data [ 1 ] = ( unsigned char ) ( p o s i t i o n ) ;
canTxMessage . data [ 2 ] = d i r e c t i o n ;

f o r ( i =3; i <8; i ++)

{
canTxMessage . data [ i ] = 0 x00 ;
}

sendMessageCAN ( 3 , synchroToSensorCART , 0 ) ; // s e n d s ROD p o s i t i o n t o sensorCART

LED = 0 ;
}

/∗ C o n f i g u r a t i o n Task ∗/

/∗ This t a s k o n l y a c t i v a t e s once a c o n f i g u r a t i o n message came t h r o u g h CAN∗/

// s t a t i c f l o a t ∗ p u = ( f l o a t ∗ )&m e s s a g e C o n f i g S e n s o r . d a t a [ 0 ] ;

TASK( TaskConfig )
{
switch ( m e s s a g e C o n f i g S e n s o r . data [ 0 ] )
{
case cmdStart : // s t a r t

msSamplingSen = ( unsigned i n t ) ( m e s s a g e C o n f i g S e n s o r . data [1] < <8) |

183
 ( unsigned i n t ) ( m e s s a g e C o n f i g S e n s o r . data [ 2 ] ) ; //
u p d a t e s a new s a m p l i n g time

CancelAlarm ( AlarmSensor ) ;
SetRelAlarm ( AlarmSensor , 1 0 0 0 , msSamplingSen ) ; // Sensor r e a d i n g
a c t i v a t e s e v e r y msSampling
workStateLed = working ;
LED = 1 ;

sendACK ( ) ;
break ;

case cmdStop : // s t o p
CancelAlarm ( AlarmSensor ) ; // s t o p s t a s k
workStateLed = standby ;
cntTaskLed = 0 ;
sendACK ( ) ;
break ;

case cmdAskSettings : // s e n d s s e t t i n g s
sendSettings () ;
break ;

case c m d I n i t S e n s o r :
POSCNT = 3 0 6 8 ; // R e s e t p o s i t i o n c o u n t e r −−> 270 d e g r e e s
sendACK ( ) ;
break ;

default :
break ;
}
}

/∗ CAN b u s 1 I n t e r r u p t , ISR2 t y p e ∗/
ISR2 ( C 1 I n t e r r u p t )
{
I F S 2 b i t s . C1IF = 0 ; // c l e a r i n t e r r u p t f l a g

/∗ Transmission i n t e r r u p t ( n o t h i n g t o be done b u t c l e a r f l a g ) ∗/
i f ( C1INTFbits . TBIF )
{
C1INTFbits . TBIF = 0 ;
}

/∗ R e c e p t i o n i n t e r r u p t , d i f f e r e n t code f o r d i f f e r e n t f i l t e r e d IDs ∗/
i f ( C1INTFbits . RBIF )
{
// f i l t e r 0 , b u f f e r 1 , mask 0 −−> c o n f i g T o S e n s o r
i f ( C1RXFUL1bits . RXFUL1 == 1 )
{
/∗ T e l l s rxECAN1 t h e b u f f e r t o p a s s from DMA t o RAM ∗/
messageConfigSensor . b u f f e r = 1;
C1RXFUL1bits . RXFUL1 = 0 ;
rxECAN1(& m e s s a g e C o n f i g S e n s o r ) ;
A c t i v a t e T a s k ( TaskConfig ) ;
}
C1INTFbits . RBIF = 0 ; // c l e a r i n t e r r u p t
}
}

/∗ main f u n c t i o n , o n l y t o i n i t i a l i z e s o f t w a r e and hardware ,

∗ f i r e alarms , and implement b a c k g r o u n d a c t i v i t i e s ∗/
i n t main ( void )
{

184
 S y s i n i t ( ) ; // I n i t i a l i z e c l o c k , d e v i c e s and p e r i p h e r a l s

/∗ F o r e v e r l o o p : b a c k g r o u n d a c t i v i t i e s ( i f any ) s h o u l d go h e r e ∗/
for ( ; ; )
{
i f ( workStateLed == standby )
{
i f ( workStateLed == standby )
{
LED = 0 ;
}
e l s e i f ( workStateLed == working )
{
//LED = ˜LED; // Orange l e d t o g g l e s
}
}
}
return 0 ;
}

A.5.4 Cart-sensor-node main code (sensorCART.c)

Listing A.15: Cart-sensor-node main code

// INVERTED PENDULUM − CART POSITION SENSOR
// PERFORMED BY CARLOS XAVIER ROSERO & MANEL VELASCO
// BASED ON PREVIOUS WORK FROM ANTONIO CAMACHO

#include " ee . h "

#include " cpu / pic30 / inc / ee_irqstub . h "
#include " setupSENCART . h "
#include " ua rt _ dm aS EN C AR T . h "
#include " e_can1SENCART . h "

FOSCSEL(FNOSC PRIPLL) ; // Primary (XT, HS, EC) O s c i l l a t o r w i t h PLL

FOSC (OSCIOFNC ON & POSCMD XT) ; // OSC2 Pin Function : OSC2 i s C l o c k Output
// Primary O s c i l l a t o r
Mode : XT C r y s t a l
FWDT(FWDTEN OFF) ; // Watchdog Timer Enabled / d i s a b l e d by
user software
FGS (GCP OFF) ; // D i s a b l e Code P r o t e c t i o n

//DEFINITIONS
#define LED LATBbits . LATB14
#define ON 1
#define OFF 0

#define standby 0
#define working 1
#define s e t t i n g 2

//commands t h r o u g h CAN, f o r c o n f i g u r a t i o n
#define cmdStop 0 xf0
#define cmdStart 0 xf1
#define cmdSupervOff 0 xf0

185
#define cmdSupervOn 0 xf1
#define cmdAskSettings 0 xf2
#define c m d I n i t S e n s o r 0 xf3

/∗ Variables ∗/
// t y p e s o f messages t h r o u g h CAN
unsigned long configToSensorROD = 0 x00 ;
unsigned long configToSensorCART = 0 x01 ;
unsigned long c o n f i g T o A c t u a t o r = 0 x02 ;
unsigned long synchroToSensorCART = 0 x03 ;
unsigned long b o t h S t a t e s = 0 x04 ;
unsigned long changePWM = 0 x05 ;
unsigned long configFromSensorROD = 0 x06 ;
unsigned long configFromSensorCART = 0 x07 ;
unsigned long c o n f i g F r o m A c t u a t o r = 0 x08 ;

unsigned char cntTaskLed = 0 ;

unsigned char workStateLed = standby ;

// D e f i n e ECAN Message B u f f e r s
ECAN1MSGBUF ecan1msgBuf a t t r i b u t e ( ( s p a c e (dma) , a l i g n e d (ECAN1 MSG BUF LENGTH∗ 1 6 ) ) ) ;
//ECAN2MSGBUF ecan2msgBuf attribute ( ( s p a c e (dma) , a l i g n e d (ECAN2 MSG BUF LENGTH∗16) ) ) ;
// CAN Messages i n RAM

mID m e s s a g e C o n f i g S e n s o r ;
mID m e s s a g e R o d P o s i t i o n ;
mID canTxMessage ; // message t o send t h r o u g h CAN b u s

void sendMessageCAN ( unsigned char l e n g t h , unsigned char id , unsigned char b u f f e r ) ;

void sendACK ( )
{
unsigned char i = 0 ;

f o r ( i =0; i <8; i ++)

canTxMessage . data [ i ] = 0 x00 ; // f i l l s the r e s t with n u l l

sendMessageCAN ( 0 , configFromSensorCART , 0 ) ; // s e n d s message t h r o u g h CAN

}

void s e n d S e t t i n g s ( )
{
unsigned char i = 0 ;

canTxMessage . data [ 0 ] = cmdStart ;

f o r ( i =1; i <8; i ++)

canTxMessage . data [ i ] = 0 x00 ; // f i l l s the r e s t with n u l l

sendMessageCAN ( 1 , configFromSensorCART , 0 ) ; // s e n d s message t h r o u g h CAN

}

s t a t i c unsigned i n t p o s i t i o n = 0 ;
s t a t i c unsigned char d i r e c t i o n = 0 ;

void s e n d S t a t e M e s s a g e ( )
{
unsigned char i ;

//LED = ˜LED;
LED = 1 ;

// r e a d s s t a t e ( c a r t p o s i t i o n )

186
 p o s i t i o n = POSCNT;

i f (QEICON & 0 x0800 )

d i r e c t i o n = 0 x01 ;
else
d i r e c t i o n = 0 x00 ;

canTxMessage . data [ 0 ] = m e s s a g e R o d P o s i t i o n . data [ 0 ] ; // r e p e a t s rod p o s i t i o n

canTxMessage . data [ 1 ] = m e s s a g e R o d P o s i t i o n . data [ 1 ] ;
canTxMessage . data [ 2 ] = m e s s a g e R o d P o s i t i o n . data [ 2 ] ;
canTxMessage . data [ 3 ] = ( unsigned char ) ( p o s i t i o n >>8) ;
canTxMessage . data [ 4 ] = ( unsigned char ) ( p o s i t i o n ) ;
canTxMessage . data [ 5 ] = direction ;

f o r ( i =6; i <8; i ++)

{
canTxMessage . data [ i ] = 0 x00 ;
}

sendMessageCAN ( 6 , b o t h S t a t e s , 0 ) ; // s e n d s message t h r o u g h CAN

LED = 0 ;
}

void sendMessageCAN ( unsigned char l e n g t h , unsigned char id , unsigned char b u f f e r )

{
C1CTRL1bits .ABAT = 1 ;

while ( C1TR01CONbits .TXREQ0) { } ;

canTxMessage . b u f f e r = b u f f e r ; // B u f f e r number
canTxMessage . f r a m e t y p e = 1 ; //0−>S t d Id , 1−>Ext I d
canTxMessage . i d = i d ; // I d e n t i f i e r ;
canTxMessage . m e s s a g e t y p e = 0 ; //0−>Normal , 1−>Remote Transmit

i f ( length > 8) length = 8 ;

canTxMessage . d a t a l e n g t h = l e n g t h ; // Length o f d a t a (0 t o 8 b y t e s )

ecan1SendMessage (&canTxMessage ) ;
while ( C1TR01CONbits .TXREQ0) { } ;
}

/∗ t a s k s ∗/

TASK( TaskStateMess age )

{
sendStateMessage ( ) ;
}

TASK( TaskConfig )
{
switch ( m e s s a g e C o n f i g S e n s o r . data [ 0 ] )
{
case cmdStart : // s t a r t
workStateLed = working ;
LED = 1 ;
sendACK ( ) ;
break ;

case cmdStop : // s t o p
workStateLed = standby ;
cntTaskLed = 0 ;
sendACK ( ) ;

187
 break ;

case cmdAskSettings : // s e n d s s e t t i n g s
sendSettings () ;
break ;

case c m d I n i t S e n s o r :

i f ( m e s s a g e C o n f i g S e n s o r . data [ 1 ] == 0 x f 0 ) // l a r g e c a l i b r a t i o n
POSCNT = 0 ; // R e s e t p o s i t i o n c o u n t e r as t h e v e r y l e f t o f
the track
e l s e i f ( m e s s a g e C o n f i g S e n s o r . data [ 1 ] == 0 x f 1 ) // s h o r t c a l i b r a t i o n
POSCNT = 9 5 5 2 ; // R e s e t p o s i t i o n c o u n t e r as t h e m i d d l e o f
the track
sendACK ( ) ;
break ;

default :
break ;
}
}

/∗ CAN b u s 1 I n t e r r u p t , ISR2 t y p e ∗/
ISR2 ( C 1 I n t e r r u p t )
{
I F S 2 b i t s . C1IF = 0 ; // c l e a r i n t e r r u p t f l a g

/∗ Transmission i n t e r r u p t ( n o t h i n g t o be done b u t c l e a r f l a g ) ∗/
i f ( C1INTFbits . TBIF )
{
C1INTFbits . TBIF = 0 ;
}

/∗ R e c e p t i o n i n t e r r u p t , d i f f e r e n t code f o r d i f f e r e n t f i l t e r e d IDs ∗/
i f ( C1INTFbits . RBIF )
{
// f i l t e r 0 , b u f f e r 1 , mask 0 −−> configToSensorCART
i f ( C1RXFUL1bits . RXFUL1 == 1 )
{
/∗ T e l l s rxECAN1 t h e b u f f e r t o p a s s from DMA t o RAM ∗/
messageConfigSensor . b u f f e r = 1;
C1RXFUL1bits . RXFUL1 = 0 ;
rxECAN1(& m e s s a g e C o n f i g S e n s o r ) ;
A c t i v a t e T a s k ( TaskConfig ) ;
}

// f i l t e r 1 , b u f f e r 2 , mask0 −−> synchroToSensorCART

i f ( C1RXFUL1bits . RXFUL2 == 1 )
{
/∗ T e l l s rxECAN1 t h e b u f f e r t o p a s s from DMA t o RAM ∗/
messageRodPosition . b u f f e r = 2 ;
C1RXFUL1bits . RXFUL2 = 0 ;
rxECAN1(& m e s s a g e R o d P o s i t i o n ) ;

i f ( workStateLed == working ) // s e n d s s t a t e i f o n l y t h e s y st e m i s
” working ”
A c t i v a t e T a s k ( TaskStateMessage ) ;
}
C1INTFbits . RBIF = 0 ; // c l e a r i n t e r r u p t
}
}

/∗ main f u n c t i o n , o n l y t o i n i t i a l i z e s o f t w a r e and hardware ,

∗ f i r e alarms , and implement b a c k g r o u n d a c t i v i t i e s ∗/

188
i n t main ( void )
{
S y s i n i t ( ) ; // I n i t i a l i z e c l o c k , d e v i c e s and p e r i p h e r a l s

/∗ F o r e v e r l o o p : b a c k g r o u n d a c t i v i t i e s ( i f any ) s h o u l d go h e r e ∗/
for ( ; ; )
{
i f ( workStateLed == standby )
{
LED = 0 ;
}
e l s e i f ( workStateLed == working )
{
//LED = ˜LED; // Orange l e d t o g g l e s
}
}
return 0 ;
}

189