Difference between revisions of "ELDER Project"
(Motor board section added) |
|||
Line 1: | Line 1: | ||
+ | [[Image:elder_logo.png|128px|alt=Elder logo]] | ||
+ | [[Image:elder_robot.png|300px|right|alt=Elder Robot]] | ||
== Project overview == | == Project overview == | ||
ELDER (Embedded Linux Development for Robotics) Project is a robotic team which participated to the [http://www.planete-sciences.org/robot/index.php?section=pages&pageid=100 French robotic cup] in May 2010. The team used an Armadeus APF27 + APF27Dev boards. This involved to work on both hardware and software designs. One of the goal of this project is to evolve and some parts are still under development. | ELDER (Embedded Linux Development for Robotics) Project is a robotic team which participated to the [http://www.planete-sciences.org/robot/index.php?section=pages&pageid=100 French robotic cup] in May 2010. The team used an Armadeus APF27 + APF27Dev boards. This involved to work on both hardware and software designs. One of the goal of this project is to evolve and some parts are still under development. | ||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
Line 34: | Line 29: | ||
− | |||
+ | __TOC__ | ||
− | |||
− | + | == Detailled informations == | |
+ | |||
+ | |||
+ | ===Daughter board=== | ||
+ | {| align="center" | ||
+ | |[[Image:elder_main-system.png|thumb|400px|center|ELDER's main system architecture]] | ||
+ | |[[Image:elder_pic-daughterboard.png|thumb|400px|center|ELDER's main boards (APF27 connected to the daughter board)]] | ||
+ | |} | ||
====Introduction==== | ====Introduction==== | ||
Line 182: | Line 183: | ||
+ | === VHDL Motion controller === | ||
+ | The motion controller used is based on a VHDL design which is implemented in the FPGA. | ||
− | |||
− | [[Image:Elder_onboard_gui.jpg| | + | The very first stage of the motion control is to decode the signals provided by the incremental encoders and output Pulse Width Modulated signals to control the motors |
+ | speeds. This is done in VHDL and implemented in the APF27 FPGA (programmable logic device). Further features are then build upon this like speed computation, X/Y | ||
+ | localizer, PID controllers etc. | ||
+ | |||
+ | |||
+ | In order to communicate properly with the CPU, the I.MX bus has to be used. | ||
+ | Signals are first of all wrapped into wishbone signals. The hierarchical structure contains components who owe a specific address range. As the address bus is 10bits | ||
+ | wide, it has been chosen to use a maximum of 8 components with 128 registers each. The motion controller is a component (certainly the biggest!) but other components can be added (mezzanine control for instance). This principle of “virtual components” is fully detailed with a led and button example on the [http://www.armadeus.com/wiki/index.php?title=A_simple_design_with_Wishbone_bus Armadeus wiki page]. | ||
+ | |||
+ | [[Image:elder_vhdl_design_diagram.png|thumb|center|600px|VHDL Design architecture]] | ||
+ | |||
+ | |||
+ | ==== System design ==== | ||
+ | |||
+ | |||
+ | ===== Encoder subsystem ===== | ||
+ | The aim of this subsystem is to give the encoder speed (a signed value on 16 bits), given the A and B signals. The system again, can be split into different functions. | ||
+ | |||
+ | |||
+ | ====== Digital filter ====== | ||
+ | The first stage consists to filter the incoming signals. The filter should be able to remove noise peaks. Due to the physical aspects of the robot, the A and B signals cannot trigger faster than a specific rate. | ||
+ | |||
+ | |||
+ | For instance, if the encoder wheel's size is d = 60mm, the maximum speed is V = 2m/s and the number of pulse per revolution is R = 1024, 1 revolution of the wheel corresponds to : | ||
+ | Pi.d <=> R ticks | ||
+ | |||
+ | |||
+ | So there are | ||
+ | a = R / (Pi.d) ~ 5433 ticks/meter | ||
+ | |||
+ | |||
+ | Now the robot moves at V m/s, there are Vt ticks per seconds: | ||
+ | Vt = V.a ~ 10865 ticks/sec | ||
+ | |||
+ | |||
+ | Finally, it corresponds to a minimum width of 46μs. The filter has to remove any spike smaller than that. | ||
+ | |||
+ | The method used remains quite simple to implement and requires few logic elements. The filter uses a shift register, its input is fed by the actual signals (A, B). | ||
+ | The filtered output is stable (high or low) if all the register values are the same. Otherwise, the values remain unchanged. The sampling rate determines the filter latency and its capability to reject noise. For instance, in order to filter less than 46μs (approximated to 40μs) wide peaks, the sampling rate can be 250kHz (period = 5μs) and the filter 4bits wide. | ||
+ | |||
+ | |||
+ | ====== Quadrature decoder ====== | ||
+ | The second stage uses both input signals (A and B) to produce two outputs: '''COUNT''' and '''UP'''. COUNT is a counter output which triggers at each change (weather it is forward or backward). UP simply indicates if the rotation is clockwise or counter clockwise. This stage is achieved with a 4 states machine describe on the diagram bellow. | ||
+ | |||
+ | |||
+ | [[Image:elder_vhdl_quad_decoder.png|thumb|center|600px|Quadrature decoder state machine]] | ||
+ | |||
+ | |||
+ | {| align="center" width="400px" border="1" | ||
+ | |+ Combinatorial states of outputs truth table | ||
+ | ! A !! B !! State !! COUNT !! UP | ||
+ | |- align="center" | ||
+ | | 1 || X || S0 || 1 || 1 | ||
+ | |- align="center" | ||
+ | | X || 1 || S0 || 1 || 0 | ||
+ | |- align="center" | ||
+ | | X || 1 || S1 || 1 || 1 | ||
+ | |- align="center" | ||
+ | | 0 || X || S1 || 1 || 0 | ||
+ | |- align="center" | ||
+ | | 0 || X || S2 || 1 || 1 | ||
+ | |- align="center" | ||
+ | | X || 0 || S2 || 1 || 0 | ||
+ | |- align="center" | ||
+ | | X || 0 || S3 || 1 || 1 | ||
+ | |- align="center" | ||
+ | | 1 || X || S3 || 1 || 0 | ||
+ | |- align="center" | ||
+ | | X || X || SX || 0 || X | ||
+ | |} | ||
+ | |||
+ | ====== Speed counter ====== | ||
+ | The output provided by the quadrature decoder can be directly connected to a counter. The increment/decrement input is UP and the clock rate is COUNT. In order to compute the speed, this counter is reset at a specific period. Thus, the value contained by the counter is directly the number of ticks between two resets, it | ||
+ | corresponds to a basic digital derivation. | ||
+ | |||
+ | |||
+ | However, this rate cannot be chosen randomly, it has to be in a certain range. | ||
+ | * If it's too slow, the speed value will be fed to the controller slowly and the physical motion between two computation might be important. | ||
+ | * If it's too quick, small variations may not be detected. | ||
+ | |||
+ | |||
+ | A common value of reset rate is around 100Hz. Thus, at the maximum speed, the speed counter contains around 109 ticks (maximum speed is around 10,865 ticks per second). | ||
+ | |||
+ | |||
+ | ===== Left/Right to Alpha/Delta converter ===== | ||
+ | The next stage is to convert these speeds into delta (linear translation) and alpha (orientation) values. The logical bloc uses signed values and compute the following formula: | ||
+ | D = (L+R)/2 | ||
+ | A = R−L | ||
+ | |||
+ | |||
+ | ===== Alpha/Delta to X/Y/T and Cordic core ===== | ||
+ | The last stage of the localizer is the bloc that compute actual positions in X/Y domain. In order to do that, it's needed to compute trigonometric operations, this is | ||
+ | enabled with the Cordic core (which is provided by Xilinx as a free IP). | ||
+ | |||
+ | |||
+ | However there are few things to compute before, it's mainly about unit conversions. | ||
+ | The Cordic core needs radian value but the Left/Right to Alpha/Delta converter gives only a relative angle without any units. | ||
+ | |||
+ | |||
+ | The first thing to do is to compute the angle in radian, according to the following formula: | ||
+ | A[rad] = A/I | ||
+ | |||
+ | |||
+ | Where A is the angle and I is the interaxial value (distance between the two wheels). A and I have to be in the same unit (ticks), so that the result is in radian. | ||
+ | This division is done by an hardware divider and the result is fed to the cordic core. | ||
+ | |||
+ | |||
+ | The Cordic core gives the values of cos(A[rad]) and sin(A[rad]), the last stage is to multiply it by the linear speed Vn which is done by embedded multipliers. | ||
+ | |||
+ | |||
+ | The entity also has to manage the different latency and insure signals integrity. A specific process fetches dX and dY only when the outputs are valid. The last thing to do is insure that angle values are bounded between -Pi and +Pi (chosen representation). This is basically done by an hardware modulo. | ||
+ | |||
+ | |||
+ | ===== PWM Generator ===== | ||
+ | This entity is used to produce a Pulse Width Modulation signal which control the motors speeds. This is simply done by a counter which compares the its value to the | ||
+ | desired speed and set the output to 0/1, whether the counter's value is above or below the threshold. | ||
+ | |||
+ | |||
+ | Some specific features have also been added, the PWM is actually signed and also delivers a direction output depending on the speed's sign. This output can be inverted | ||
+ | (using a XOR gate) with the inverted input. This input is useful because the motors are mounted “head to head” and opposite voltage need to be applied to make the robot goes straight. | ||
+ | |||
+ | |||
+ | ===== Alpha/Delta to Left/Right converter ===== | ||
+ | This entity is basically the opposite of the Left/Right to Alpha/Delta entity. It computes the following formula: | ||
+ | R = D−A | ||
+ | L = D+A | ||
+ | The outputs are fed to the PWM blocs. | ||
+ | |||
+ | |||
+ | ===== Error feeder ===== | ||
+ | The error feeder is the link between the localization manager and the control manager. It uses orders (from the CPU) as reference, and computes the errors between those and the feedbacks. Outputs are positions and speeds in alpha/delta domain. These outputs are then used by the PID controllers. | ||
+ | |||
+ | |||
+ | ===== PID Controllers ===== | ||
+ | This is the heart of the controllers. A PID bloc is basically: | ||
+ | * 3 multipliers, between the error and associated gains (Kp, Ki, Kd) | ||
+ | * An accumulator (integral effect) | ||
+ | * A unitary delay with a subtracter (derivative effect) | ||
+ | * A three-inputs adder (output computing). | ||
+ | |||
+ | |||
+ | The resources needed can start to be important depending on the resolution. Internally, registers are 24 bits wide but only the 16 MSB can be retrieved by the CPU. | ||
+ | |||
+ | |||
+ | === Main Software === | ||
+ | |||
+ | ==== Specifications ==== | ||
+ | This section describes the main software running on the APF27 board. It includes both low-level/drivers functions and high-level (like A.I.) routines. This also also one of the part which needs a lot of work and development is still in progress. That's why some high level functions haven't been written yet, focus on low level primitives and different drivers has been given. | ||
+ | |||
+ | |||
+ | The software is multithreading and has to deal with a lot of different components: | ||
+ | * Motion-controller low-level modules | ||
+ | * CAN bus managment | ||
+ | * GUI and user I/O managment | ||
+ | * High-level routines (path-finding, A.I.) | ||
+ | * Log manager | ||
+ | * Other features (e.g. beacons) | ||
+ | |||
+ | ==== Low-level software ==== | ||
+ | The low-level software of the motion controller provides the link between higher software (A.I., obstacle avoidance etc.) and the FPGA registers themselves. The motion control is composed of 3 modules: | ||
+ | * FPGA module which is the proper driver to read/write values | ||
+ | * Motion Control driver, which computes the values that need to be written in the registers | ||
+ | * Trajectory Manager module, used to build paths that are then given to the motion control module. These paths need waypoints points (logical point where the robot has to go) which are fed by the higher levels (obstacle avoidance and pathfinder). | ||
+ | |||
+ | |||
+ | ===== FPGA module ===== | ||
+ | The FPGA is mapped at a specific address and can be acceded like a memory. It's achieved by using pointers and the C code is relatively compact and an example is shown on the dedicated [http://www.armadeus.com/wiki/index.php?title=FPGA_register Armadeus wiki webpage]. | ||
+ | |||
+ | |||
+ | ===== Motion control module ===== | ||
+ | This module is linked to the trajectory manager. The main function of the motion control module is moveFollowPath, which takes a path in parameter. This function write appropriate values in the FPGA registers in order to follow the computed path. | ||
+ | The written values take into account the possible drift of the robot and then correct it accordingly. | ||
+ | |||
+ | |||
+ | ===== Trajectory manager module ===== | ||
+ | This is certainly the heavier module in terms of computation. This module provide different functions to compute a path, given a set of waypoints. The first and simpler “path rule” consist of two basic steps: | ||
+ | * Turn on place to align the robot on the next waypoint | ||
+ | * Go straight until the waypoint is reached | ||
+ | |||
+ | |||
+ | When the last point is reached, the robot can turn on place a last time to be on a specific direction. It's easy to see that if there are N waypoints, the total number of points/moves will be '''2N-1''' (1 turn and 1 straight path per waypoint except for the 1st point). The function is called pathStraight() and takes two parameters: the waypoints array and its size. The returned path's points are allocated dynamically and then need to be free once the path is used. | ||
+ | |||
+ | |||
+ | |||
+ | The second function is an enhancement of the previous one, instead of turning on place at each waypoint the robot can make a complex move (rotation+translation) in | ||
+ | order to “round” the corners. The function pathStraightRoundCorners() takes the same parameters as previously plus the corner size radius. The two points computed for the | ||
+ | same waypoints are now shifted from the corner size radius along the previous and next direction respectively, as shown on the illustration bellow. | ||
+ | |||
+ | |||
+ | [[Image:elder_software_round_corners.png|center|thumb|500px|Illustration of the "round corner" path generation]] | ||
+ | |||
+ | |||
+ | |||
+ | ==== High-level software ==== | ||
+ | The main program's goal is to manage all the different and individual modules. | ||
+ | There are many ways to do that, but the program runs on an Operating System so it's possible to use the POSIX multithreading features. | ||
+ | Each “manager” is considered as an independent thread and is launched by the main process. This project intended to work on three threads (but they are more than that): | ||
+ | * The strategy manager, which calls all the pathfinding and motion control algorithms, manage to send message when it's needed etc. | ||
+ | * The CAN manager, which is responsible for messages reception and take proper actions. | ||
+ | * The Log manager; a passive thread that simply store the robot's status in a logfile. | ||
+ | |||
+ | |||
+ | The figure bellow shows the different interconnections between software entities. | ||
+ | |||
+ | |||
+ | [[Image:elder_software_architecture.png|center|thumb|500px|Software architecture]] | ||
+ | |||
+ | |||
+ | ===== Strategy manager ===== | ||
+ | The strategy manager is the main thread of the program. It uses all the available functions provided by the artificial intelligence, path finder, trajectory generator, motion controller and can manager modules. It also uses the robot's status which is stored and manipulated with the context module. | ||
+ | The artificial intelligence uses a set a rules, determined at the beginning. These rules say, for instance, which action between two possibilities has the highest priority. | ||
+ | It provides primitives that actually call related function(s). For example '''“MOVE THE ARM”''' corresponds to send a message on the CAN bus where “MOVE THE ROBOT” corresponds to a complex cascading call of functions. | ||
+ | |||
+ | |||
+ | |||
+ | ===== CAN manager ===== | ||
+ | The CAN manager module provides the high level interface with the other robot's peripherals. The initialisation function open the device and create a socket, using the socketCAN module for linux. | ||
+ | The main thread keeps waiting for incoming messages. When something arrives, it's decoded and fetched. Then, the robot's status is updated if needed (for example, the position of a servomotor has changed). | ||
+ | |||
+ | |||
+ | The library used is the one developped by [[User:YoannC|Yoann Congal]] in 2008/2009 for the [http://clubrobotique.free.fr Robotnik Club (INSA of Rennes)]. | ||
+ | |||
+ | |||
+ | ===== Log manager ===== | ||
+ | The log manager is surely the smallest thread but remains nonetheless important for debugging purposes. | ||
+ | The first thing done by the initialisation function is to create a new file with a timestamp in its name. The format is '''“elder_yyyy_mm_dd-hh_mm.log”'''. So that files are sorted by name from the oldest to the newest. | ||
+ | |||
+ | |||
+ | As long as the main program runs, the context is stored in this file periodically (the rate can be adjusted, 50ms is used which corresponds to 20 samples per seconds). | ||
+ | When the program ends, the close function is called. The logfile is then closed and copied with a different name: “last.log”. Then, in order to retrieve the log there is no need to know the exact name (extremely useful for automated tasks that can be scripted). | ||
+ | |||
+ | |||
+ | These logfiles are then used by a Gnuplot script. This script can be found on the SVN repository ('''svn-elder/projects/elder_sw/gnuplot/elder.gnu'''). | ||
+ | |||
+ | |||
+ | [[Image:gnuplot_example.png|thumb|center|800px|Gnuplot example of output]] | ||
+ | |||
+ | |||
+ | |||
+ | ===== Graphical User Interface ===== | ||
+ | |||
+ | [[Image:Elder_onboard_gui.jpg|600px|right|alt=Onboard GUI]] | ||
Line 194: | Line 437: | ||
A simple graphical user interface has been developed in order to be able to display any type of useful informations on the robot such as: | A simple graphical user interface has been developed in order to be able to display any type of useful informations on the robot such as: | ||
− | *The battery level | + | * The battery level |
− | *The time left during the matches | + | * The time left during the matches |
− | *The position of the robot on the game table | + | * The position of the robot on the game table |
− | *The number of points the robot has scored | + | * The number of points the robot has scored |
On this screen, there is also a console to display miscellaneous messages. | On this screen, there is also a console to display miscellaneous messages. | ||
Line 208: | Line 451: | ||
− | + | === Remaining work === | |
− | ' | + | Regarding the diagram shown at the begining, some parts are still missing. The A.I. is quite a huge piece of work. During the 2010's contest, no A.I. was used due to a lack of time but only deterministic operations (pre-determined path following). Some changes will be also done because some specifications could have been better (often in a simpler way!). |
Latest revision as of 11:24, 30 June 2010
Project overview
ELDER (Embedded Linux Development for Robotics) Project is a robotic team which participated to the French robotic cup in May 2010. The team used an Armadeus APF27 + APF27Dev boards. This involved to work on both hardware and software designs. One of the goal of this project is to evolve and some parts are still under development.
Project members
This project is actually the work of two students in last year of their Master in Engineering at the University of Strathclyde.
- PaulM
- Sebastien
Files and links
In order to help the development of this project and the team's communication, a wiki and a subversion repository have been created. Links are bellow.
- Wiki Note that the wiki might be updated during the next days.
- SVN repository
Notes about the Subversion repository
- The actual FPGA firmware files are located at "svn-elder/projects/fpga_fw"
- The actual software files are located at "svn-elder/projects/elder_sw/armadeus"
Students project reports
- PaulM: Motion control, Beacon positioning system, High level software, Motor board: PDF
- Sebastien: Mechanical design, Power Board, Daughter board, GUI: PDF
Contents
- 1 Project overview
- 2 Detailled informations
- 2.1 Daughter board
- 2.2 Motor board
- 2.3 VHDL Motion controller
- 2.4 Main Software
- 2.5 Remaining work
Detailled informations
Daughter board
Introduction
The aim of the daughter board is to interface the APF27 board ( which is used for the intelligence and the motion control) with everything else on the robot such as sensors, actuators, servos, motors, etc... Modules can also be plugged on it according to the robot needs and be interfaced with the Armadeus FPGA or the daughter board’s own microcontroller. Therefore each year, small boards can be developed in line with the project theme and the chosen mechanical solutions.
Requirements
The electrical and mechanical requirements are:
- 100 x 160 mm board : the board has to be the same size as Armadeus board in order to be stacked over it.
- 45 x 45 mm plug-in modules (up to 6 can be plugged) with on each:
- 3.3V logic supply
- 5V and 12V power supply
- CAN Bus
- UART (serial transmission)
- 8 GPIO
- Encoders interface
- Motors interface (PWM and directions)
- Fan control
The daughter board is meant to become a ’mother board’ on which can run the motion control (PID) and the intelligence of the robot.
System design
Microcontroller
The choice of the microcontroller was based on the requirements, and first an MSP430 from Texas Instrument was chosen. But after two weeks of trying to make a programmer for this type of microcontroller without success, a PIC32MX has finally been selected instead.
The PIC32 is a 32 bits microcontroller architecture family from Microchip, and the PIC32MX575F512H has the following features:
- Max Speed: 80 Mhz
- 10-bits ADC 16 channels
- 5 PWM outputs
- 5 timers
- 53 I/O pins
- 6 UARTS
All this features were enough to fit to the requirements, moreover, this chip could be acquired for free, with the Microchip free samples process.
Note that the microcontroller is also interfaced with the CAN bus, and the implementation is the same as described before for the power board. A LED has been putted on a free pin at the end, which is useful to debug or to make the first ’led blinking test’ for example.
Fan control
The Armadeus board, daughter board, power board and plug-in modules are all mounted in a same box.
This configuration generate heat which has to be cooled by a fan in order to have correct performances.
A small dedicated microcontroller is used to realize this function, which again has been chosen from the Microchip manufacturer. It is a PIC12F683 which is small (8 pins), has got an ADC for the temperature reading and a PWM output for the fan speed setting.
The fan is driven by a MOSFET which is triggered by the PWM, and gives the possibility to apply a 12V supply voltage directly on it.
A freewheeling diode is also putted between the fan’s terminals, in order to protect it from being damaged by instant high voltages generated by the inductive property of the motor.
Encoders interface
The encoders are used to measure accurately the distance traveled by the robot, it gives 1024 ’ticks’ for each wheel revolution.
These ticks are transmitted via two channels A and B, which both are differential outputs of the encoder.
In order to treat these signals, AM26LV32 differential line receiver are used. This kind of chip can be supplied with only 3.3V however it accepts 5V inputs, which is the minimum supply level for the encoders.
As a result, it plays a double role as converting the differential input in a single output, and as converting a 5V input to a 3.3V output which is acceptable for the FPGA or the microcontroller.
Modules sockets
Each module can access to a 5V and 12V power supply generated from the power board, and a 3.3V logic supply generated by the daughter board.
It also has the possibility to use the CAN bus or an UART from the PIC32MX.
Three of six modules have 8 GPIO each directly mapped to the Armadeus board’s FPGA, and the 3 others have these available GPIO linked to the PIC32MX microcontroller’s ports.
Program
PIC32MX
At this stage of the project, there is no module which has been realized, so the only program running on the PIC32MX is blinking led code. This one has been developed in order to test if microcontroller was working, according to the configuration bits. This test has been successful, now need to be debugged the UART and the CAN bus.
PIC12F
For the PIC12F which control the fan, the program is very simple, there is a main loop in which the ADC is read, and this value determines the PWM duty cycle. The relation between the temperature and the duty cycle value is shown in the figure below , where three phases can be seen:
- before 30°C, the fan is stopped.
- between 30°C and 60°C the speed of the fan is linearly increasing according the temperature. An initial offset is required to start the fan’s motor.
- after 60°C, the fan remains at 100% speed
There is also a blinking LED, to be sure the microcontroller is correctly running.
Motor board
The role of this board is to provide a safe and reliable interface between the logic driver device and motors. It has to meet the motors requirement (MFA970D161). The diagram bellow shows motors board inputs/outputs.
System design
Isolated logic driver
The protection and voltage translation stage is done by using optocouplers. There are small devices that contain a diode and a photo-transistor. When the diode emits light, the photo-transistor becomes saturated and the logic level at its output is pulled high. Otherwise, the transistor is blocked and the logic level is low. The main advantage is to not have any electrical link between the input and the output. So if a short-circuit happens, the optocouplers might be destroyed but not the logic behind it. The illustration bellow presents how optocouplers are polarized on the motor board. The reference used is FOD617 from Fairchild semiconductor.
Power stage
The main devices that drive the motors are two full H-bridge drivers. These devices allow the motor to turn backward or forward at different speeds (controlled by using a Pulse Width Modulation – PWM). The chosen devices are the LMD18200 from National Semiconductor. They can drive 3A and have a current sensing output. Two capacitors are simply connected between outputs and boostraps pins, as shown on the figure below.
VHDL Motion controller
The motion controller used is based on a VHDL design which is implemented in the FPGA.
The very first stage of the motion control is to decode the signals provided by the incremental encoders and output Pulse Width Modulated signals to control the motors
speeds. This is done in VHDL and implemented in the APF27 FPGA (programmable logic device). Further features are then build upon this like speed computation, X/Y
localizer, PID controllers etc.
In order to communicate properly with the CPU, the I.MX bus has to be used.
Signals are first of all wrapped into wishbone signals. The hierarchical structure contains components who owe a specific address range. As the address bus is 10bits
wide, it has been chosen to use a maximum of 8 components with 128 registers each. The motion controller is a component (certainly the biggest!) but other components can be added (mezzanine control for instance). This principle of “virtual components” is fully detailed with a led and button example on the Armadeus wiki page.
System design
Encoder subsystem
The aim of this subsystem is to give the encoder speed (a signed value on 16 bits), given the A and B signals. The system again, can be split into different functions.
Digital filter
The first stage consists to filter the incoming signals. The filter should be able to remove noise peaks. Due to the physical aspects of the robot, the A and B signals cannot trigger faster than a specific rate.
For instance, if the encoder wheel's size is d = 60mm, the maximum speed is V = 2m/s and the number of pulse per revolution is R = 1024, 1 revolution of the wheel corresponds to :
Pi.d <=> R ticks
So there are
a = R / (Pi.d) ~ 5433 ticks/meter
Now the robot moves at V m/s, there are Vt ticks per seconds:
Vt = V.a ~ 10865 ticks/sec
Finally, it corresponds to a minimum width of 46μs. The filter has to remove any spike smaller than that.
The method used remains quite simple to implement and requires few logic elements. The filter uses a shift register, its input is fed by the actual signals (A, B). The filtered output is stable (high or low) if all the register values are the same. Otherwise, the values remain unchanged. The sampling rate determines the filter latency and its capability to reject noise. For instance, in order to filter less than 46μs (approximated to 40μs) wide peaks, the sampling rate can be 250kHz (period = 5μs) and the filter 4bits wide.
Quadrature decoder
The second stage uses both input signals (A and B) to produce two outputs: COUNT and UP. COUNT is a counter output which triggers at each change (weather it is forward or backward). UP simply indicates if the rotation is clockwise or counter clockwise. This stage is achieved with a 4 states machine describe on the diagram bellow.
A | B | State | COUNT | UP |
---|---|---|---|---|
1 | X | S0 | 1 | 1 |
X | 1 | S0 | 1 | 0 |
X | 1 | S1 | 1 | 1 |
0 | X | S1 | 1 | 0 |
0 | X | S2 | 1 | 1 |
X | 0 | S2 | 1 | 0 |
X | 0 | S3 | 1 | 1 |
1 | X | S3 | 1 | 0 |
X | X | SX | 0 | X |
Speed counter
The output provided by the quadrature decoder can be directly connected to a counter. The increment/decrement input is UP and the clock rate is COUNT. In order to compute the speed, this counter is reset at a specific period. Thus, the value contained by the counter is directly the number of ticks between two resets, it corresponds to a basic digital derivation.
However, this rate cannot be chosen randomly, it has to be in a certain range.
- If it's too slow, the speed value will be fed to the controller slowly and the physical motion between two computation might be important.
- If it's too quick, small variations may not be detected.
A common value of reset rate is around 100Hz. Thus, at the maximum speed, the speed counter contains around 109 ticks (maximum speed is around 10,865 ticks per second).
Left/Right to Alpha/Delta converter
The next stage is to convert these speeds into delta (linear translation) and alpha (orientation) values. The logical bloc uses signed values and compute the following formula:
D = (L+R)/2 A = R−L
Alpha/Delta to X/Y/T and Cordic core
The last stage of the localizer is the bloc that compute actual positions in X/Y domain. In order to do that, it's needed to compute trigonometric operations, this is enabled with the Cordic core (which is provided by Xilinx as a free IP).
However there are few things to compute before, it's mainly about unit conversions.
The Cordic core needs radian value but the Left/Right to Alpha/Delta converter gives only a relative angle without any units.
The first thing to do is to compute the angle in radian, according to the following formula:
A[rad] = A/I
Where A is the angle and I is the interaxial value (distance between the two wheels). A and I have to be in the same unit (ticks), so that the result is in radian.
This division is done by an hardware divider and the result is fed to the cordic core.
The Cordic core gives the values of cos(A[rad]) and sin(A[rad]), the last stage is to multiply it by the linear speed Vn which is done by embedded multipliers.
The entity also has to manage the different latency and insure signals integrity. A specific process fetches dX and dY only when the outputs are valid. The last thing to do is insure that angle values are bounded between -Pi and +Pi (chosen representation). This is basically done by an hardware modulo.
PWM Generator
This entity is used to produce a Pulse Width Modulation signal which control the motors speeds. This is simply done by a counter which compares the its value to the desired speed and set the output to 0/1, whether the counter's value is above or below the threshold.
Some specific features have also been added, the PWM is actually signed and also delivers a direction output depending on the speed's sign. This output can be inverted
(using a XOR gate) with the inverted input. This input is useful because the motors are mounted “head to head” and opposite voltage need to be applied to make the robot goes straight.
Alpha/Delta to Left/Right converter
This entity is basically the opposite of the Left/Right to Alpha/Delta entity. It computes the following formula:
R = D−A L = D+A
The outputs are fed to the PWM blocs.
Error feeder
The error feeder is the link between the localization manager and the control manager. It uses orders (from the CPU) as reference, and computes the errors between those and the feedbacks. Outputs are positions and speeds in alpha/delta domain. These outputs are then used by the PID controllers.
PID Controllers
This is the heart of the controllers. A PID bloc is basically:
- 3 multipliers, between the error and associated gains (Kp, Ki, Kd)
- An accumulator (integral effect)
- A unitary delay with a subtracter (derivative effect)
- A three-inputs adder (output computing).
The resources needed can start to be important depending on the resolution. Internally, registers are 24 bits wide but only the 16 MSB can be retrieved by the CPU.
Main Software
Specifications
This section describes the main software running on the APF27 board. It includes both low-level/drivers functions and high-level (like A.I.) routines. This also also one of the part which needs a lot of work and development is still in progress. That's why some high level functions haven't been written yet, focus on low level primitives and different drivers has been given.
The software is multithreading and has to deal with a lot of different components:
- Motion-controller low-level modules
- CAN bus managment
- GUI and user I/O managment
- High-level routines (path-finding, A.I.)
- Log manager
- Other features (e.g. beacons)
Low-level software
The low-level software of the motion controller provides the link between higher software (A.I., obstacle avoidance etc.) and the FPGA registers themselves. The motion control is composed of 3 modules:
- FPGA module which is the proper driver to read/write values
- Motion Control driver, which computes the values that need to be written in the registers
- Trajectory Manager module, used to build paths that are then given to the motion control module. These paths need waypoints points (logical point where the robot has to go) which are fed by the higher levels (obstacle avoidance and pathfinder).
FPGA module
The FPGA is mapped at a specific address and can be acceded like a memory. It's achieved by using pointers and the C code is relatively compact and an example is shown on the dedicated Armadeus wiki webpage.
Motion control module
This module is linked to the trajectory manager. The main function of the motion control module is moveFollowPath, which takes a path in parameter. This function write appropriate values in the FPGA registers in order to follow the computed path. The written values take into account the possible drift of the robot and then correct it accordingly.
Trajectory manager module
This is certainly the heavier module in terms of computation. This module provide different functions to compute a path, given a set of waypoints. The first and simpler “path rule” consist of two basic steps:
- Turn on place to align the robot on the next waypoint
- Go straight until the waypoint is reached
When the last point is reached, the robot can turn on place a last time to be on a specific direction. It's easy to see that if there are N waypoints, the total number of points/moves will be 2N-1 (1 turn and 1 straight path per waypoint except for the 1st point). The function is called pathStraight() and takes two parameters: the waypoints array and its size. The returned path's points are allocated dynamically and then need to be free once the path is used.
The second function is an enhancement of the previous one, instead of turning on place at each waypoint the robot can make a complex move (rotation+translation) in order to “round” the corners. The function pathStraightRoundCorners() takes the same parameters as previously plus the corner size radius. The two points computed for the same waypoints are now shifted from the corner size radius along the previous and next direction respectively, as shown on the illustration bellow.
High-level software
The main program's goal is to manage all the different and individual modules. There are many ways to do that, but the program runs on an Operating System so it's possible to use the POSIX multithreading features. Each “manager” is considered as an independent thread and is launched by the main process. This project intended to work on three threads (but they are more than that):
- The strategy manager, which calls all the pathfinding and motion control algorithms, manage to send message when it's needed etc.
- The CAN manager, which is responsible for messages reception and take proper actions.
- The Log manager; a passive thread that simply store the robot's status in a logfile.
The figure bellow shows the different interconnections between software entities.
Strategy manager
The strategy manager is the main thread of the program. It uses all the available functions provided by the artificial intelligence, path finder, trajectory generator, motion controller and can manager modules. It also uses the robot's status which is stored and manipulated with the context module. The artificial intelligence uses a set a rules, determined at the beginning. These rules say, for instance, which action between two possibilities has the highest priority. It provides primitives that actually call related function(s). For example “MOVE THE ARM” corresponds to send a message on the CAN bus where “MOVE THE ROBOT” corresponds to a complex cascading call of functions.
CAN manager
The CAN manager module provides the high level interface with the other robot's peripherals. The initialisation function open the device and create a socket, using the socketCAN module for linux. The main thread keeps waiting for incoming messages. When something arrives, it's decoded and fetched. Then, the robot's status is updated if needed (for example, the position of a servomotor has changed).
The library used is the one developped by Yoann Congal in 2008/2009 for the Robotnik Club (INSA of Rennes).
Log manager
The log manager is surely the smallest thread but remains nonetheless important for debugging purposes. The first thing done by the initialisation function is to create a new file with a timestamp in its name. The format is “elder_yyyy_mm_dd-hh_mm.log”. So that files are sorted by name from the oldest to the newest.
As long as the main program runs, the context is stored in this file periodically (the rate can be adjusted, 50ms is used which corresponds to 20 samples per seconds).
When the program ends, the close function is called. The logfile is then closed and copied with a different name: “last.log”. Then, in order to retrieve the log there is no need to know the exact name (extremely useful for automated tasks that can be scripted).
These logfiles are then used by a Gnuplot script. This script can be found on the SVN repository (svn-elder/projects/elder_sw/gnuplot/elder.gnu).
Graphical User Interface
The APF27 board can be interfaced with graphical LCD display and its associated touch screen.
During the matches, the robot has to be completely autonomous, without a computer plugged to it.
A simple graphical user interface has been developed in order to be able to display any type of useful informations on the robot such as:
- The battery level
- The time left during the matches
- The position of the robot on the game table
- The number of points the robot has scored
On this screen, there is also a console to display miscellaneous messages.
The touch screen allows the user to be interactive with the robot, to browse menu and ask the robot execute an action. This can be useful just before the matches to check if everything works properly.
The LCD is directly interfaced with the SDL library. This library can be used in C language to develop multimedia application on various platforms.
Remaining work
Regarding the diagram shown at the begining, some parts are still missing. The A.I. is quite a huge piece of work. During the 2010's contest, no A.I. was used due to a lack of time but only deterministic operations (pre-determined path following). Some changes will be also done because some specifications could have been better (often in a simpler way!).