Difference between revisions of "MotionSystem"

Revision as of 16:23, 7 July 2009

Project Description

The application field of this project is robotic.

The goal of this project is to provide a R/C Servos Controller to manage R/C by different ways, and a motion measurement system via accelerometers.

The project is composed by different componants:

• A hardware board on which are plugged R/C servos, accelerometers and ADC multiplexers.
• A FPGA firmware which manages R/C servo at low level (Already written, thanks to Sonzerro and Fabien Marteau).
• A Linux Kernel Driver which manages the FPGA Firware, the ADC multiplexers and ADC Max1027.
• A user space daemon which manages the driver and listen on TCP port to accept remote commands from the network.
• A user desktop application which sends commands to the daemon.

Texte italique

Participating Armadeus members

• Yvan ROCH

Project data

Introduction

Overall original objectives of the project have not been reached. The following functionality are not implemented:

• The accelerometers and ADC multiplexers are under development (hardware, driver and user application).
• The board is available in schematic form. No PCB layout.

The following functionality are implemented:

• A FPGA firmware which manages R/C servo at low level.
• A Linux Kernel Driver which manages the FPGA Firware.
• A user space daemon which manages the driver and listen on TCP port to accept remote commands from the network.
• A user desktop application which sends commands to the daemon.

Project Files

All the necessary files are available in an archive under the following URL: Project Files

Hardware Interface Board

The board contains the logic and voltage level adaptation to drive the R/C servos. As already said, the is no PCB layout.

The schematic is available in the file...

The schematic was design with Eagle available at the following URL: Eagle

FPGA Firmware

Linux Kernel Driver

User Space Daemon

User GUI Application

How to use the project

Introduction

