Difference between revisions of "MotionSystem"

From ArmadeusWiki
Jump to: navigation, search
(→‎Introduction)
(→‎Links)
 
(200 intermediate revisions by 3 users not shown)
Line 8: Line 8:
 
* A hardware board on which are plugged R/C servos, accelerometers and ADC multiplexers.
 
* 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 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 Linux Kernel Driver which manages the FPGA Firmware, the ADC multiplexers and the [[ADC_max1027|Max1027 ADC]].
 
* A user space daemon which manages the driver and listen on TCP port to accept remote commands from the network.
 
* 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.
+
* A user desktop application which sends commands to the daemon over the network connexion.
  
 
==Participating Armadeus members==
 
==Participating Armadeus members==
Line 24: Line 24:
 
The following functionality are implemented:
 
The following functionality are implemented:
 
* A FPGA firmware which manages R/C servo at low level.
 
* A FPGA firmware which manages R/C servo at low level.
−
* A Linux Kernel Driver which manages the FPGA Firware.
+
* A Linux Kernel Driver which manages the FPGA Firmware.
 
* A user space daemon which manages the driver and listen on TCP port to accept remote commands from the network.
 
* 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.
 
* A user desktop application which sends commands to the daemon.
  
−
===Hardware Board===
+
===Project Files===
−
The board contains the following stuff:
+
All the necessary files are available in an archive under the following URL:
−
* The logic and voltage level adaptation to drive the R/C servos
+
[http://yvan.roch.free.fr/armadeus/MotionSystem-00.00.00.tgz Project Files]
 +
 
 +
;Hardware Interface Board
 +
The board contains the logic and voltage level adaptation to drive the R/C servos. As already said, there is no PCB layout. The schematic is available in the file <font face="Courier">Hardware-Interface-Board/Hardware-Interface-Board.sch</font>. The schematic was design with Eagle available at the following URL: [http://www.cadsoft.de Eagle].
 +
 
 +
;FPGA Firmware
 +
The FPGA firmware is located in the <font face="Courier">fpgaFirmware</font> directory.
 +
 
 +
;Linux Kernel Driver
 +
The Linux Kernel Driver is located in the <font face="Courier">ArmadeusServoDriver</font> directory.
 +
 
 +
;User Space Daemon
 +
The User Space Daemon is located in the <font face="Courier">MotionServer</font> directory.
 +
 
 +
;User GUI Application
 +
The User GUI Application is located in the <font face="Courier">ServoGui</font> directory.
  
 
==How to use the project==
 
==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 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/bdtech/pic/pic_et_servo.html http://fribotte.free.fr]. R/C servo needs a pulse-width modulated (PWM) signal to manage his angular position. This signal looks like this:
 +
[[Image:Servos-signal.jpg|800px|center|R/C Servo PWM Signal]]
 +
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. With latest Linux kernel (after 2.6.16), it would be possible to implement short delays with high-resolution timers if they are enabled in the kernel configuration. But I have not explore this way.
 +
* Due to the intrinsic non real time characteristic of the Linux kernel, the accuracy of the delays are impossible on a heavy loaded system. This causes R/C servo jittering (When a R/C servo jitters, it seems to have Parkinson's disease... [http://en.wikipedia.org/wiki/Jitter 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:
 +
[[Image:MotionSystem-Architecture.png|800px|center|MotionSystem Architecture]]
 +
 +
===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:
 +
[[Image:Hardware-Interface-Board.png|1024px|center|MotionSystem Schematic Diagram]]
 +
====How to connect the hardware interface board to the Armadeus APF9328DevLight board====
 +
The following table establishes the connexions 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.
 +
 +
{| border="1"
 +
| Signal on the diagram || APF9328DevLight pin || Spartan 3 pin
 +
|-
 +
| pwmOut00 || L24N_3 || P87
 +
|-
 +
| pwmOut01 || L24P_3 || P86
 +
|-
 +
| pwmOut02 || L23N_3 || P85
 +
|-
 +
| pwmOut03 || L40P_2 || P92
 +
|-
 +
| pwmOut04 || L40N_3 || P90
 +
|-
 +
| pwmOut05 || L40P_3 || P89
 +
|-
 +
| pwmOut06 || L24N_2 || P96
 +
|-
 +
| pwmOut07 || L24P_2 || P95
 +
|-
 +
| pwmOut08 || L40N_2 || P93
 +
|-
 +
| pwmOut09 || L22P_2 || P99
 +
|-
 +
| pwmOut10 || L23N_2 || P98
 +
|-
 +
| pwmOut11 || L23P_2 || P97
 +
|-
 +
| pwmOut12 || L21N_2 || P103
 +
|-
 +
| pwmOut13 || L21P_2 || P102
 +
|-
 +
| pwmOut14 || L22N_2 || P100
 +
|-
 +
| pwmOut15 || L01P_2 || P107
 +
|-
 +
| pwmOut16 || L20N_2 || P105
 +
|-
 +
| pwmOut17 || L20P_2 || P104
 +
|-
 +
| pwmOut18 || L32N_0 || P128
 +
|-
 +
| pwmOut19 || L32P_0 || P127
 +
|-
 +
| pwmOut20 || L01N_2 || P108
 +
|-
 +
| pwmOut21 || L31P_0 || P129
 +
|-
 +
| pwmOut22 || L31N_0 || P130
 +
|-
 +
| pwmOut23 || L32N_1 || P125
 +
|-
 +
| pwmOut24 || L30P_0 || P131
 +
|-
 +
| pwmOut25 || L30N_0 || P132
 +
|-
 +
| pwmOut26 || L27P_0 || P135
 +
|-
 +
| pwmOut27 || L28N_1 || P119
 +
|-
 +
| pwmOut28 || L28P_1 || P118
 +
|-
 +
| pwmOut29 || IO1 || P116 
 +
|-
 +
| pwmOut30 || L01P_1 || P112
 +
|-
 +
| pwmOut31 || L01N_1 || P113
 +
|}
 +
 +
===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:
 +
 +
<b><font face="Courier">SERVO_top.vhd</font></b>: This is the main module.
 +
 +
<b><font face="Courier">COUNTER.vhd</font></b>: This module is a 16 bits counter at 4 MHz used by all the PWM modules.
 +
 +
<b><font face="Courier">RESET_MODULE.vhd</font></b>: This module is a cold start reset generator.
 +
 +
<b><font face="Courier">pwm_module.vhd</font></b>: This module generates the PWM output.
 +
 +
<b><font face="Courier">servo.ucf</font></b>: 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 <font face="Courier">SERVO_top.bit</font> 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 [[MotionSystem#Hardware_Interface_Board|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 <font face="Courier">ArmadeusServoDriver</font> directory of the main archive file of the project (see above). The following files form the Linux kernel driver:
 +
 +
<b><font face="Courier">servo.c</font></b>: Main file.
 +
 +
<b><font face="Courier">servo.h</font></b>: Header file.
 +
 +
<b><font face="Courier">Makefile</font></b>: Kernel module makefile.
 +
 +
<b><font face="Courier">Kconfig</font></b>: 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, <font face="Courier">$ARMADEUS_ROOT</font> will refer to the root directory of Armadeus SDK (for example: <font face="Courier">/usr/local/src/armadeus3-trunk</font>)
 +
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 <font face="Courier">$ARMADEUS_ROOT/target/linux/modules/fpga/Makefile</font> 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 <font face="Courier">$ARMADEUS_ROOT//target/linux/modules/fpga/Kconfig</font> by adding:
 +
source "drivers/armadeus/fpga/ArmadeusServoDriver/Kconfig"
 +
Configure the Linux kernel to build the servo module:
 +
cd $ARMADEUS_ROOT
 +
make linux-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 <font face="Courier">servo.ko</font> 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 commands to the driver via writing in the <font face="Courier">/sys</font> 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 [[MotionSystem#FPGA_Firmware|FPGA Firmware]].
 +
Load the Linux Kernel Servo Driver. Log on the Armadeus target and type:
 +
  modprobe servo
 +
The system answers:
 +
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
 +
Communications with the driver use the /sys filesystem:
 +
ls -l /sys/class/servo_contoller/servo_contoller0/
 +
 +
Files relative to Servo Controller:
 +
lrwxrwxrwx    1 root    root            0 Jan  1 01:11 device -> ../../../devices/platform/servo_contoller.0
 +
-rw-r--r--    1 root    root        4096 Jan  1 01:11 enable
 +
-r--r--r--    1 root    root        4096 Jan  1 01:11 firmware_version
 +
-r--r--r--    1 root    root        4096 Jan  1 01:11 nbr_servos
 +
--w-------    1 root    root        4096 Jan  1 01:11 reset
 +
lrwxrwxrwx    1 root    root            0 Jan  1 01:11 subsystem -> ../../servo_contoller
 +
-rw-r--r--    1 root    root        4096 Jan  1 01:11 uevent
 +
 +
ls /sys/class/servo
 +
Each R/C servo has a directory:
 +
servo0  servo12  servo16  servo2  servo23  servo27  servo30  servo6
 +
servo1  servo13  servo17  servo20  servo24  servo28  servo31  servo7
 +
servo10  servo14  servo18  servo21  servo25  servo29  servo4  servo8
 +
servo11  servo15  servo19  servo22  servo26  servo3  servo5  servo9
 +
 +
ls -l /sys/class/servo/servo0/
 +
 +
-r--r--r--    1 root    root        4096 Jan  1 01:14 current_position
 +
-rw-r--r--    1 root    root        4096 Jan  1 01:14 desired_position
 +
lrwxrwxrwx    1 root    root            0 Jan  1 01:14 device -> ../../../devices/platform/servo.0
 +
-rw-r--r--    1 root    root        4096 Jan  1 01:14 enable
 +
-rw-r--r--    1 root    root        4096 Jan  1 01:14 lower_boundary
 +
-rw-r--r--    1 root    root        4096 Jan  1 01:14 offset
 +
-rw-r--r--    1 root    root        4096 Jan  1 01:14 speed_step
 +
lrwxrwxrwx    1 root    root            0 Jan  1 01:14 subsystem -> ../../servo
 +
-rw-r--r--    1 root    root        4096 Jan  1 01:14 uevent
 +
-rw-r--r--    1 root    root        4096 Jan  1 01:14 upper_boundary
 +
 +
Explanations:
 +
*current_position: Read Only, the current position of this R/C servo.
 +
*desired_position: Read/Write, setpoint position (the position that you want for this R/C servo).
 +
*enable: Read/Write, R/C servo enable status, 0 the R/C is freewheeling, 1 it is on.
 +
*lower_boundary: Read/Write, the minimal position for this R/C servo.
 +
*upper_boundary: Read/Write, the maximal position for this R/C servo.
 +
*offset: Read/Write, the position offset for this R/C servo.
 +
 +
Enable R/C servo 0:
 +
cd /sys/class/servo/servo0
 +
echo '1' > enable
 +
The R/C servo takes mid position (default position).
 +
 +
Show R/C servo 0 enable status:
 +
cat enable
 +
The R/C servo 0 is enable:
 +
1
 +
 +
Show R/C servo 0 setpoint position:
 +
cat desired_position
 +
The R/C servo 0 is at mid position:
 +
2048
 +
 +
Put the R/C servo 0 on the maximal counterclockwise position:
 +
echo '0' > desired_position
 +
Show R/C servo 0 setpoint position:
 +
cat desired_position
 +
The R/C servo 0 is at the maximal counterclockwise position:
 +
0
 +
Put the R/C servo 0 on the maximal clockwise position:
 +
echo '4095' > desired_position
 +
Show R/C servo 0 setpoint position:
 +
cat desired_position
 +
The R/C servo 0 is at the maximal counterclockwise position:
 +
4095
 +
 +
===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 [[MotionSystem#Motion_Daemon_Language|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 <font face="Courier">command.y</font> and <font face="Courier">command.l</font> respectively in the <font face="Courier">MotionServer</font> directory. The commands available are the following:
 +
*Set the R/C servo <font face="Courier">servo</font> (integer between 0 and 31) at the position <font face="Courier">position</font> (integer between 0 and 4095):
 +
SET_POSITION ( servo , position )
 +
*Enable the R/C servo <font face="Courier">servo</font>. If R/C servo is enable, it keeps its position else it is freewheeling.
 +
SET_ENABLE ( servo , status):
 +
*Set the position offset for the R/C servo <font face="Courier">servo</font> (integer between 0 and 31) at the value <font face="Courier">offset</font> (integer between -4095 and 4095):
 +
SET_OFFSET  ( servo , offset )
 +
*Set the minimal position for the R/C servo <font face="Courier">servo</font> (integer between 0 and 31) at the value <font face="Courier">min</font> (integer between 0 and 4095):
 +
SET_MIN ( servo , min )
 +
*Set the maximal position for the R/C servo <font face="Courier">servo</font> (integer between 0 and 31) at the value <font face="Courier">max</font> (integer between 0 and 4095):
 +
SET_MAX ( servo , max )
 +
*Return the position of the R/C servo <font face="Courier">servo</font>:
 +
GET_POSITION ( servo )
 +
*Return the enable state of the R/C servo <font face="Courier">servo</font>:
 +
GET_ENABLE ( servo )
 +
*Return the position offset of the R/C servo <font face="Courier">servo</font>:
 +
GET_OFFSET ( servo )
 +
*Return the minimal position of the R/C servo <font face="Courier">servo</font>:
 +
GET_MIN ( servo )
 +
*Return the maximal position of the R/C servo <font face="Courier">servo</font>:
 +
GET_MAX ( servo )
 +
*Write the current state (R/C servos position, position offset, minimum position, maximum position, enable state) in a file <font face="Courier">path</font>on the local Armadeus board:
 +
WRITE_CONFIG_FILE ( path )
 +
*Wait <font face="Courier">seconds</font> seconds before processing next command:
 +
SLEEP ( seconds )
 +
 +
====Motion Daemon Configuration File====
 +
The User Space Motion Daemon need a configuration file <font face="Courier">.MotionServerrc</font> located in the <font face="Courier">MotionServer</font> binary file directory. The minimal file is:
 +
[Module]
 +
 +
[Default]
 +
 +
The configuration file is in two sections.
 +
*<font face="Courier">[Module]</font>: This section concerns the configuration of the Linux Kernel Servo Driver. Options are
 +
**<font face="Courier">ModuleFilePath /path/to/the/module</font>: Set the path of the Linux Kernel Servo Driver module.
 +
**<font face="Courier">ModuleLoad [0|1]</font>: Set the auto-loading of the Linux Kernel Servo Driver module at the daemon startup.
 +
*<font face="Courier">[Default]</font>: 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.
 +
**<font face="Courier">ServoPosition idServo value</font>: Set the default position of R/C servo <font face="Courier">idServo</font> at value <font face="Courier">value</font>.
 +
**<font face="Courier">ServoOffset idServo value</font>: Set the default position of R/C servo <font face="Courier">idServo</font> at value <font face="Courier">value</font>.
 +
**<font face="Courier">ServoMin idServo value</font>: Set the default lower position boundary of R/C servo <font face="Courier">idServo</font> at value <font face="Courier">value</font>.
 +
**<font face="Courier">ServoMax idServo value</font>: Set the default upper position boundary of R/C servo <font face="Courier">idServo</font> at value <font face="Courier">value</font>.
 +
**<font face="Courier">ServoEnable idServo value</font>: Set the default enable status of R/C servo <font face="Courier">idServo</font> at value <font face="Courier">value</font>.
 +
Example of typical <font face="Courier">.MotionServerrc</font> 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 build the User Space Motion Daemon====
 +
The User Space Motion Daemon is located in the MotionServer directory of the main archive file of the project (see above). The following files form the User Space Motion Daemon:
 +
 +
<b><font face="Courier">MotionServer.c</font></b>: Main file.
 +
 +
<b><font face="Courier">MotionServer.h</font></b>: Header file.
 +
 +
<b><font face="Courier">Makefile</font></b>: Daemon makefile.
 +
 +
<b><font face="Courier">.MotionServerrc</font></b>: Configuration sample file.
 +
 +
<b><font face="Courier">rcfile.l</font></b>: Lex lexer file for the configuration file.
 +
 +
<b><font face="Courier">rcfile.y</font></b>: Yacc BNF grammar file for the configuration file.
 +
 +
<b><font face="Courier">command.l</font></b>: Lex lexer file for the command language.
 +
 +
<b><font face="Courier">command.y</font></b>: Yacc BNF grammar file for the command language.
 +
 +
Before building the daemon, please adapt the path of the variables CC and STRIP to your environment.
 +
 +
cd MotionServer
 +
make
 +
 +
You get a <font face="Courier">MotionServer</font> ELF ARM file. Copy the files <font face="Courier">MotionServer</font> and <font face="Courier">.MotionServerrc</font> somewhere on the Armadeus target.
 +
 +
====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 [[MotionSystem#Linux_Kernel_Servo_Driver|Linux Kernel Servo Driver]].
 +
Run the User Space Motion Daemon:
 +
./MotionServer
 +
After some default parameters settings, you get:
 +
Starting server
 +
Making socket
 +
Binding to port 1972opened socket as fd (4) on port (1972) for stream i/o
 +
Server
 +
                  sin_family        = 2
 +
                  sin_addr.s_addr  = 0
 +
                  sin_port          = 1972
 +
 +
Making a listen queue of 0 elements
 +
Waiting for a connection
 +
he User Space Motion Daemon is ready and waits for incoming client connexion. Be sure that the Armadeus target and your host development system can dialog over a TCP/IP connection. On your host type:
 +
telnet XXX.XXX.XXX.XXX 1972
 +
Where <font face="Courier">XXX.XXX.XXX.XXX</font> is the IP address of the Armadeus target. The Motion Daemon answers:
 +
MOTION SERVER OK
 +
The connection between your host and the Motion Daemon is established. You can send command. Enable the R/C servo 0:
 +
SET_ENABLE ( 0 , 1 )
 +
The Motion Daemon answers:
 +
SET_ENABLE ( 0 , 1 )
 +
Set the R/C servo 0 setpoint position to 250:
 +
SET_POSITION ( 0 , 250 )
 +
The Motion Daemon answers:
 +
SET_POSITION ( 0 , 250 )
 +
Get the R/C servo 0 setpoint position:
 +
GET_POSITION ( 0 )
 +
The Motion Daemon answers:
 +
250
 +
 +
===User GUI Application===
 +
The User GUI Application is an client example of the Servo User Space Daemon. This application is based on [http://www.qtsoftware.com/ Qt] version 4.3.x, 4.4.x, or 4.5.x. User GUI Application is located in the ServoGui directory of the main archive file of the project (see above). The following files form the User GUI Application:
 +
 +
<b><font face="Courier">main.cpp</font></b>: Main application file.
 +
 +
<b><font face="Courier">servogui.cpp</font></b>: Application file.
 +
 +
<b><font face="Courier">servogui.h</font></b>: Header file.
 +
 +
<b><font face="Courier">ServoGui.pro</font></b>: Qt project file.
 +
 +
<b><font face="Courier">servogui.ui</font></b>: Qt form file.
 +
 +
<b><font face="Courier">Batchfile.txt</font></b>: Example of batch file.
 +
 +
<b><font face="Courier">rc/servo.PNG</font></b>: Icon file.
 +
 +
====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:
 +
<font face="Courier">
 +
:libqt4-assistant 
 +
:libqt4-core     
 +
:libqt4-dbus     
 +
:libqt4-designer 
 +
:libqt4-dev       
 +
:libqt4-gui       
 +
:libqt4-help     
 +
:libqt4-network   
 +
:libqt4-opengl   
 +
:libqt4-opengl-dev
 +
:libqt4-qt3support
 +
:libqt4-script   
 +
:libqt4-scripttools
 +
:libqt4-sql       
 +
:libqt4-sql-mysql 
 +
:libqt4-sql-sqlite
 +
:libqt4-svg       
 +
:libqt4-test     
 +
:libqt4-webkit   
 +
:libqt4-xml       
 +
:libqt4-xmlpatterns
 +
:qt4-demos       
 +
:qt4-designer     
 +
:qt4-dev-tools   
 +
:qt4-doc         
 +
:qt4-doc-html     
 +
:qt4-qmake       
 +
:qt4-qtconfig     
 +
</font>
 +
Prepare the Qt project for building:
 +
qmake-qt4
 +
Build the User GUI Application:
 +
make
 +
Now, the ELF binary file <font face="Courier">ServoGui</font> is available in the <font face="Courier">ServoGui</font>directory.
 +
 +
====How to use the User GUI Application====
 +
The User Space Motion Daemon must be launched on the Armadeus target.
 +
To launch the User GUI Application, in the build directory, type:
 +
./ServoGui
 +
You get the following screen:
 +
[[Image:ServoGui-0.png|center|ServoGui Home 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:
 +
[[Image:ServoGui-1.png|center|ServoGui Advanced Controls]]
 +
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 <font face="Courier">ServoGui</font> is running. This file contains command that are syntactically correct in the [[MotionSystem#Motion_Daemon_Language|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.
 +
 +
Connection to the Armadeus target:
 +
Type the IP address or the host name of the Armadeus target in the field Remote Host. Then click on the Connect button. ServoGui says:
 +
state: MOTION_CLIENT_STATE_NOT_CONNECTED
 +
state: MOTION_CLIENT_STATE_CONNECTED_NO_SIGNED
 +
Connexion Ok:  XXX.XXX.XXX.XXX
 +
Reply From Server: MOTION SERVER OK
 +
state: MOTION_CLIENT_STATE_CONNECTED_READY_TO_SEND
 +
Now you can play with the graphical controls to move R/C servo...
  
 
==Links==
 
==Links==
 +
 +
[http://www.embedded-wire.com/communaute/controleur-de-servomoteurs-fpga-sur-plate-forme-armadeus Project Files (new version 00.00.02)]
 +
 +
[http://yvan.roch.free.fr/armadeus/MotionSystem-00.00.00.tgz Project Files (first release)]
 +
 +
[http://www.cadsoft.de Eagle]
 +
 +
[http://www.lynxmotion.com http://www.lynxmotion.com]
 +
 +
[http://fribotte.free.fr/bdtech/pic/pic_et_servo.html http://fribotte.free.fr]
 +
 +
[http://en.wikipedia.org/wiki/Jitter Jitter]
 +
 +
[http://www.qtsoftware.com/ Qt]

Latest revision as of 15:01, 4 April 2015

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 Firmware, the ADC multiplexers and the Max1027 ADC.
  • 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 over the network connexion.

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 Firmware.
  • 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, there is no PCB layout. The schematic is available in the file Hardware-Interface-Board/Hardware-Interface-Board.sch. The schematic was design with Eagle available at the following URL: Eagle.

FPGA Firmware

The FPGA firmware is located in the fpgaFirmware directory.

Linux Kernel Driver

The Linux Kernel Driver is located in the ArmadeusServoDriver directory.

User Space Daemon

The User Space Daemon is located in the MotionServer directory.

User GUI Application

The User GUI Application is located in the ServoGui directory.

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:

R/C Servo PWM Signal

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. With latest Linux kernel (after 2.6.16), it would be possible to implement short delays with high-resolution timers if they are enabled in the kernel configuration. But I have not explore this way.
  • Due to the intrinsic non real time characteristic of the Linux kernel, the accuracy of the delays are impossible 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:

MotionSystem Architecture

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:

MotionSystem Schematic Diagram

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

The following table establishes the connexions 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.

Signal on the diagram APF9328DevLight pin Spartan 3 pin
pwmOut00 L24N_3 P87
pwmOut01 L24P_3 P86
pwmOut02 L23N_3 P85
pwmOut03 L40P_2 P92
pwmOut04 L40N_3 P90
pwmOut05 L40P_3 P89
pwmOut06 L24N_2 P96
pwmOut07 L24P_2 P95
pwmOut08 L40N_2 P93
pwmOut09 L22P_2 P99
pwmOut10 L23N_2 P98
pwmOut11 L23P_2 P97
pwmOut12 L21N_2 P103
pwmOut13 L21P_2 P102
pwmOut14 L22N_2 P100
pwmOut15 L01P_2 P107
pwmOut16 L20N_2 P105
pwmOut17 L20P_2 P104
pwmOut18 L32N_0 P128
pwmOut19 L32P_0 P127
pwmOut20 L01N_2 P108
pwmOut21 L31P_0 P129
pwmOut22 L31N_0 P130
pwmOut23 L32N_1 P125
pwmOut24 L30P_0 P131
pwmOut25 L30N_0 P132
pwmOut26 L27P_0 P135
pwmOut27 L28N_1 P119
pwmOut28 L28P_1 P118
pwmOut29 IO1 P116
pwmOut30 L01P_1 P112
pwmOut31 L01N_1 P113

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 linux-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 commands 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 answers: 

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

Communications with the driver use the /sys filesystem: 

ls -l /sys/class/servo_contoller/servo_contoller0/

Files relative to Servo Controller: 

lrwxrwxrwx    1 root     root            0 Jan  1 01:11 device -> ../../../devices/platform/servo_contoller.0
-rw-r--r--    1 root     root         4096 Jan  1 01:11 enable
-r--r--r--    1 root     root         4096 Jan  1 01:11 firmware_version
-r--r--r--    1 root     root         4096 Jan  1 01:11 nbr_servos
--w-------    1 root     root         4096 Jan  1 01:11 reset
lrwxrwxrwx    1 root     root            0 Jan  1 01:11 subsystem -> ../../servo_contoller
-rw-r--r--    1 root     root         4096 Jan  1 01:11 uevent

ls /sys/class/servo

Each R/C servo has a directory: 

servo0   servo12  servo16  servo2   servo23  servo27  servo30  servo6
servo1   servo13  servo17  servo20  servo24  servo28  servo31  servo7
servo10  servo14  servo18  servo21  servo25  servo29  servo4   servo8
servo11  servo15  servo19  servo22  servo26  servo3   servo5   servo9

ls -l /sys/class/servo/servo0/

-r--r--r--    1 root     root         4096 Jan  1 01:14 current_position
-rw-r--r--    1 root     root         4096 Jan  1 01:14 desired_position
lrwxrwxrwx    1 root     root            0 Jan  1 01:14 device -> ../../../devices/platform/servo.0
-rw-r--r--    1 root     root         4096 Jan  1 01:14 enable
-rw-r--r--    1 root     root         4096 Jan  1 01:14 lower_boundary
-rw-r--r--    1 root     root         4096 Jan  1 01:14 offset
-rw-r--r--    1 root     root         4096 Jan  1 01:14 speed_step
lrwxrwxrwx    1 root     root            0 Jan  1 01:14 subsystem -> ../../servo
-rw-r--r--    1 root     root         4096 Jan  1 01:14 uevent
-rw-r--r--    1 root     root         4096 Jan  1 01:14 upper_boundary

Explanations:

  • current_position: Read Only, the current position of this R/C servo.
  • desired_position: Read/Write, setpoint position (the position that you want for this R/C servo).
  • enable: Read/Write, R/C servo enable status, 0 the R/C is freewheeling, 1 it is on.
  • lower_boundary: Read/Write, the minimal position for this R/C servo.
  • upper_boundary: Read/Write, the maximal position for this R/C servo.
  • offset: Read/Write, the position offset for this R/C servo.

Enable R/C servo 0: 

cd /sys/class/servo/servo0
echo '1' > enable

The R/C servo takes mid position (default position).

Show R/C servo 0 enable status: 

cat enable

The R/C servo 0 is enable: 

1

Show R/C servo 0 setpoint position: 

cat desired_position

The R/C servo 0 is at mid position: 

2048

Put the R/C servo 0 on the maximal counterclockwise position: 

echo '0' > desired_position

Show R/C servo 0 setpoint position: 

cat desired_position

The R/C servo 0 is at the maximal counterclockwise position: 

0

Put the R/C servo 0 on the maximal clockwise position: 

echo '4095' > desired_position

Show R/C servo 0 setpoint position: 

cat desired_position

The R/C servo 0 is at the maximal counterclockwise position: 

4095

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 , status):
  • 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 build the User Space Motion Daemon

The User Space Motion Daemon is located in the MotionServer directory of the main archive file of the project (see above). The following files form the User Space Motion Daemon:

MotionServer.c: Main file.

MotionServer.h: Header file.

Makefile: Daemon makefile.

.MotionServerrc: Configuration sample file.

rcfile.l: Lex lexer file for the configuration file.

rcfile.y: Yacc BNF grammar file for the configuration file.

command.l: Lex lexer file for the command language.

command.y: Yacc BNF grammar file for the command language.

Before building the daemon, please adapt the path of the variables CC and STRIP to your environment. 

cd MotionServer
make

You get a MotionServer ELF ARM file. Copy the files MotionServer and .MotionServerrc somewhere on the Armadeus target.

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. Run the User Space Motion Daemon: 

./MotionServer

After some default parameters settings, you get: 

Starting server
Making socket
Binding to port 1972opened socket as fd (4) on port (1972) for stream i/o
Server
                 sin_family        = 2
                 sin_addr.s_addr   = 0
                 sin_port          = 1972

Making a listen queue of 0 elements
Waiting for a connection

he User Space Motion Daemon is ready and waits for incoming client connexion. Be sure that the Armadeus target and your host development system can dialog over a TCP/IP connection. On your host type: 

telnet XXX.XXX.XXX.XXX 1972

Where XXX.XXX.XXX.XXX is the IP address of the Armadeus target. The Motion Daemon answers: 

MOTION SERVER OK

The connection between your host and the Motion Daemon is established. You can send command. Enable the R/C servo 0: 

SET_ENABLE ( 0 , 1 )

The Motion Daemon answers: 

SET_ENABLE ( 0 , 1 )

Set the R/C servo 0 setpoint position to 250: 

SET_POSITION ( 0 , 250 )

The Motion Daemon answers: 

SET_POSITION ( 0 , 250 )

Get the R/C servo 0 setpoint position: 

GET_POSITION ( 0 )

The Motion Daemon answers: 

250

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. User GUI Application is located in the ServoGui directory of the main archive file of the project (see above). The following files form the User GUI Application:

main.cpp: Main application file.

servogui.cpp: Application file.

servogui.h: Header file.

ServoGui.pro: Qt project file.

servogui.ui: Qt form file.

Batchfile.txt: Example of batch file.

rc/servo.PNG: Icon file.

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:

libqt4-assistant
libqt4-core
libqt4-dbus
libqt4-designer
libqt4-dev
libqt4-gui
libqt4-help
libqt4-network
libqt4-opengl
libqt4-opengl-dev
libqt4-qt3support
libqt4-script
libqt4-scripttools
libqt4-sql
libqt4-sql-mysql
libqt4-sql-sqlite
libqt4-svg
libqt4-test
libqt4-webkit
libqt4-xml
libqt4-xmlpatterns
qt4-demos
qt4-designer
qt4-dev-tools
qt4-doc
qt4-doc-html
qt4-qmake
qt4-qtconfig

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

The User Space Motion Daemon must be launched on the Armadeus target. To launch the User GUI Application, in the build directory, type: 

./ServoGui

You get the following screen:

ServoGui Home 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:

ServoGui Advanced Controls

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.

Connection to the Armadeus target: Type the IP address or the host name of the Armadeus target in the field Remote Host. Then click on the Connect button. ServoGui says: 

state: MOTION_CLIENT_STATE_NOT_CONNECTED
state: MOTION_CLIENT_STATE_CONNECTED_NO_SIGNED
Connexion Ok:  XXX.XXX.XXX.XXX
Reply From Server: MOTION SERVER OK
state: MOTION_CLIENT_STATE_CONNECTED_READY_TO_SEND

Now you can play with the graphical controls to move R/C servo...

Links

Project Files (new version 00.00.02)

Project Files (first release)

Eagle

http://www.lynxmotion.com

http://fribotte.free.fr

Jitter

Qt

Retrieved from "http://armadeus.org/wiki/index.php?title=MotionSystem&oldid=13204"