Post on 24-Jan-2016
description
Centre for Mechanical Technology and Automation
Institute of Electronics Engineering and Telematics
TEMA
IEETA http://www.mec.ua.pt/robotics
A Distributed Communication System in a Humanoid Robot
Milton Ruas1
Filipe M. T. Silva1
Vítor M. F. Santos2
1 Department of Electronics and Telecommunications2 Department of Mechanical Engineering
University of Aveiro, PORTUGALUniversity of Aveiro, PORTUGAL
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Overview Introduction Distributed Control System The Communications System Model PC-Master Communication
RS-232 communication RS-232 messages and protocol
Master-Slaves communication CAN: an introduction CAN configuration in a PIC18F258 CAN messages and protocol
System variables access Conclusions and Future perspectives
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Introduction Complete humanoid model
22 degrees of freedom Weight - 5 kg Height - 60 cm Max. width - 25 cm Foot print - 20 8 (cm2)
Actuation Servomotors with transmission belts
Sensors A vision Camera Servos’ position (through its internal
potentiometers) Sensitive foot to applied forces Accelerometers/Inclinometers Gyroscopes
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Local Control
Power resistor (0.47)
16:1 multiplexer
CAN connector
Piggy-back socket
PIC Cristal oscillator
CAN driver
PIC
Unit CAN Address
PWM plugs
Servo fuse
Fuse status LED
Piggy-back board 2
Piggy-back board 1
Connector to sensor
CAN bus Power plug
Power regulator Reset button
RS232 plug
Connector to sensor
Each slave controller is made of a PIC 18F258 device with I/O interfacing
All slave units: Connect up to 3 servomotors Have a common base (a piggy-
back unit can add I/O sensors)
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Distributed Control System A Main Control Unit (PC):
Exchanges high-level orders and interacts with the camera.
RS-232 communication with the Distributer unit.
Distributer Unit (Master): Interface between the Main and
the Local control units. Adapts the RS-232 to CAN
commands (and vice-versa). 8 Local Control Units (Slaves):
Control the low-level features of the several devices.
CAN bus to connect them.
Main ControlMain Control
RS23RS2322MasteMaste
rr
CAN CAN BUSBUS
1
23 1
2
3
1
2
31
2 1
2
3
1
2
3
1
2
3
1
2
SlaveSlavess
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
PC-Master Communication Communication system: RS-232
Data to be exchanged: Actuation
• Final position (3 components)• Mean velocity for the 3 components• Type of Controller• Controller gains (2+1)
Sensors information• Servomotors position (3)• Servomotors mean velocity (3)• Servomotors current consumption (3)• Special sensors data (4)
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
RS-232 Communication
SOF OpCode … BCC
Exchange Model Request/Response from Main Unit
to Master RS-232 Setup:
Asynchronous flow 1 start + 8 data + 1 parity + 1 stop bits Baudrate: 115K2 bps
Messages: Start of Frame (SOF) OpCode: Operation Code ... BCC: EXOR of Full message
Start Data (8 bits) Parity Stop
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
RS-232 Messages
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
RS-232 Messages
SCU id Operation Parameter
SCU Address
OP_APPLY_JOINT (0b00)Mensagem de actuação sobre as três juntas do SCU alvo (SCU id).
PARAM_POSITION (0b00)Posição (em graus) a ser atingida.PARAM_VELOCITY (0b01)Velocidade média do movimento.PARAM_SPECIAL (0b11)Activação/desactivação do PWM aplicado aos motores.
OP_APPLY_CONTROL (0b01)Mensagem de actuação sobre as três juntas do SCU alvo com a definição dos parâmetros do controlador PID.
PARAM_KI (0b00)Ganho integral do controlador local.PARAM_KP (0b01)Ganho proporcional do controlador local.PARAM_KD (0b10)Ganho do controlador de 1º nível.PARAM_CONTROLON (0b11)Tipo de controlo a aplicar na junta.
OP_READ_SENSORS (0b10)Mensagem de leitura dos sensores adicionais (sensores de força dos pés, giroscópio e inclinómetro).
PARAM_POSITION (0b00)Posição actual de cada junta.PARAM_VELOCITY (0b01)Velocidade média de cada junta.PARAM_CURRENT (0b10)Corrente drenada por cada servo.PARAM_SPECIAL (0b11)Saída dos sensores especiais.
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
RS-232 Algorithms
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
RS-232 Commands (examples) Communications initialization
H=initcom(1,115200) % COM1, Baud=115k2
Joints Actuation Applyjoint(H,2,0,[0 0 70]) % SCU=2, pos=[0 0 70]
Motion velocity Applyjoint(H,2,1,[100 100 100])% Period=[2 2 2]s
Joints position reading Readjoint(H,2,0) [pos1 pos2 pos3]
Force sensors reading ReadSpecial(H,2) [data1 data2 data3 data4]
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Master-Slaves Communication Communication system: CAN
Data to be exchanged: Actuation
• Final position to reach (3 components)• Mean velocity (3 components)• Type of Controller• Controller gains (2+1)
Sensors information• Servomotors position (3)• Servomotors mean velocity (3)• Status of the slave unit• Servomotors current consumption (3)• Special sensors data (4)
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Communication Developed by Robert Bosch in the 1980s Initially applied in the automotive industry,
but soon has expanded to other fields: Very reliable and Robust Easy to add/remove nodes Ideal for Real Time Distributed Control
CAN: MAC based on CSMA/CR to control collisions Message prioritisation Broadcast messages Guaranteed data consistency Acknowledgment Fault tolerance Error detection and signalization Automatic error recovery
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Stack CAN is based on the traditional OSI model Only the Physical and Datalink layers are
used:
Physical layer• Bit encoding/decoding• Bit timing• Bit synchronization
Datalink layers Logical Link Control (LLC)
• Reception• Masking/Filtering• Recovery management
Medium Access Control (MAC)• Data encapsuling• Frame encoding• Medium Access management• Error detection and Signalization
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Physical Layer
Bit Encoding NRZ: Non return to zero Each bit is composed by several time
quantums Synchronization:
Bit stuffing 5 bits Synchronization between nodes:
Interrupt generation inside the bit time after successful RX/TX
Maximum #nodes: 32, 64, 128... Depends on the line
transceivers Transmission rate:
From 5kb/s (10km) to 1Mb/s (40m) Depends on the time quantums used.
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Data Link Layer
MAC: CSMA/CR Non-destructive arbitration at
bit level In a collision in the access
bus, the highest priority message is selected without any delay.
Errors signalled in the CAN data frame.
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Versions CAN 2.0A (Basic CAN)
Partial filtering RTRs processing and messages acceptance made at
the application layer Only one Buffer for RX, and one for TX High overhead at the application level
CAN 2.0B (Full CAN) Total filtering. 16 buffers can be programmed for RX/TX Low overhead at the application level
PIC 18F2x8x implements the full CAN version
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Protocol
CAN ID information: Source address: transmitter node address Destination address: receiver node address Index: Index of message (1st or 2nd) Type of operation: Actuation/Sensor reading
2 types of messages exchanged (operation): Actuation: from Master to Slaves Sensor reading: from Slaves to Master
Each message is composed of 2 CAN packets to accommodate all the necessary information.
Prioritized messages: Actuation messages (from Master to Slaves) have higher priority Lower address nodes have higher priority First packet of each message have higher priority
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Protocol
Controller Unit Section Address
Master Master unit 0b0000
Slave 1 Right Leg 0b0001
Slave 2 Left Leg 0b0010
Slave 3 Right Hip 0b0011
Slave 4 Left Hip 0b0100
Slave 5 Body 0b0101
Slave 6 Right Arm 0b0110
Slave 7 Left Arm 0b0111
Slave 8 Head 0b1000
Lower Parts (legs and hips) have higher priority than others
Priority defined according to the part importance
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Protocol
bit 7 bit 6 bit 5 bit 4 bit 3 bit 2 bit 1 bit 0
PWM Calib Cont3 Cont2 Cont0
bit 7 6 5 4 3 2 1 bit 0
PWM Calib Deadl. FinallFinon
eMotion finished
bit 7 6 5 4 3 2 1 bit 0
Source Address Index 0 0 0
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
in PIC 18F258 PIC 18F258 basic specifications:
Message bitrates up to 1Mbps 11/29-bit ID fields (Normal/Extended CAN) Up to 8 bytes of message length 3 prioritized TX buffers + 2 RX buffers 6 full prioritized, 29-bit acceptance filters Overflow RX prevention Advanced error management features
Configuration of CAN: Enter in Configuration Mode: avoids accidental reconfiguration Configure Baudrate Configure Mask & Filters for message acceptance How messages should be transmitted/received Enter in Normal Mode: ready to exchange messages
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Initial Configuration: Baudrate
Registers to setup: BRP=2 Quanta duration (depends of the crystal frequency) PRSEG Propagation Segment (1..8 2) SEG1PH Phase Segment 1 (1..8 3) SEG2PH Phase Segment 2 (2..8 2) SJW Synchronization Jump Width (1..4 2) SAM Sample Point (sample once)
Rules to obey: (#Q = Sync Seg+Prop Seg+Phase Seg 1+Phase Seg2) 8 ≤ #Q ≤ 25 (Selected: #Q=8) Sync Seg = 1 Prop Seg+Phase Seg 1≥Phase Seg 2 (2+3 ≥ 2) Phase Seg 2≥Sync Jump Width (2 ≥ 2)
KHzQTF
sQTT
sMHzF
BRPT
QBIT
QBIT
OSCQ
833#
2.1#
15.040
12
1
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Initial Configuration: Mask & Filters 2 CAN messages are always
exchanged at once Use one RX buffer for each one:
RXB0: First message (Index=0) RXB1: Second message (Index=1)
Master Mask: Receive all messages destined to
master• 0b00001111100
Filter:• To RXB0: 0bxxxx00000xx• To RXB1: 0bxxxx00001xx
Slaves Mask: Receive messages from Master
• 0b11110000100 Filter:
• To RXB0: 0b0000xxxx0xx• To RXB1: 0b0000xxxx1xx
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Initial Configuration: Message TX/RX Reception
Each message is directed to one RX buffer• RXB0 First message (index=0)• RXB1 Second message (index=1)
No “RX Buffer 0 overflow to RX Buffer 1” Receive only valid messages with std ID
Transmission Each message will use one TX buffer:
• TXB0 First message (index=0)• TXB1 Second message (index=1)
Prioritize TX buffers:• TXB0: Highest priority• TXB1: Lowest priority
Messages will be transmitted with std ID Frame Remote TX will not be requested
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Low-Level RX/TX
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
High-Level RX/TX in the Master
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
CAN Management in the Master
250 500 750 1000 1250 1500 t (µs)0
RS-232
CANTX1 TX1TX2RX1 RX2 RX1 (INT)
(Background)
Full Time-triggered CAN to guarantee BW to CAN and RS-232 comm RS-232 executed in
background Master initiates always
data exchange for each slave, avoiding collisions in responses: Actuation: Master Slave Sensors: Master Slave
Timings: One slave service: 1ms Turn-around time: 8ms (for
8 slaves)
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
High-Level RX/TX in the Slaves
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
CAN Management in the Slaves
Presented Algorithm is applied to each Packet of the message (2) Master starts communication with a
actuation message Slave responds with a sensor data
message
PWM control can cause Jitter up to 2ms. In this case, response is not sent to avoid collisions With a 20ms PWM period, one sensor
message is guaranteed to be sent.
500 8000 8500 t (µs)0
CANRX1 TX1 RX2 TX2 RX1 TX1 RX2 TX2
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Data Access in Master/Slaves
Actuation 1
Actuation 1
Sensors 1
Sensors 1
Actuation 2
Actuation 2
Sensors 2
Sensors 2
TX
TX
RX
RX
TX
TX
RX
RX
RX+TX
RX+TX
RX+TX
RX+TX
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Data Access in Master/Slaves
Building messages for TX requires access to actuation/sensorial data
For slaves The actuation message provided by the Master is
unpredictable, and so the sensorial data for the response must be available at any time
For the Master Data must be exchanged between the RS-232 and CAN
communications with minimal mutual interference
The best solution is to use a global data base where all the system variables are available! In Master, RS-232 and CAN communications are completely
independent Slaves have access to actuation/sensorial information anytime
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Data Base in Mastertypedef struct { // Estrutura descritiva dos sensores byte sysStatus; // Estado do sistema struct_servo servo[N_SERVOS]; // Sensores dos servos unsigned char special[N_SPECIAL_SENSORS]; // Sensores de força} struct_sensors;
typedef struct { // Valores de referência dos controladores struct_paramControl servo[N_SERVOS]; struct_paramControl cop[N_SERVOS]; struct_paramControl inc[N_SERVOS]; struct_paramControl giro[N_SERVOS]; } struct_refControl;
typedef struct { // Estrutura de actuação struct { // Status para cada SCU bool pwm, calib; } sysStatus; struct { // Estrutura de Controlo para cada junta byte ki, kp, kd; enum_controlType type; } control[N_SERVOS];} struct_actuators;
extern volatile struct_sensors sensors[N_SCU]; // Sensorsextern volatile struct_refControl refControl[N_SCU]; // Controller Refextern volatile struct_actuators actuators[N_SCU]; // Actuators
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Data Base in Slavestypedef struct { // Estrutura descritiva dos sensores struct { bool pwm, calib; // Motores ligados/desligados + Calibração on/off bool deadlineError; // Violação de deadline bool motionFinAll; // Todos os motores terminaram o movimento bool motionFinOne; // Um dos motores terminaram o movimento bool motionFin[N_SERVOS]; // Movimento terminado } sysStatus; // Estado sensorial do sistema struct_servo servo[N_SERVOS]; // Sensores dos servos (pos, vel & corrente) unsigned char special[N_SPECIAL_SENSORS]; // Sensores especiais} struct_sensors;
typedef struct { // Estrutura descritiva dos actuadores struct { // Estrutura de actuação de status para cada SCU bool pwm, calib; } sysStatus; struct { // Controlador byte kp, kd, ki; byte type; } control[N_SERVOS]; struct_servo servo[N_SERVOS]; // Informação de actuação (pos, vel e corrente)} struct_actuators;
// Variáveis globais descritivas do sistemaextern volatile struct_sensors sensors; // Sensoresextern volatile struct_actuators actuators; // Actuadores
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Alternatives: CAN applications
Usage of a Industrial Application of CAN CANopen FTTCan DeviceNet SDS, J1939, CAN Kingdom, …
Advantages: Simplicity of implementation Some applications are freeware
Disadvantages: Low direct control over the physical layer for best performance Increased overhead for the microcontroller. Some applications are not freeware
Conclusion: the usage of the standard CAN, allows Control of the performance for the application and hardware involved But, is required more development time!
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Hardware Improvements
To increase performance and provide more BW for the control algorithms of the robot, more powerfull microcontrollers can be used.
Adittionally, code compiling reliability depends of the architecture.
Example: PIC 18F2580 Same pin-out Fully compatible with PIC 18F258 C compiler optimized architecture with optional extended instruction set Enhanced CAN feature (ECAN)
• 3 modes of operation (Legacy, Enhanced Legacy, FIFO)• 6 programmable RX/TX buffers• 3 full 29-bit acceptance masks• 16 full 29-bit acceptance filters w/ dynamic association• Advanced error management features
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Conclusions
After careful memory management, the system is working efficiently during several hours without interruption. However, more tests are necessary!
Due to instability by the Microchip compiler, some improvements can be achieved.
The usage of a C optimized architecture (18F2580) can improve the compiler reliability, which is worth to experiment.
If instability persists, the HI-TECH compiler can be experimented which offers easy integration into MPLAB and reduced source code modifications.