The main goal of this project is to develop a system to manage motion on robotic platform. Finally only the subsystem that manages R/C servos is achieved. The MotionSystem can manage up to 32 R/C servos. R/C servos are a good solution to the moving issue on a small robotic platform. Hobbyist robotic platform like Lynxmotion Robots (http://www.lynxmotion.com) use many R/C servos.

R/C Servo

A description (in French) of such R/C servo can be found at http://fribotte.free.fr. R/C servo needs a pulse-width modulated (PWM) signal to manage his angular position. This signal looks like this:

Basically, it is possible to generate this signal with a GPIO pin controlled by Linux kernel code. But this solution has three major drawbacks:

• The true concurrency is impossible, R/C servo signal are not synchronous
• The delays of PWM signal (between 0.5 ms and 2.5 ms) are mandatory implemented with busy waiting (udelay Linux kernel function) to give the necessary accuracy. It is very processor time consuming.
• Due to the intrinsic non real time characteristic of the Linux kernel, the accuracy of the delays are impassible on a heavy loaded system. This causes R/C servo jittering (When a R/C servo jitters, it seems to have Parkinson's disease... Jitter

For these reasons, the subsystem that generates PWM signals is implemented in the FPGA. In a FPGA, true concurrency is feasible, no processor time is used and timings are very strict.

MotionSystem Architecture

The MotionSystem architecture is describe in the following diagram:

Hardware Interface Board

The schematic diagram of the interface board is located in the hardwareInterfaceBoard directory of the main archive file of the project (see above). The design is very simple for this version without R/C servo strength control and accelerometers. It is mainly composed of buffers between FPGA output and R/C servo input. The following diagram is an export of the original Eagle file of the main archive:

How to connect the hardware interface board to the Armadeus APF9328DevLight board

The following table establish the connexion between the interface board and the Armadeus APF9328DevLight board. The pinout of the Armadeus APF9328DevLight board is available at APF9328DevLight. Please refer to the official documentation for other boards.

FPGA Firmware

For basics about FPGA on Armadeus board, please consult FPGA.

The FPGA firmware is located in the fpgaFirmware directory of the main archive file of the project (see above). The following files form the FPGA firmware:

SERVO_top.vhd: This is the main module.

COUNTER.vhd: This module is a 16 bits counter at 4 MHz used by all the PWM modules.

RESET_MODULE.vhd: This module is a cold start reset generator.

pwm_module.vhd: This module generates the PWM output.

servo.ucf: Signals/pins mapping file.

How to build the FPGA Firmware:

The Xilinx ISE® WebPACK™ design software is used to build the FPGA firmware.

1 - Create a new project for the Spartan 3

• File->New Project
• Select Top Level Source: HDL (Hardware Description Language)
• For the Armadeus FPGA choose:

Family: Spartan3

Device: XC3S200

Package: TQ144

Speed: -4

Top Level Source: HDL

Synthesis Tool: XST

Simulator: ISE Simulator (VHDL/Verilog)

Preferred Language: VHDL

2 - Import the source files in the project

• Project->Add Source
• Select files:

SERVO_top.vhd

COUNTER.vhd

RESET_MODULE.vhd

pwm_module.vhd

servo.ucf

3 - Synthesize SERVO_top.bit firmware binary file

• Generate Programming File->right click->Run
• You get a SERVO_top.bit in the ISE project directory.

How to load SERVO_top.bit firmware binary file in the Spartan 3

You need a connexion with the Armadeus board, RS323 or TFTP. For basics about communication with the Armadeus board, please consult Connection_with_U-Boot_on_Linux. The RS232 method is describe here but the TFTP transfer is possible and faster.

• Install and configure Kermit like describe in Kermit.
• Launch Kermit.
• Power up the Armadeus board.
• Hit any key to stop auto boot and to obtain the command prompt:

BIOS>

• Choose the upload memory address:

loadb 08000000

• The Armadeus board is ready to receive file:

## Ready for binary (kermit) download to 0x08000000 at 115200 bps...

• Change to the Kermit command mode:

Ctrl+Altgr+\+c

• File upload:

send SERVO_top.bit

• Kermit upload the file. After uploading, Kermit says:

Hit c to reconnect the terminal.

• Firmware Flashing:

run flash_firmware

• Loading the firmware in the FPGA:

fpga load 0 ${firmware_addr} ${firmware_len}

• For an automatic loading at boot:

setenv firmware_autoload 1
saveenv

• The firmware is now usable!!!

How to test the FPGA firmware

The FPGA firmware is not design to be used with U-Boot command. In the final configuration, firmware is managed by the Servo Linux Kernel Driver. However, it is possible to send command to the firmware with U-Boot command and show the result. It is not very "Pretty User", but it is enough to validate the good working order of the firmware. First, you must build hardware interface board as describe in section Hardware Interface Board and connect R/C servos to the interface.

At the command prompt:

BIOS>

Show the FPGA Memory Register:

md.w 12000000

Result:

12000000: 7207 0003 0020 0000 0000 0000 0000 0000    .r.. ...........
12000010: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000020: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000030: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000040: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000050: 0000 0000 0000 0000 0000 0000 0000 0000    ................
12000060: 0000 0000 0000 0000 0000 0000 0000 0000    ................
12000070: 0000 0000 0000 0000 0000 0000 0000 0000    ................

Explaination:

• @12000000 7207: Firmware Magic Number
• @12000002 0003: Firmware Version
• @12000004 0020: Number of R/C servos managed (32 in decimal)
• @12000006 0000: Status Register
• @12000008 0000: Command Register
• @1200000A 0000: Enable Servo Register LSB
• @1200000C 0000: Enable Servo Register MSB
• @12000010 to *@1200004E 0800: Servos Position Registers, default position

Servos 0-15 enabling:

mw.w 1200000A FFFF

Result: The R/C servos 0-15

md.w 12000000

12000000: 7207 0003 0020 0000 0000 ffff 0000 0000    .r.. ...........
12000010: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000020: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000030: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000040: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000050: 0000 0000 0000 0000 0000 0000 0000 0000    ................
12000060: 0000 0000 0000 0000 0000 0000 0000 0000    ................
12000070: 0000 0000 0000 0000 0000 0000 0000 0000    ................

Put the servo 0 on the maximal counterclockwise position:

mw.w 12000010 0000

BIOS>  md.w 12000000
12000000: 7207 0003 0020 0000 0000 ffff 0000 0000    .r.. ...........
12000010: 0000 0800 0800 0800 0800 0800 0800 0800    ................
12000020: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000030: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000040: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000050: 0000 0000 0000 0000 0000 0000 0000 0000    ................
12000060: 0000 0000 0000 0000 0000 0000 0000 0000    ................
12000070: 0000 0000 0000 0000 0000 0000 0000 0000    ................

Put the servo 0 on the median position:

mw.w 12000010 0800

BIOS> md.w 12000000
12000000: 7207 0003 0020 0000 0000 ffff 0000 0000    .r.. ...........
12000010: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000020: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000030: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000040: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000050: 0000 0000 0000 0000 0000 0000 0000 0000    ................
12000060: 0000 0000 0000 0000 0000 0000 0000 0000    ................
12000070: 0000 0000 0000 0000 0000 0000 0000 0000    ................

Put the servo 0 on the maximal clockwise position:

mw.w 12000010 0FFF

BIOS> md.w 12000000
12000000: 7207 0003 0020 0000 0000 ffff 0000 0000    .r.. ...........
12000010: 0fff 0800 0800 0800 0800 0800 0800 0800    ................
12000020: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000030: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000040: 0800 0800 0800 0800 0800 0800 0800 0800    ................
12000050: 0000 0000 0000 0000 0000 0000 0000 0000    ................
12000060: 0000 0000 0000 0000 0000 0000 0000 0000    ................
12000070: 0000 0000 0000 0000 0000 0000 0000 0000    ................

Linux Kernel Servo Driver

The Linux kernel driver is located in the ArmadeusServoDriver directory of the main archive file of the project (see above). The following files form the Linux kernel driver:

servo.c: Main file.

servo.h: Header file.

Makefile: Kernel module makefile.

Kconfig: Kernel configuration file.

How to build and install the Linux Kernel Servo Driver

Before building the servo driver, be sure that the Toolchain and the Linux Kernel build system work fine on your host system. In the following, $ARMADEUS_ROOT will refer to the root directory of Armadeus SDK (for example: /usr/local/src/armadeus3-trunk)

export ARMADEUS_ROOT=/usr/local/src/armadeus3-trunk

Create a symbolic link in the FPGA driver directory of the Armadeus SDK to the ArmadeusServoDriver directory:

ln -s ~/ArmadeusServoDriver  $ARMADEUS_ROOT/target/linux/modules/fpga/

Reference the new directory in the file $ARMADEUS_ROOT/target/linux/modules/fpga/Makefile by replacing:

obj-$(CONFIG_ARMADEUS_FPGA_DRIVERS)     += dev_tools/ others/ POD/ wishbone_example/

by

obj-$(CONFIG_ARMADEUS_FPGA_DRIVERS)     += dev_tools/ others/ POD/ wishbone_example/ ArmadeusServoDriver/

Reference the new directory in the file $ARMADEUS_ROOT//target/linux/modules/fpga/Kconfig by adding:

source "drivers/armadeus/fpga/ArmadeusServoDriver/Kconfig"

Configure the Linux kernel to build the servo module:

cd $ARMADEUS_ROOT
make linux26-menuconfig

In Device Drivers ---> Armadeus specific drivers ---> FPGA Drivers, select Armadeus Servo driver (NEW) as module. Rebuild the new Linux kernel and modules:

make clean
make

The module servo.ko is now in target root filesystem and in the ArmadeusServoDriver directory. Install your new Linux Kernel and your root filesystem as describe in Target_Software_Installation

How to test the Linux Kernel Servo Driver

Linux Kernel Servo Driver is not design to be used with shell command. In the final configuration, the driver is managed by the User Space Daemon. However, it is possible to send command to the driver via writing in the /sys filesystem. It is not very "Pretty User", but it is enough to validate the good working order of the driver. First, you must build and load the Servo FPGA Firmware as describe in section FPGA Firmware. Load the Linux Kernel Servo Driver: Log on the Armadeus target and type:

 modprobe servo

The system says:

Armadeus FPGA R/C servo driver: Version 0.1
Armadeus FPGA R/C servo driver: Magic ID OK 0X7207
Armadeus FPGA R/C servo driver: Firmware version 0X3
Armadeus FPGA R/C servo driver: 32 R/C servo(s) managed by the FPGA firmware

User Space Motion Daemon

The User Space Motion Daemon is a user space Linux process that drives R/C servos via the Linux Kernel Servo Driver. At startup it loads the Linux Kernel Servo Driver if specified in his configuration file. It sets the R/C servos at their default values as specified in the configuration file. Next it listens at the TCP port 1972 and waits connexion from a client. When a client connects to him, they dialog using a specific language described in section Motion Daemon Language. The daemon interprets and executes commands send using this language.

Motion Daemon Language

The communication between a client and the Motion Daemon is controlled by a little context-free language. It is build under Lex and Yacc. The BNF and the lexical can be found in files command.y and command.l respectively in the MotionServer directory. The commands available are the following:

• Set the R/C servo servo (integer between 0 and 31) at the position position (integer between 0 and 4095):

SET_POSITION ( servo , position )

• Enable the R/C servo servo. If R/C servo is enable, it keeps its position else it is freewheeling.

SET_ENABLE ( servo ):

• Set the position offset for the R/C servo servo (integer between 0 and 31) at the value offset (integer between -4095 and 4095):

SET_OFFSET  ( servo , offset )

• Set the minimal position for the R/C servo servo (integer between 0 and 31) at the value min (integer between 0 and 4095):

SET_MIN ( servo , min )

• Set the maximal position for the R/C servo servo (integer between 0 and 31) at the value max (integer between 0 and 4095):

SET_MAX ( servo , max )

• Return the position of the R/C servo servo:

GET_POSITION ( servo )

• Return the enable state of the R/C servo servo:

GET_ENABLE ( servo )

• Return the position offset of the R/C servo servo:

GET_OFFSET ( servo )

• Return the minimal position of the R/C servo servo:

GET_MIN ( servo )

• Return the maximal position of the R/C servo servo:

GET_MAX ( servo )

• Write the current state (R/C servos position, position offset, minimum position, maximum position, enable state) in a file pathon the local Armadeus board:

WRITE_CONFIG_FILE ( path )

• Wait seconds seconds before processing next command:

SLEEP ( seconds )

Motion Daemon Configuration File

The User Space Motion Daemon need a configuration file .MotionServerrc located in the MotionServer binary file directory. The minimal file is:

[Module]

[Default]

The configuration file is in two sections.

• [Module]: This section concerns the configuration of the Linux Kernel Servo Driver. Options are
  • ModuleFilePath /path/to/the/module: Set the path of the Linux Kernel Servo Driver module.
  • ModuleLoad [0|1]: Set the auto-loading of the Linux Kernel Servo Driver module at the daemon startup.
• [Default]: This section concerns the default configuration of each R/C servo. However, all R/C servos and all parameters for each R/C servo are not mandatory. Only specified parameters are processed.
  • ServoPosition idServo value: Set the default position of R/C servo idServo at value value.
  • ServoOffset idServo value: Set the default position of R/C servo idServo at value value.
  • ServoMin idServo value: Set the default lower position boundary of R/C servo idServo at value value.
  • ServoMax idServo value: Set the default upper position boundary of R/C servo idServo at value value.
  • ServoEnable idServo value: Set the default enable status of R/C servo idServo at value value.

Example of typical .MotionServerrc file:

[Module]
ModuleFilePath /mnt/motion_system.ko
ModuleLoad 1
[Default]
ServoPosition 0 1569
ServoOffset 0 10
ServoMin 0 300
ServoMax 0 3500
ServoEnable 0 1
ServoPosition 10 3000
ServoOffset 10 -20
ServoMin 10 150
ServoMax 10 3900
ServoEnable 10 0

How to test the User Space Motion Daemon

The User Space Motion Daemon is not design to be used with shell command. In the final configuration, the daemon is managed by the client application (the User GUI Application provided or yours). However, it is possible to send command to the daemon via opening a socket to daemon and writing Motion Daemon Language command on it. It is not very "Pretty User", but it is enough to validate the good working order of the daemon. First, you must build and load the Linux Kernel Servo Driver as describe in section Linux Kernel Servo Driver.

Warning: A completer !!!

User GUI Application

The User GUI Application is an client example of the Servo User Space Daemon. This application is based on Qt version 4.3.x, 4.4.x, or 4.5.x

How to build the User GUI Application

Before building the User GUI Application, be sure that the Qt development environment is correctly installed. On a Debian system the following packages must be installed:

Prepare the Qt project for building:

qmake-qt4

Build the User GUI Application:

make

Now, the ELF binary file ServoGui is available in the ServoGuidirectory.

How to use the User GUI Application

To launch the User GUI Application, in the build directory, type:

./ServoGui

You get the following screen:

The application is mainly composed of four tabs. The three first tabs are similar and dedicated to R/C servos management. Each of these tabs can manages eight R/C servos. The field Remote Host is the host name or IP address of the remote Armadeus board on which run the Motion Daemon and are connected R/C servos. The "Connect" button establish the TCP connection to the remote Armadeus board. The "Quit" button close the application after closing the TCP socket. The "Bank 1" tab manages R/C Servo 0 to 7, the "Bank 2" tab manages R/C Servo 8 to 15, and the "Bank 3" tab manages R/C Servo 16 to 23. The application manages only 24 R/C servos, even if the MotionSystem is 32 R/C servos capable. If you need to manage more than 24 R/C servos, duplicate and modify the tab source code.

Each R/C servo on the tab had six controls:

• A position slider that puts the R/C to the desired position.
• A position spinbox that does the same than the slider but more finely.
• An offset spinbox that sets the R/C servo offset position (real position = position + offset).
• A minimum spinbox that sets the lower position boundary.
• A maximum spinbox that sets the upper position boundary.
• A enable checkbox that enables R/C servo. If it is unchecked, the R/C servo is freewheeling.

The fourth tab looks like:

The "Send Batch File" button sends a file of which path is specified in text field on the right. This path is relative to the host filesystem on which ServoGui is running. This file contains command that are syntactically correct in the Motion Daemon Language. Each line contains only one command. An example of batch file:

SET_POSITION (0 ,1000 )
SET_POSITION (1 ,3000 )
SLEEP (1)
SET_POSITION (1 ,1000 )

The "Save Config" button saves the actual state in a file on the remote Armadeus system. The file path is specified in text field on the right. This path is relative to remote Armadeus system on which Motion Daemon is running.

Links

Project Files

Eagle

http://www.lynxmotion.com

http://fribotte.free.fr

Jitter

Qt